Publish base_link position wrt odom in republish

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
 
+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"
+    if len(sys.argv)>3:
+        publish_root_wrt_odom = (sys.argv[3].lower() in ("true","on"))
 else:
     input_topic = "/sot_hpp/state"
 
@@ -20,13 +23,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 +45,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()