Arm joint state missing telescoping links bugfix

stretch_core/nodes/stretch_driver

@@ -144,21 +144,29 @@ class StretchBodyNode:
         joint_state = JointState()
         joint_state.header.stamp = current_time
         for cg in self.joint_trajectory_action.command_groups:
+            pos, vel, effort = cg.joint_state(robot_status, robot_mode=self.robot_mode,
+                               manipulation_origin=self.mobile_base_manipulation_origin)
             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)
+            elif cg.name == 'wrist_extension':
+                joint_state.name.append(cg.name)
+                joint_state.name.extend(cg.telescoping_joints)
+                joint_state.position.append(pos)
+                joint_state.velocity.append(vel)
+                joint_state.effort.append(effort)
+                for _ in range(len(cg.telescoping_joints)):
+                    joint_state.position.append(pos / len(cg.telescoping_joints))
+                    joint_state.velocity.append(vel / len(cg.telescoping_joints))
+                    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)