Calibrate Unit D Multisense, add automatic general setup to drc_environment.sh

software/config/drc_environment.sh

@@ -82,7 +82,7 @@ setup_drc()
   export ATLAS_ROBOT_INTERFACE=$DRC_BASE/software/atlas-collection/atlas/AtlasRobotInterface_3.3.0
 }
 
-setup_robot_computers()
+setup_atlas_computers()
 {
   # field computer
   if [ "paladin-12" = $(hostname) ]
@@ -114,6 +114,27 @@ setup_robot_computers()
   fi
 }
 
+setup_valkyrie_computers()
+{
+  # Edinburgh operator workstations
+  if [ "angmar" = $(hostname) ] || [ "gondolin" = $(hostname) ] || [ "vis04" = $(hostname) ]; then
+    echo "Setting up Valkyrie Unit D for Edinburgh"
+    export VAL_LINK_IP=10.185.0.40
+    export VAL_ZELDA_IP=10.185.0.41
+    export VAL_MULTISENSE_IP=10.185.0.42
+    export VAL_UNIT=D
+    export LCM_DEFAULT_URL="udpm://239.255.76.76:7676?ttl=1"
+  fi
+
+  #elif [ "val0" = $(hostname) ]; then # TODO: MIT Valkyrie workstations
+  #  echo "Setting up Valkyrie Unit C for MIT"
+  #  export VAL_LINK_IP=10.185.0.30
+  #  export VAL_ZELDA_IP=10.185.0.31
+  #  export VAL_MULTISENSE_IP=10.185.0.32
+  #  export VAL_UNIT=C
+  #fi
+}
+
 setup_network_sim()
 {
     export LCM_URL_DRC_ROBOT="udpm://239.255.76.68:7668?ttl=0"
@@ -159,8 +180,10 @@ set_drc_base
 setup_drc
 setup_network_sim
 setup_lcm_communities
-setup_robot_computers
+setup_atlas_computers
+setup_valkyrie_computers
 
 # aliases
 alias cddrc='cd $DRC_BASE/software'
 alias rundrc='bot-procman-sheriff -l $DRC_BASE/software/config/atlas/robot.pmd'
+alias runval='bot-procman-sheriff -l $DRC_BASE/software/config/val/robot.pmd'

software/config/val/multisense_D_50.cfg

@@ -0,0 +1,141 @@
+# sensor head on unit C in Jan 2016
+
+coordinate_frames {
+
+  ################## Sensor Frames ###################################
+  CAMERA_LEFT {
+    # this cannot change - its rigid
+    relative_to = "head";
+    history = 1000;
+    update_channel = "HEAD_TO_LEFT_CAMERA_OPTICAL_FRAME";
+    initial_transform{
+      # location of left camera:
+      translation = [ 0.0, 0.035, -0.002];
+      rpy = [ -90.0, 0.0, -90.0];
+    }
+  }
+
+  CAMERA_RIGHT {
+    relative_to = "CAMERA_LEFT";
+    history = 1000;
+    update_channel = "CAMERA_LEFT_TO_CAMERA_RIGHT"; 
+    initial_transform{
+      translation = [ 0.070, 0.0, 0.0];
+      rpy = [ 0.0, 0.0, 0.0];
+    }
+  }
+
+
+  ################## Path from Camera to Laser: ####################################
+  PRE_SPINDLE {
+    # rosrun tf tf_echo multisense/left_camera_optical_frame  multisense/motor
+    relative_to = "CAMERA_LEFT";
+    history = 5000;
+    update_channel = "CAMERA_LEFT_TO_PRE_SPINDLE";
+    initial_transform{
+      translation = [0.044, -0.085, -0.013];
+      rpy = [0.721, -0.419, -29.660];
+    }
+  }
+
+  POST_SPINDLE {
+    relative_to = "PRE_SPINDLE";
+    history = 5000;
+    max_frequency = 100;
+    update_channel = "PRE_SPINDLE_TO_POST_SPINDLE";
+    initial_transform{
+      # pure rotation transform
+      translation = [ 0.0, 0.0, 0.0 ];
+      quat = [ 1,0,0,0 ];
+    }
+  }
+
+  SCAN {
+    relative_to = "POST_SPINDLE";
+    history = 5000;
+    update_channel = "POST_SPINDLE_TO_SCAN";
+    initial_transform{
+      # rosrun tf tf_echo multisense/spindle multisense/head_hokuyo_frame
+      translation = [-0.001, 0.015, 0.000];
+      rpy = [47.496, -89.187, -137.493];
+    }
+  }
+
+  SCAN_FREE {
+    relative_to = "SCAN";
+    history = 1000;                       
+    update_channel = "SCAN_TO_SCAN_FREE";
+    initial_transform{
+      translation=[  0.0 , 0.0 , 0.0 ];
+      rpy = [0.0 , 0.0 , 0.0 ];
+    }
+  } 
+
+}
+
+
+cameras {
+  # fx fy skew cx cy 
+  CAMERA_LEFT {
+    lcm_channel = "CAMERA_LEFT";
+    coord_frame = "CAMERA_LEFT";
+    intrinsic_cal{
+      width = 1024;
+      height= 1024;
+      distortion_model = "plumb-bob";
+      distortion_k = [0,0,0];
+      distortion_p = [0,0];
+      pinhole = [ 556.183166504, 556.183166504, 0, 512.0, 512.0 ];
+    }
+  }
+
+  CAMERA_RIGHT {
+    lcm_channel = "CAMERA_RIGHT";
+    coord_frame = "CAMERA_RIGHT";
+    intrinsic_cal{
+      width = 1024;
+      height= 1024;
+      distortion_model = "plumb-bob";
+      distortion_k = [0,0,0];
+      distortion_p = [0,0];
+      pinhole = [ 556.183166504, 556.183166504, 0, 512.0, 512.0 ];
+    }
+  }  
+
+  CAMERA {
+    lcm_channel = "CAMERA";
+    type = "stereo";
+    rotation = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0];
+    translation = [-0.0700, 0.0, 0.0];
+  }
+}
+
+
+planar_lidars {
+  SCAN {
+    viewer_color = [ 1.0, 0.0, 0.0 ]; # red
+    max_range = 29.0;
+    min_range = 0.1;
+    frequency = 40;
+    laser_type = "HOKUYO_UTM";
+    coord_frame = "SCAN";
+    lcm_channel = "SCAN";
+    surround_region = [0, 1000000];        
+    up_region = [-1,-1];
+    down_region = [-1, -1];
+    max_range_free_dist = 29.0;
+  }
+  SCAN_FREE {
+    viewer_color = [ 0.0, 0.0, 1.0 ]; # blue
+    max_range = 29.0;
+    min_range = 0.1;
+    frequency = 40;
+    laser_type = "HOKUYO_UTM";
+    coord_frame = "SCAN";
+    lcm_channel = "SCAN_FREE";
+    surround_region = [0, 1000000];        
+    up_region = [-1,-1];
+    down_region = [-1, -1];
+    max_range_free_dist = 29.0;
+  }
+}

