From 764883aca2fe2546dc268781363799676e36e009 Mon Sep 17 00:00:00 2001
From: Benjamin Coudrin <benjamin.coudrin@laas.fr>
Date: Fri, 22 Nov 2013 15:45:37 +0100
Subject: [PATCH] Add a script to publish JointState to TF
---
scripts/robot_pose_publisher | 32 ++++++++++++++++++++++++++++++++
1 file changed, 32 insertions(+)
create mode 100755 scripts/robot_pose_publisher
diff --git a/scripts/robot_pose_publisher b/scripts/robot_pose_publisher
new file mode 100755
index 0000000..855a8e6
--- /dev/null
+++ b/scripts/robot_pose_publisher
@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+#
+# Listens to TransformStamped messages and publish them to tf
+#
+
+import roslib
+roslib.load_manifest('dynamic_graph_bridge')
+import rospy
+
+import tf
+#import geometry_msgs.msg
+import sensor_msgs.msg
+
+frame = ''
+childFrame = ''
+
+def pose_broadcaster(msg):
+ translation = msg.position[0:3];
+ rotation = tf.transformations.quaternion_from_euler(msg.position[3], msg.position[4], msg.position[5])
+ tfbr = tf.TransformBroadcaster()
+ tfbr.sendTransform(translation, rotation,
+ rospy.Time.now(), childFrame, frame)
+
+if __name__ == '__main__':
+ rospy.init_node('robot_pose_publisher', anonymous=True)
+
+ frame = rospy.get_param('~frame', 'odom')
+ childFrame = rospy.get_param('~child_frame', 'base_link')
+ topic = rospy.get_param('~topic', 'joint_states')
+
+ rospy.Subscriber(topic, sensor_msgs.msg.JointState, pose_broadcaster)
+ rospy.spin()
--
GitLab