Remove hero prefix from frames

hsrb_bringup/launch/include/sensors/common.xml

@@ -13,7 +13,10 @@
   <arg name="personal_name" default=""/>
 
   <arg if="$(eval personal_name == '')" name="frame_prefix" value="" />
+<!--
+  <arg if="$(eval personal_name == '')" name="frame_prefix" value="" />
   <arg if="$(eval personal_name != '')" name="frame_prefix" value="$(arg personal_name)/" />
+-->
 
   <remap from="/diagnostics" to="$(arg namespace)/diagnostics" />
 

hsrb_bringup/launch/include/sensors/dynpick.xml

@@ -3,7 +3,7 @@
   <node pkg="tmc_dynpick_driver" name="dynpick_driver_node" type="dynpick_driver_node" respawn="true" respawn_delay="3" >
     <param name="device" value="$(optenv DYNPICK_DEVICE /dev/dynpick)" />
     <param name="rate" value="100.0" />
-    <param name="frame_id" value="hero/wrist_ft_sensor_frame" />
+    <param name="frame_id" value="wrist_ft_sensor_frame" />
     <rosparam param="frame_to_sensor">
       [0.0, 0.0, 0.0025, -0.707106781, 0.707106781, 0.0, 0.0]
     </rosparam>

hsrb_common_config/params/hsrb_controller_config.yaml

@@ -30,8 +30,8 @@ omni_base_controller:
     odom_t:
       trajectory: 0.1
   stop_trajectory_duration: 1.0
-  wheel_odom_map_frame: hero/odom
-  wheel_odom_base_frame: hero/base_footprint_wheel
+  wheel_odom_map_frame: odom
+  wheel_odom_base_frame: base_footprint_wheel
   odometry_publish_rate: 50
   transform_publish_rate: 50
 

hsrb_common_config/params/hsrb_odometry_config.yaml

@@ -2,5 +2,5 @@ odom_topics:
   wheel_odom: wheel_odom
   laser_odom: laser_odom
 initial_odom: laser_odom
-odom_frame: hero/odom
-odom_child_frame: hero/base_footprint
+odom_frame: odom
+odom_child_frame: base_footprint

hsrb_common_config/params/hsrb_sensor_config.yaml

@@ -1,6 +1,6 @@
 urg_node:
   ip_address: 10.255.255.2
-  frame_id: hero/base_range_sensor_link
+  frame_id: base_range_sensor_link
   # angle range 250[deg]
   angle_min: -2.1
   angle_max: 2.1
@@ -9,8 +9,8 @@ laser_scan_matcher_node:
   max_iterations: 10
   publish_pose_stamped: true
   publish_tf: false
-  fixed_frame: hero/odom
-  base_frame: hero/base_footprint
+  fixed_frame: odom
+  base_frame: base_footprint
   use_odom: false
   outliers_adaptive_order: 0.8
 

hsrb_description/robots/upload_hsrb.launch

@@ -10,9 +10,8 @@
   <arg name="ground_truth_rpy_offset" default="0.0\ 0.0\ 0.0" />
 
   <group ns="$(arg namespace)">
-    <param name="tf_prefix" value="$(arg personal_name)" />
     <param name="robot_description"
-           command="$(find xacro)/xacro --inorder $(find hsrb_description)/robots/$(arg robot_model).urdf.xacro gazebo_visualization_enabled:=$(arg visualization) implicit_damping:=$(arg implicit_damping) personal_name:=$(arg personal_name) ground_truth_xyz_offset:=$(arg ground_truth_xyz_offset) ground_truth_rpy_offset:=$(arg ground_truth_rpy_offset)" />
+           command="$(find xacro)/xacro --inorder $(find hsrb_description)/robots/$(arg robot_model).urdf.xacro gazebo_visualization_enabled:=$(arg visualization) implicit_damping:=$(arg implicit_damping) ground_truth_xyz_offset:=$(arg ground_truth_xyz_offset) ground_truth_rpy_offset:=$(arg ground_truth_rpy_offset)" />
     <param name="robot_collision_pair" textfile="$(find hsrb_description)/robots/collision_pair_hsrb.xml" />
   </group>
 </launch>