software/config/val/robot.cfg

@@ -1,6 +1,6 @@
 INCLUDE = [
   "core_config.cfg",
-  "multisense.cfg",
+  "multisense_D_50.cfg",
   "../atlas/common_components.cfg",
   "../atlas_sim_mit/control.cfg",
   "state_estimation.cfg"

software/config/val/robot.pmd

@@ -7,22 +7,21 @@ group "0.ros_bridge" {
 
 group "1.params_and_model_pub" {
     cmd "pserver" {
-        exec = "bot-param-server $DRC_BASE/software/config/val/robot.cfg";
+        #exec = "bot-param-server ${DRC_BASE}'/software/config/val/robot_'${VAL_UNIT}.cfg";
+        exec = "bot-param-server ${DRC_BASE}/software/config/val/robot.cfg";
         host = "localhost";
     }
 
     cmd "model" {
         exec = "robot_model_publisher $DRC_BASE/software/models/val_description/urdf/valkyrie_sim.urdf";
         host = "localhost";
     }
-
-
 }
 
 
 group "2.drivers" {
     cmd "multisense" {
-        exec = "multisense-lcm-driver -o 0 -n hokuyo_joint -a 10.185.0.32 -m 1500";
+        exec = "multisense-lcm-driver -o 0 -n hokuyo_joint -a $VAL_MULTISENSE_IP -m 1500";
         host = "localhost";
     }
 }
@@ -107,7 +106,7 @@ cmd "pose-util" {
 }
 
 cmd "ros-multisense" {
-    exec = "roslaunch multisense_bringup multisense.launch ip_address:=10.185.0.32 mtu:=1500";
+    exec = "roslaunch multisense_bringup multisense.launch ip_address:=$VAL_MULTISENSE_IP mtu:=1500";
     host = "localhost";
 }
 

software/config/val/robot_C.cfg

@@ -0,0 +1,7 @@
+INCLUDE = [
+  "core_config.cfg",
+  "multisense.cfg",
+  "../atlas/common_components.cfg",
+  "../atlas_sim_mit/control.cfg",
+  "state_estimation.cfg"
+];