Add launch file to display the SoT output in rviz (servo off mode)

roscontrol_sot_talos/launch/display_sot.launch

+<launch>
+  <node pkg="roscontrol_sot_talos" name="republish" type="republish"/>
+
+  <node pkg="tf" type="static_transform_publisher"
+    name="base_link_broadcaster"
+    args="0 0 0 0 0 0 1 base_link sot/base_link 100" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" >
+    <remap from="/joint_states" to="/sot/joint_state" />
+    <param name="tf_prefix" value="/sot" />
+  </node>
+
+  <node pkg="rviz" type="rviz" name="rviz" />
+</launch>

roscontrol_sot_talos/scripts/republish.py → roscontrol_sot_talos/scripts/republish

@@ -5,28 +5,37 @@ from dynamic_graph_bridge_msgs.msg import Vector
 from sensor_msgs.msg import JointState
 
 rospy.init_node('sot_reemitter', anonymous=True)
-pub = rospy.Publisher('/sot/joint_state', String, queue_size=10)
+pub = rospy.Publisher('/sot/joint_state', JointState, queue_size=10)
     
+seqnb = 0
 aJS = JointState()
-   
+
+jointnames =  [
+'leg_left_1_joint', 'leg_left_2_joint', 'leg_left_3_joint', 'leg_left_4_joint', 'leg_left_5_joint', 'leg_left_6_joint',
+'leg_right_1_joint', 'leg_right_2_joint', 'leg_right_3_joint', 'leg_right_4_joint', 'leg_right_5_joint', 'leg_right_6_joint',
+'torso_1_joint', 'torso_2_joint',
+'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint', 'gripper_left_joint','arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint',  'gripper_right_joint',
+'head_1_joint', 'head_2_joint',
+]
+    
 def jointreceived(jstates):
-    aJS.header.seq = seqnb
-    pub.publish(hello_str)
+    global seqnb
     seqnb = seqnb+1
-    seqnb.stamp = ros::Time::now()
-    seqnb.frameid = "base_link"
-    seqnb.name = "joint_state"
-    seqnb.position = jstates.data[6:]
-    seqnb.velocity = []
-    seqnb.effort = []
+    aJS.header.seq = seqnb
+    aJS.header.stamp = rospy.Time.now()
+    aJS.header.frame_id = "base_link"
+    aJS.name = jointnames
+    aJS.position = jstates.data[6:]
+    aJS.velocity = []
+    aJS.effort = []
+    pub.publish(aJS)
 
 def listener():
     rospy.Subscriber("/sot_hpp/state", Vector, jointreceived)
-    ros.spin()
+    rospy.spin()
     
 if __name__ == '__main__':
     try:
         listener()
     except rospy.ROSInterruptException:
         pass
-