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

software/config/drc_environment.sh

@@ -114,6 +114,35 @@ setup_robot_computers()
   fi
 }
 
+setup_valkyrie_for_host_sites()
+{
+  OUTSIDE_IP=`curl -s ifconfig.co` # gets our outside IP
+  OUTSIDE_HOSTNAME=`nslookup $OUTSIDE_IP`
+
+  EDINBURGH='.ed.ac.uk'
+  MIT='mit.edu'
+
+  if [[ "$OUTSIDE_HOSTNAME" =~ "$EDINBURGH" ]]; then
+    if [ "angmar" = $(hostname) ]; then # Only print status msg on operator workstation
+      echo "Setting up Valkyrie Unit D for Edinburgh"
+    fi
+
+    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
+  elif [[ "$OUTSIDE_HOSTNAME" =~ "$MIT" ]]; then
+    if [ "val0" = $(hostname) ]; then # Only print status msg on operator workstation, TODO: adjust hostname
+      echo "Setting up Valkyrie Unit C for MIT"
+    fi
+
+    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"
@@ -160,6 +189,7 @@ setup_drc
 setup_network_sim
 setup_lcm_communities
 setup_robot_computers
+setup_valkyrie_for_host_sites
 
 # aliases
 alias cddrc='cd $DRC_BASE/software'

software/config/val/multisense_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.pmd

@@ -7,22 +7,20 @@ 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";
         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";
     }
 }
@@ -92,12 +90,12 @@ group "6.exotica" {
 
 
 cmd "director" {
-    exec = "director -c $DRC_BASE/software/config/val/robot.cfg -val2";
+    exec = "director -c $DRC_BASE/software/config/val/robot_${VAL_UNIT}.cfg -val2";
     host = "localhost";
 }
 
 cmd "director_exotica" {
-    exec = "director -c $DRC_BASE/software/config/val/robot.cfg -val2 -exo";
+    exec = "director -c $DRC_BASE/software/config/val/robot_${VAL_UNIT}.cfg -val2 -exo";
     host = "localhost";
 }
 
@@ -107,7 +105,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_D.cfg

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