hsrb_gazebo_bringup/launch/include/hsrb.xml

@@ -12,7 +12,7 @@
     <arg name="robot_name" default="hsrb" />
 
     <include file="$(find hero_bringup)/hsrb_gazebo_bringup/launch/include/hsrb_no_controllers.xml" pass_all_args="true" />
-    <include file="$(find hsrb_gazebo_bringup)/launch/include/hsrb_default_controllers.xml">
+    <include file="$(find hero_bringup)/hsrb_gazebo_bringup/launch/include/hsrb_default_controllers.xml">
       <arg name="namespace" value="" />
       <arg name="personal_name" value="$(arg personal_name)" />
     </include>

hsrb_gazebo_bringup/launch/include/hsrb_bringup.xml

@@ -23,7 +23,6 @@
 <!--
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
           respawn="false" output="screen">
-      <param name="tf_prefix" value='$(arg personal_name)' />
     </node>
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"
           respawn="false" output="screen" >
@@ -48,7 +47,6 @@
     <node name="robot_state_pub" pkg="robot_state_publisher" type="robot_state_publisher">
       <param name="publish_frequency" value="50.0" />
       <param name="use_tf_static" value="true" />
-      <param name="tf_prefix" value='$(arg personal_name)' />
     </node>
   </group>
 
@@ -73,13 +71,13 @@
 
   <group ns="$(arg namespace)">
     <node name="head_l_stereo_camera_frame_pub" pkg="tf" type="static_transform_publisher"
-          args="0 0 0 0 0 0 $(arg personal_name)/head_l_stereo_camera_link $(arg personal_name)/head_l_stereo_camera_frame 100" />
+          args="0 0 0 0 0 0 head_l_stereo_camera_link head_l_stereo_camera_frame 100" />
     <node name="head_r_stereo_camera_frame_pub" pkg="tf" type="static_transform_publisher"
-          args="0 0 0 0 0 0 $(arg personal_name)/head_r_stereo_camera_link $(arg personal_name)/head_r_stereo_camera_frame 100" />
+          args="0 0 0 0 0 0 head_r_stereo_camera_link head_r_stereo_camera_frame 100" />
     <node name="head_rgbd_sensor_rgb_frame_pub" pkg="tf" type="static_transform_publisher"
-          args="0 0 0 0 0 0 $(arg personal_name)/head_rgbd_sensor_link $(arg personal_name)/head_rgbd_sensor_rgb_frame 100" />
+          args="0 0 0 0 0 0 head_rgbd_sensor_link head_rgbd_sensor_rgb_frame 100" />
     <node name="head_rgbd_sensor_depth_frame_pub" pkg="tf" type="static_transform_publisher"
-          args="0 0 0 0 0 0 $(arg personal_name)/head_rgbd_sensor_link $(arg personal_name)/head_rgbd_sensor_depth_frame 100" />
+          args="0 0 0 0 0 0 head_rgbd_sensor_link head_rgbd_sensor_depth_frame 100" />
   </group>
 
   <include file="$(find hero_bringup)/hsrb_gazebo_bringup/launch/include/hsrb_relay_topics.xml">
@@ -89,8 +87,11 @@
 
   <!-- odometry_switcher -->
   <group ns="$(arg namespace)">
+    <arg name="odom_frame_prefix" value="" />
+<!--
     <arg if="$(eval personal_name == '')" name="odom_frame_prefix" value="" />
     <arg if="$(eval personal_name != '')" name="odom_frame_prefix" value="$(arg personal_name)/" />
+-->
     <node name="odometry_switcher" pkg="tmc_odometry_switcher" type="odometry_switcher_node">
       <!--
       <remap from="switched_odom" to="odom"/>

hsrb_gazebo_bringup/launch/include/hsrb_default_controllers.xml

@@ -0,0 +1,21 @@
+<launch>
+    <arg name="namespace" default="hsrb" />
+    <arg name="personal_name" default="" />
+
+    <group ns="$(arg personal_name)/$(arg namespace)">
+      <rosparam command="load" file="$(find hsrb_common_config)/params/hsrb_controller_config.yaml"/>
+      <rosparam command="load" file="$(find hsrb_gazebo_bringup)/params/hsrb_controller_config.yaml"/>
+      <!-- Remove tf_prefix -->
+      <param name="omni_base_controller/model_name" value="$(arg personal_name)/robot_description" />
+      <node name="controller_spawner"
+            pkg="controller_manager"
+            type="spawner"
+            respawn="false"
+            output="screen"
+            args="joint_state_controller
+                  arm_trajectory_controller
+                  head_trajectory_controller
+                  omni_base_controller
+                  --timeout 60" />
+    </group>
+</launch>

hsrb_gazebo_bringup/launch/include/hsrb_sensor.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="namespace" default="hsrb" />
+  <arg name="personal_name" default="" />
+
+  <group ns="$(arg personal_name)/$(arg namespace)">
+    <node pkg="nodelet" type="nodelet" name="stereo_camera_nodelet_manager"
+          args="manager" output="screen"/>
+
+    <!-- wrist wrench compensation -->
+    <group ns="wrist_wrench">
+      <node name="compensation_node"
+            pkg="tmc_force_torque_sensor_compensation"
+            type="force_torque_sensor_compensation_node">
+        <remap from="/wrench_state" to="raw" />
+        <remap from="~wrench_state" to="compensated" />
+        <remap from="~offset_refresh_triger" to="readjust_offset" />
+        <remap from="~offset_refresh_time" to="readjustment_time" />
+        <rosparam param="config_files" subst_value="True">
+          ["$(find hsrb_manipulation_launch)/config/default_hand_weight_config.yaml"]
+        </rosparam>
+        <remap from="~dead_zone_with_linear_interpolation/threshold"
+               to="dead_zone/threshold" />
+        <remap from="~dead_zone_with_linear_interpolation/interpolation_width"
+               to="dead_zone/interpolation_width" />
+      </node>
+    </group>
+
+    <node pkg="nodelet" type="nodelet" name="metric_rect"
+          args="standalone depth_image_proc/convert_metric record_player_manager --no-bond">
+      <remap from="image_raw" to="head_rgbd_sensor/depth_registered/image_raw"/>
+      <remap from="image" to="head_rgbd_sensor/depth_registered/image"/>
+    </node>
+  </group>
+</launch>

hsrb_impedance_control/launch/impedance_control_node.launch

@@ -3,10 +3,6 @@
   <arg name="personal_name" default="/" />
   <arg name="robot_name" default="hsrb" />
 
-  <arg name="tf_prefix" value=""
-       if="$(eval personal_name == '/' or personal_name == '')" />
-  <arg name="tf_prefix" value="$(arg personal_name)/"
-       if="$(eval personal_name != '/' and personal_name != '')" />
   <group ns="$(arg robot_name)">
     <node pkg="hsrb_impedance_control"
           name="impedance_control"
@@ -18,39 +14,12 @@
       <remap from="~configuration_names" to="~config_names"/>
       <remap from="~robot_description" to="/$(arg personal_name)/robot_description"/>
 
-      <param name="base_frame_id" value="$(arg tf_prefix)odom"/>
-      <param name="robot_frame_id" value="$(arg tf_prefix)base_link"/>
+      <param name="base_frame_id" value="odom"/>
+      <param name="robot_frame_id" value="base_link"/>
       <param name="control_rate" value="50.0"/>
       <param name="command_duration" value="0.3"/>
       <rosparam command="load"
                 file="$(find hsrb_impedance_control)/launch/configurations.yaml"/>
