Fix joint names.

diff --git a/wam_node_sim/launch/gazebo.launch b/wam_node_sim/launch/gazebo.launch
index bb011b7..33d3f3f 100644 (file)
--- a/wam_node_sim/launch/gazebo.launch
+++ b/wam_node_sim/launch/gazebo.launch
@@ -9,20 +9,27 @@
        <arg name="controller" default="pid_plus_gravity_controller"/>
        <arg name="config" default="$(find wam_bringup)/config/$(arg controller).yaml"/>
 
-       <include file="$(find wam_bringup)/launch/gazebo.launch" >
-               <arg name="paused" value="$(arg paused)"/>
-               <arg name="headless" value="$(arg headless)"/>
-               <arg name="use_sim_time" value="$(arg use_sim_time)"/>
-               <arg name="table" value="$(arg table)"/>
-               <arg name="bhand" value="$(arg bhand)"/>
-               <arg name="controller" value="$(arg controller)" />
-               <arg name="config" value="$(arg config)" />
-       </include>
+       <group>
+               <remap from="/joint_states" to="gazebo/joint_states" />
+               <remap from="wam/command" to="controller/command" />
 
-       <remap from="wam/command" to="controller/command" />
+               <include file="$(find wam_bringup)/launch/gazebo.launch" >
+                       <arg name="paused" value="$(arg paused)"/>
+                       <arg name="headless" value="$(arg headless)"/>
+                       <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+                       <arg name="table" value="$(arg table)"/>
+                       <arg name="bhand" value="$(arg bhand)"/>
+                       <arg name="controller" value="$(arg controller)" />
+                       <arg name="config" value="$(arg config)" />
+               </include>
 
-       <include file="$(find wam_node_sim)/launch/wam_node_sim.launch" />
-       <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
-               <remap from="/joint_states" to="wam/joint_states" />
+               <include file="$(find wam_node_sim)/launch/wam_node_sim.launch" />
+       </group>
+
+       <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+               <rosparam param="source_list">["/wam/joint_states","/bhand/joint_states"]</rosparam>
        </node>
+
+       <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
 </launch>

diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp
index b95af1e..2bfa79f 100644 (file)
--- a/wam_node_sim/src/wam_node_sim.cpp
+++ b/wam_node_sim/src/wam_node_sim.cpp
@@ -235,7 +235,8 @@ template<size_t DOF>
       hand_sprd_vel_srv = nh_.advertiseService("spread_vel", &WamNode::handSpreadVel, this); // bhand/spread_vel
 
       //Set up the BarrettHand joint state publisher
-      const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"};
+//      const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"};
+      const char* bhand_jnts[] = {"bhand_finger1_joint_2", "bhand_finger2_joint_2", "bhand_finger3_joint_2", "bhand_spread", "bhand_finger1_joint_3", "bhand_finger2_joint_3", "bhand_finger3_joint_3"};
       std::vector < std::string > bhand_joints(bhand_jnts, bhand_jnts + 7);
       bhand_joint_state.name.resize(7);
       bhand_joint_state.name = bhand_joints;
@@ -245,7 +246,8 @@ template<size_t DOF>
     wam.gravityCompensate(true); // Turning on Gravity Compenstation by Default when starting the WAM Node
 
     //Setting up WAM joint state publisher
-    const char* wam_jnts[] = {"wam_j1", "wam_j2", "wam_j3", "wam_j4", "wam_j5", "wam_j6", "wam_j7"};
+//    const char* wam_jnts[] = {"wam_j1", "wam_j2", "wam_j3", "wam_j4", "wam_j5", "wam_j6", "wam_j7"};
+    const char* wam_jnts[] = {"wam_joint_1", "wam_joint_2", "wam_joint_3", "wam_joint_4", "wam_joint_5", "wam_joint_6", "wam_joint_7"};
     std::vector < std::string > wam_joints(wam_jnts, wam_jnts + 7);
     wam_joint_state.name = wam_joints;
     wam_joint_state.name.resize(DOF);