From e348f8c8eba9ad76ef29993deb32514c2d80751d Mon Sep 17 00:00:00 2001
From: Thomas Moulard <thomas.moulard@gmail.com>
Date: Sat, 11 Feb 2012 17:35:43 +0100
Subject: [PATCH] Add tf_publisher.
---
scripts/tf_publisher | 65 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 65 insertions(+)
create mode 100755 scripts/tf_publisher
diff --git a/scripts/tf_publisher b/scripts/tf_publisher
new file mode 100755
index 0000000..bd2d6ee
--- /dev/null
+++ b/scripts/tf_publisher
@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+#
+# This script looks for a particular tf transformation
+# and publish it as a TransformStamped topic.
+# This may be useful to insert tf frames into dynamic-graph
+# through dynamic_graph_bridge.
+#
+
+import roslib
+roslib.load_manifest('dynamic_graph_bridge')
+import rospy
+
+import tf
+import geometry_msgs.msg
+
+
+def main():
+ rospy.init_node('tf_publisher', anonymous=True)
+
+ frame = rospy.get_param('~frame', '')
+ childFrame = rospy.get_param('~child_frame', '')
+ topic = rospy.get_param('~topic', '')
+ rateSeconds = rospy.get_param('~rate', 5)
+
+ if not frame or not childFrame or not topic:
+ logpy.error("frame, childFrame and topic are required parameters")
+ return
+
+ rate = rospy.Rate(rateSeconds)
+ tl = tf.TransformListener()
+ pub = rospy.Publisher(topic, geometry_msgs.msg.TransformStamped)
+
+ transform = geometry_msgs.msg.TransformStamped()
+ transform.header.frame_id = frame
+ transform.child_frame_id = childFrame
+
+ ok = False
+ while not rospy.is_shutdown() and not ok:
+ try:
+ tl.waitForTransform(childFrame, frame,
+ rospy.Time(), rospy.Duration(0.1))
+ ok = True
+ except tf.Exception, e:
+ rospy.logwarn("waiting for tf transform")
+ ok = False
+
+ while not rospy.is_shutdown():
+ time = tl.getLatestCommonTime(frame, childFrame)
+ (p, q) = tl.lookupTransform(childFrame, frame, time)
+ transform.header.seq += 1
+ transform.header.stamp = time
+
+ transform.transform.translation.x = p[0]
+ transform.transform.translation.y = p[1]
+ transform.transform.translation.z = p[2]
+
+ transform.transform.rotation.x = q[0]
+ transform.transform.rotation.y = q[1]
+ transform.transform.rotation.z = q[2]
+ transform.transform.rotation.w = q[3]
+
+ pub.publish(transform)
+ rate.sleep()
+
+main()
--
GitLab