-      <rosparam subst_value="true">
-        origin_frame_id: "$(arg tf_prefix)odom"
-        target_frame_id: "$(arg tf_prefix)hand_palm_link"
-        sensor_frame_id: "$(arg tf_prefix)wrist_ft_sensor_frame"
-        control_configurations/settings:
-          compliance_hard:
-            model_coordinate:
-              frame_id: "$(arg tf_prefix)base_link"
-          compliance_middle:
-            model_coordinate:
-              frame_id: "$(arg tf_prefix)base_link"
-          compliance_soft:
-            model_coordinate:
-              frame_id: "$(arg tf_prefix)base_link"
-          dumper_hard:
-            model_coordinate:
-              frame_id: "$(arg tf_prefix)base_link"
-          dumper_soft:
-            model_coordinate:
-              frame_id: "$(arg tf_prefix)base_link"
-          grasping:
-            model_coordinate:
-              frame_id: "$(arg tf_prefix)hand_palm_link"
-          placing:
-            model_coordinate:
-              frame_id: "$(arg tf_prefix)hand_palm_link"
-      </rosparam>
     </node>
   </group>
 </launch>

launch/simulation/gazebo.launch

@@ -17,6 +17,13 @@
     <arg if="$(eval personal_name == '' and robot_name != '')" name="ns" value="/$(arg robot_name)/" />
     <arg if="$(eval personal_name != '' and robot_name != '')" name="ns" value="/$(arg personal_name)/$(arg robot_name)/" />
 
+    <arg if="$(eval namespace == '' and robot_name == '')" name="ns2" value="/" />
+    <arg if="$(eval namespace != '' and robot_name == '')" name="ns2" value="$(arg namespace)/" />
+    <arg if="$(eval namespace == '' and robot_name != '')" name="ns2" value="/$(arg robot_name)/" />
+    <arg if="$(eval namespace != '' and robot_name != '')" name="ns2" value="$(arg namespace)/$(arg robot_name)/" />
+
+
+
     <!-- General -->
     <remap from="$(arg ns)/joint_states" to="$(arg namespace)/joint_states_hardware" />
     <remap from="$(arg ns)/pressure_sensor" to="$(arg namespace)/pressure_sensor" /> <!-- Gazebo publishes this topic, maybe this is related to the suction. Exact function unknown -->
@@ -27,8 +34,8 @@
     <remap from="$(arg ns)/odom_ground_truth" to="$(arg namespace)/measurements_ground_truth" /> <!-- Gazebo publishes this topic, exact function unknown -->
 
     <!-- Sensors -->
-    <remap from="$(arg ns)/base_scan" to="$(arg namespace)/base_laser/scan" />
-    <remap from="$(arg ns)/base_imu/data" to="$(arg namespace)/base_imu/data" />
+    <remap from="$(arg ns2)/base_scan" to="$(arg namespace)/base_laser/scan" />
+    <remap from="$(arg ns2)/base_imu/data" to="$(arg namespace)/base_imu/data" />
     <remap from="$(arg ns)/base_b_bumper_sensor" to="$(arg namespace)/base_b_bumper_sensor" />
     <remap from="$(arg ns)/base_f_bumper_sensor" to="$(arg namespace)/base_f_bumper_sensor" />
 
