Skip to content
Snippets Groups Projects
Commit 764883ac authored by Benjamin Coudrin's avatar Benjamin Coudrin Committed by Francois Keith
Browse files

Add a script to publish JointState to TF

parent 370408d7
No related branches found
No related tags found
No related merge requests found
#!/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()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment