diff --git a/bin/tf_broadcaster b/bin/tf_broadcaster new file mode 100755 index 0000000000000000000000000000000000000000..ae1f2010d61744a2c2dbd96ddbb821d1c8dbe69f --- /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()