@@ -57,16 +64,16 @@
     <remap from="$(arg ns)/gazebo_ros_control/pid_gains/wrist_roll_joint" to="$(arg namespace)/gazebo_ros_control/pid_gains/wrist_roll_joint" />
 
     <!-- gazebo_ros_control -->
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/base_r_drive_wheel_joint" to="/gazebo_ros_control/pid_gains/base_r_drive_wheel_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/base_l_drive_wheel_joint" to="/gazebo_ros_control/pid_gains/base_l_drive_wheel_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/base_roll_joint" to="/gazebo_ros_control/pid_gains/base_roll_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/head_pan_joint" to="/gazebo_ros_control/pid_gains/head_pan_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/head_tilt_joint" to="/gazebo_ros_control/pid_gains/head_tilt_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/arm_lift_joint" to="/gazebo_ros_control/pid_gains/arm_lift_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/arm_flex_joint" to="/gazebo_ros_control/pid_gains/arm_flex_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/arm_roll_joint" to="/gazebo_ros_control/pid_gains/arm_roll_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/wrist_flex_joint" to="/gazebo_ros_control/pid_gains/wrist_flex_joint" />
-    <remap from="/hsrb/gazebo_ros_control/pid_gains/wrist_roll_joint" to="/gazebo_ros_control/pid_gains/wrist_roll_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/base_r_drive_wheel_joint" to="/gazebo_ros_control/pid_gains/base_r_drive_wheel_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/base_l_drive_wheel_joint" to="/gazebo_ros_control/pid_gains/base_l_drive_wheel_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/base_roll_joint" to="/gazebo_ros_control/pid_gains/base_roll_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/head_pan_joint" to="/gazebo_ros_control/pid_gains/head_pan_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/head_tilt_joint" to="/gazebo_ros_control/pid_gains/head_tilt_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/arm_lift_joint" to="/gazebo_ros_control/pid_gains/arm_lift_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/arm_flex_joint" to="/gazebo_ros_control/pid_gains/arm_flex_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/arm_roll_joint" to="/gazebo_ros_control/pid_gains/arm_roll_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/wrist_flex_joint" to="/gazebo_ros_control/pid_gains/wrist_flex_joint" />
+    <remap from="/gazebo_ros_control/pid_gains/wrist_roll_joint" to="/gazebo_ros_control/pid_gains/wrist_roll_joint" />
 
     <!-- General controllers -->
     <remap from="$(arg ns)/controller_manager" to="$(arg namespace)/controller_manager" />

launch/simulation/simulation.launch

@@ -30,7 +30,7 @@
         <arg name="paused" value="$(arg paused)" />
         <arg name="use_sim_time" value="$(arg use_sim_time)" />
         <arg name="world_name" value="$(arg world_name)" />
-        <arg name="personal_name" value="$(arg personal_name)" />
+        <arg name="personal_name" value="" />
         <arg name="robot_name" value="$(arg robot_name)" />
     </include>
 
@@ -45,9 +45,9 @@
         <arg name="robot_name" value="$(arg robot_name)" />
     </include>
 
-    <include file="$(find hsrb_gazebo_bringup)/launch/include/hsrb_sensor.xml" >
-        <arg name="namespace" value="" />
-        <arg name="personal_name" value="$(arg personal_name)" />
+    <include file="$(find hero_bringup)/hsrb_gazebo_bringup/launch/include/hsrb_sensor.xml" >
+        <arg name="namespace" value="$(arg personal_name)" />
+        <arg name="personal_name" value="" />
     </include>
 
     <!-- Pose setter -->

parameters/localization/gmapping.yaml

@@ -1,5 +1,5 @@
-odom_frame: hero/odom
-base_frame: hero/base_footprint
+odom_frame: odom
+base_frame: base_footprint
 tf_prefix: "" # gmapping will convert map to hero/map if no prefix is set.
 delta: 0.025
 

parameters/manipulation/head_cmd_vel_client.yaml

@@ -3,4 +3,4 @@ priority: 5
 pan_vel: 0.3
 tilt_vel: 0.3
 
-frame: hero/base_link
+frame: base_link

parameters/manipulation/head_ed_client.yaml

@@ -3,4 +3,4 @@ priority: 5
 pan_vel: 0.2
 tilt_vel: 0.2
 
-frame: hero/base_link
+frame: base_link

parameters/manipulation/head_look_around_client.yaml

@@ -5,7 +5,7 @@ states:
 
 duration: 1.5
 priority: 10
-frame_id: hero/base_link # TODO: hard coded namespace
+frame_id: base_link # TODO: hard coded namespace
 
 pan_vel: 0.3
 tilt_vel: 0.3

parameters/manipulation/head_server.yaml

