Gripper command group missing name bugfix

stretch_core/nodes/command_groups.py

@@ -286,6 +286,7 @@ class WristYawCommandGroup(SimpleCommandGroup):
 
 class GripperCommandGroup(SimpleCommandGroup):
     def __init__(self, range_robotis, robot=None):
+        self.gripper_conversion = GripperConversion()
         if (range_robotis is None and robot is not None and robot.end_of_arm is not None
             and robot.end_of_arm.is_tool_present('StretchGripper')):
             range_ticks = robot.end_of_arm.motors['stretch_gripper'].params['range_t']
@@ -297,9 +298,8 @@ class GripperCommandGroup(SimpleCommandGroup):
                                     self.gripper_conversion.robotis_to_aperture(range_robotis[1]))
             self.range_finger_rad = (self.gripper_conversion.robotis_to_finger(range_robotis[0]),
                                     self.gripper_conversion.robotis_to_finger(range_robotis[1]))
-        SimpleCommandGroup.__init__(self, None, None, acceptable_joint_error=1.0)
+        SimpleCommandGroup.__init__(self, 'joint_gripper', None, acceptable_joint_error=1.0)
         self.gripper_joint_names = ['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']
-        self.gripper_conversion = GripperConversion()
 
     def update(self, commanded_joint_names, invalid_joints_callback, **kwargs):
         self.active = False

stretch_core/nodes/stretch_driver

@@ -144,12 +144,24 @@ class StretchBodyNode:
         joint_state = JointState()
         joint_state.header.stamp = current_time
         for cg in self.joint_trajectory_action.command_groups:
-            joint_state.name.append(cg.name)
-            pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode,
-                                              manipulation_origin=self.mobile_base_manipulation_origin)
-            joint_state.position.append(pos)
-            joint_state.velocity.append(vel)
-            joint_state.effort.append(effort)
+            if cg.name == 'joint_gripper':
+                joint_state.name.append('joint_gripper_finger_left')
+                joint_state.name.append('joint_gripper_finger_right')
+                pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode,
+                                                manipulation_origin=self.mobile_base_manipulation_origin)
+                joint_state.position.append(pos)
+                joint_state.position.append(pos)
+                joint_state.velocity.append(vel)
+                joint_state.velocity.append(vel)
+                joint_state.effort.append(effort)
+                joint_state.effort.append(effort)
+            else:
+                joint_state.name.append(cg.name)
+                pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode,
+                                                manipulation_origin=self.mobile_base_manipulation_origin)
+                joint_state.position.append(pos)
+                joint_state.velocity.append(vel)
+                joint_state.effort.append(effort)
         self.joint_state_pub.publish(joint_state)
 
         ##################################################