jsk_topic_tools: tf_to_pose.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import tf
00005 from geometry_msgs.msg import PoseStamped
00006 
00007 
00008 def cb(event):
00009     stamp = rospy.Time.now()
00010     try:
00011         listener.waitForTransform(src_frame, dst_frame, stamp,
00012                                 timeout=rospy.Duration(1))
00013     except Exception, e:
00014         rospy.logerr(e)
00015         return
00016 
00017     dst_pose = listener.lookupTransform(src_frame, dst_frame, stamp)
00018 
00019     pose_msg = PoseStamped()
00020     pose_msg.header.frame_id = src_frame
00021     pose_msg.header.stamp = stamp
00022     pose_msg.pose.position.x = dst_pose[0][0]
00023     pose_msg.pose.position.y = dst_pose[0][1]
00024     pose_msg.pose.position.z = dst_pose[0][2]
00025     pose_msg.pose.orientation.x = dst_pose[1][0]
00026     pose_msg.pose.orientation.y = dst_pose[1][1]
00027     pose_msg.pose.orientation.z = dst_pose[1][2]
00028     pose_msg.pose.orientation.w = dst_pose[1][3]
00029 
00030     pub.publish(pose_msg)
00031 
00032 
00033 if __name__ == '__main__':
00034     rospy.init_node('tf_to_pose')
00035     pub = rospy.Publisher('~output', PoseStamped, queue_size=1)
00036     src_frame = rospy.get_param('~src_frame')
00037     dst_frame = rospy.get_param('~dst_frame')
00038     rate = rospy.get_param('~rate', 1.)
00039     timer = rospy.Timer(rospy.Duration(1.0 / rate), cb)
00040     listener = tf.TransformListener()
00041     rospy.spin()