@@ -1,4 +1,3 @@
-tf_prefix: hero
 pan_joint_name: head_pan_joint
 tilt_joint_name: head_tilt_joint
 default_pan: 0

parameters/navigation/global_costmap.yaml

@@ -1,7 +1,7 @@
 # SETUP
 
 global_frame: map
-robot_base_frame: hero/base_footprint
+robot_base_frame: base_footprint
 rolling_window: false
 track_unknown_space: false
 resolution: 0.05
@@ -29,8 +29,8 @@ ed_occupancy_grid:
 recent_obstacles:
     occupied_to_default_time: 2.0
     observation_sources: base_laser rgbd_sensor
-    base_laser:  {data_type: LaserScan, sensor_frame: hero/base_range_sensor_link, topic: /hero/base_laser/scan, observation_persistence: 0.0, inf_is_valid: true, obstacle_range: 2.5, marking: true, clearing: false, ray_trace_range: 3.0}
-    rgbd_sensor: {data_type: PointCloud2, sensor_frame: hero/head_rgbd_sensor_link, topic: /hero/ed/navigation/cloud, observation_persistence: 0.0, expected_update_rate: 1.0, min_obstacle_height: 0.0, max_obstacle_height: 0.5, obstacle_range: 10}
+    base_laser:  {data_type: LaserScan, sensor_frame: base_range_sensor_link, topic: /hero/base_laser/scan, observation_persistence: 0.0, inf_is_valid: true, obstacle_range: 2.5, marking: true, clearing: false, ray_trace_range: 3.0}
+    rgbd_sensor: {data_type: PointCloud2, sensor_frame: head_rgbd_sensor_link, topic: /hero/ed/navigation/cloud, observation_persistence: 0.0, expected_update_rate: 1.0, min_obstacle_height: 0.0, max_obstacle_height: 0.5, obstacle_range: 10}
 
 # Workspace to configuration space
 configuration_space:

parameters/navigation/global_costmap_gmapping.yaml

@@ -1,7 +1,7 @@
 # SETUP
 
 global_frame: map
-robot_base_frame: hero/base_footprint
+robot_base_frame: base_footprint
 rolling_window: false
 track_unknown_space: false
 resolution: 0.05

parameters/navigation/local_costmap.yaml

@@ -1,7 +1,7 @@
 # SETUP
 
 global_frame: map
-robot_base_frame: hero/base_footprint
+robot_base_frame: base_footprint
 rolling_window: true
 track_unknown_space: false
 resolution: 0.05
@@ -49,8 +49,8 @@ ed_occupancy_grid:
 recent_obstacles:
     occupied_to_default_time: 2.0
     observation_sources: base_laser rgbd_sensor
-    base_laser:  {data_type: LaserScan, sensor_frame: hero/base_range_sensor_link, topic: /hero/base_laser/scan, observation_persistence: 0.0, inf_is_valid: true, obstacle_range: 2.5, marking: true, clearing: false, ray_trace_range: 3.0}
-    rgbd_sensor: {data_type: PointCloud2, sensor_frame: hero/head_rgbd_sensor_link, topic: /hero/ed/navigation/cloud, observation_persistence: 0.0, expected_update_rate: 0.0, min_obstacle_height: 0.0, max_obstacle_height: 0.5, obstacle_range: 10}
+    base_laser:  {data_type: LaserScan, sensor_frame: base_range_sensor_link, topic: /hero/base_laser/scan, observation_persistence: 0.0, inf_is_valid: true, obstacle_range: 2.5, marking: true, clearing: false, ray_trace_range: 3.0}
+    rgbd_sensor: {data_type: PointCloud2, sensor_frame: head_rgbd_sensor_link, topic: /hero/ed/navigation/cloud, observation_persistence: 0.0, expected_update_rate: 0.0, min_obstacle_height: 0.0, max_obstacle_height: 0.5, obstacle_range: 10}
 
 # Eliminate small occlusions by inflating the obstacles with a radius of obstacles that can popup from unknown space
 eliminate_small_occlusions: