diff --git a/software/config/drc_environment.sh b/software/config/drc_environment.sh index 53eb59cde..59a2f168e 100644 --- a/software/config/drc_environment.sh +++ b/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,29 @@ 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 + + # MIT Valkyrie Workstations + # TODO: MIT Valkyrie workstations + if [ "vis03" = $(hostname) ]; then + 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 +182,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' diff --git a/software/config/val/multisense.cfg b/software/config/val/multisense_C_xx.cfg similarity index 100% rename from software/config/val/multisense.cfg rename to software/config/val/multisense_C_xx.cfg diff --git a/software/config/val/multisense_D_50.cfg b/software/config/val/multisense_D_50.cfg new file mode 100644 index 000000000..f4954df81 --- /dev/null +++ b/software/config/val/multisense_D_50.cfg @@ -0,0 +1,141 @@ +# sensor head on unit D in Edinburgh + +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; + } +} diff --git a/software/config/val/robot.cfg b/software/config/val/robot.cfg index cd177d59b..b7f77c11d 100644 --- a/software/config/val/robot.cfg +++ b/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" diff --git a/software/config/val/robot.pmd b/software/config/val/robot.pmd index 8e9a5a8fc..17da0bb4f 100644 --- a/software/config/val/robot.pmd +++ b/software/config/val/robot.pmd @@ -7,7 +7,7 @@ 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.cfg"; host = "localhost"; } @@ -15,14 +15,12 @@ group "1.params_and_model_pub" { 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 +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"; } diff --git a/software/config/val/robot_C.cfg b/software/config/val/robot_C.cfg new file mode 100644 index 000000000..3c8a3e57e --- /dev/null +++ b/software/config/val/robot_C.cfg @@ -0,0 +1,7 @@ +INCLUDE = [ + "core_config.cfg", + "multisense_C_xx.cfg", + "../atlas/common_components.cfg", + "../atlas_sim_mit/control.cfg", + "state_estimation.cfg" +];