From cd962bb53182e75b7a107f24f8e90d0ac1ce4f60 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 20 Jun 2018 11:10:45 +0200
Subject: [PATCH] Publish base_link position wrt odom in republish
---
.../launch/display_sot.launch | 4 ++--
roscontrol_sot_talos/scripts/republish | 24 +++++++++++++++++--
2 files changed, 24 insertions(+), 4 deletions(-)
diff --git a/roscontrol_sot_talos/launch/display_sot.launch b/roscontrol_sot_talos/launch/display_sot.launch
index 27fa8e7..1c5a9fd 100644
--- a/roscontrol_sot_talos/launch/display_sot.launch
+++ b/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" />
diff --git a/roscontrol_sot_talos/scripts/republish b/roscontrol_sot_talos/scripts/republish
index c362213..e64de85 100755
--- a/roscontrol_sot_talos/scripts/republish
+++ b/roscontrol_sot_talos/scripts/republish
@@ -1,16 +1,19 @@
#!/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()
--
GitLab