Merge pull request #2 from jmirabel/master

Update republish + config for position control

roscontrol_sot_talos/config/sot_talos_params.yaml

@@ -7,8 +7,8 @@ sot_controller:
     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
   ]
   map_rc_to_sot_device: { motor-angles: motor-angles ,
-  joint-angles: joint-angles, velocities: velocities,
-  torques: torques, cmd-joints: control, cmd-effort: control }
+  joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
+  torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
   position_control_init_pos:
     - name: leg_left_1_joint
       des_pos: 0.0

roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml

@@ -8,8 +8,8 @@ sot_controller:
     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
   ]
   map_rc_to_sot_device: { motor-angles: motor-angles ,
-  joint-angles: joint-angles, velocities: velocities,
-  torques: torques, cmd-joints: control, cmd-effort: control }
+  joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
+  torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
   position_control_init_pos:
     - name: leg_left_1_joint
       des_pos: 0.0

roscontrol_sot_talos/launch/display_sot.launch

@@ -2,11 +2,11 @@
   <arg name="input_topic" default="/sot_hpp/state"/>
   <arg name="output_prefix" default="/sot"/>
 
-  <node pkg="roscontrol_sot_talos" name="republish" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state"/>
+  <node pkg="roscontrol_sot_talos" name="republish" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state true"/>
 
   <node pkg="tf" type="static_transform_publisher"
     name="base_link_broadcaster"
-    args="0 0 0 0 0 0 1 base_link $(arg output_prefix)/base_link 100" />
+    args="0 0 0 0 0 0 1 odom $(arg output_prefix)/odom 100" />
 
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" >
     <remap from="/joint_states" to="$(arg output_prefix)/joint_state" />

roscontrol_sot_talos/scripts/republish

 #!/usr/bin/env python
 # license removed for brevity
-import rospy
+import rospy, tf
 from dynamic_graph_bridge_msgs.msg import Vector
 from sensor_msgs.msg import JointState
 import sys
 
+# Arg 1
+input_topic = "/sot_hpp/state"
+# Arg 2
+output_topic = "/sot/joint_state"
+# Arg 3
+publish_root_wrt_odom = False
 if len(sys.argv)>1:
     input_topic = sys.argv[1]
     if len(sys.argv)>2:
         output_topic = sys.argv[2]
-    else:
-        output_topic = "/sot/joint_state"
-else:
-    input_topic = "/sot_hpp/state"
+        if len(sys.argv)>3:
+            publish_root_wrt_odom = (sys.argv[3].lower() in ("true","on"))
 
 rospy.init_node('sot_reemitter', anonymous=True)
 pub = rospy.Publisher(output_topic, JointState, queue_size=10)
@@ -20,13 +24,21 @@ pub = rospy.Publisher(output_topic, JointState, queue_size=10)
 seqnb = 0
 aJS = JointState()
 
+if publish_root_wrt_odom:
+    if output_topic.find('/') >= 0:
+        output_prefix = output_topic.rsplit('/',1)[0] + '/'
+    else:
+        output_prefix = ""
+    rospy.loginfo("Will publish " + output_prefix + "base_link with respect to " + output_prefix + "odom")
+
 jointnames =  rospy.get_param("/sot_controller/joint_names")
     
 def jointreceived(jstates):
     global seqnb
     seqnb = seqnb+1
+    time = rospy.Time.now()
     aJS.header.seq = seqnb
-    aJS.header.stamp = rospy.Time.now()
+    aJS.header.stamp = time
     aJS.header.frame_id = "base_link"
     aJS.name = jointnames
     aJS.position = jstates.data[6:]
@@ -34,6 +46,15 @@ def jointreceived(jstates):
     aJS.effort = []
     pub.publish(aJS)
 
+    if publish_root_wrt_odom and len(jstates.data) > 6:
+        br = tf.TransformBroadcaster()
+        br.sendTransform(jstates.data[0:3],
+                         tf.transformations.quaternion_from_euler(
+                             jstates.data[3], jstates.data[4], jstates.data[5]),
+                         time,
+                         output_prefix + "base_link",
+                         output_prefix + "odom")
+
 def listener():
     rospy.Subscriber(input_topic, Vector, jointreceived)
     rospy.spin()