From 24d7d1ec0fa2fb8acfa40ff89e9b87d1c768626c Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Wed, 23 Nov 2011 19:12:19 +0100
Subject: [PATCH] Add tf_broadcaster.
---
bin/tf_broadcaster | 54 ++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 54 insertions(+)
create mode 100755 bin/tf_broadcaster
diff --git a/bin/tf_broadcaster b/bin/tf_broadcaster
new file mode 100755
index 0000000..ae1f201
--- /dev/null
+++ b/bin/tf_broadcaster
@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+
+# This node subscribes to topic transformation published by
+# dynamic_graph_bridge and forward them into tf.
+#
+# This is required as there is no easy way to publish to tf in
+# real-time.
+
+import roslib
+roslib.load_manifest('dynamic_graph_bridge')
+import rospy
+
+import tf
+import geometry_msgs.msg
+
+def handleTransform(msg, topic):
+ br = tf.TransformBroadcaster()
+ br.sendTransform((msg.transform.translation.x,
+ msg.transform.translation.y,
+ msg.transform.translation.z),
+ (msg.transform.rotation.x,
+ msg.transform.rotation.y,
+ msg.transform.rotation.z,
+ msg.transform.rotation.w),
+ msg.header.stamp,
+ topic,
+ #msg.child_frame_id,
+ '/world')
+ #msg.header.frame_id)
+
+def handleVector(msg, topic):
+ br = tf.TransformBroadcaster()
+ br.sendTransform((msg.vector.x, msg.vector.y, msg.vector.z), (0, 0, 0, 1),
+ msg.header.stamp,
+ topic,
+ #msg.child_frame_id,
+ '/world')
+ #msg.header.frame_id)
+
+if __name__ == '__main__':
+ rospy.init_node('dynamic_graph_broadcaster')
+
+ topicsTransform = ['base_link', 'base_footprint']
+ topicsVector = ['com', 'zmp']
+
+ for topic in topicsTransform:
+ rospy.Subscriber('/dynamic_graph/{0}'.format(topic),
+ geometry_msgs.msg.TransformStamped,
+ handleTransform, topic)
+ for topic in topicsVector:
+ rospy.Subscriber('/dynamic_graph/{0}'.format(topic),
+ geometry_msgs.msg.Vector3Stamped,
+ handleVector, topic)
+ rospy.spin()
--
GitLab