static_transform_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('camera_pose_calibration')
00004 import tf2_ros
00005 import rospy
00006 import PyKDL
00007 from tf_conversions import posemath
00008 from geometry_msgs.msg import TransformStamped
00009 
00010 
00011 class StaticPublisher:
00012     def __init__(self):
00013         self.pub = tf2_ros.TransformBroadcaster()
00014         pose = rospy.get_param('~transform')
00015         self.pose_msg = TransformStamped()
00016         self.pose_msg.header.frame_id = pose['parent_id']
00017         self.pose_msg.child_frame_id = pose['child_id']
00018         self.pose_msg.transform.translation.x = pose['translation'][0]
00019         self.pose_msg.transform.translation.y = pose['translation'][1]
00020         self.pose_msg.transform.translation.z = pose['translation'][2]
00021         q = self.pose_msg.transform.rotation
00022 
00023         if 'rotation' in pose and 'quaternion' in pose:
00024             rospy.logfatal("'quaterion' and 'rotation' specified. Please remove one")
00025             raise
00026 
00027         if 'rotation' in pose:
00028             (q.x, q.y, q.z, q.w) = PyKDL.Rotation(*pose['rotation']).GetQuaternion()
00029         elif 'quaternion' in pose:
00030             r = pose['quaternion']
00031             q.x = r['x']
00032             q.y = r['y']
00033             q.z = r['z']
00034             q.w = r['w']
00035         else:
00036             rospy.logfatal("No rotation specified. Use 'rotation' or 'quaternion'.")
00037             raise
00038              
00039 
00040 
00041     def publish(self):
00042         self.pose_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
00043         self.pub.sendTransform(self.pose_msg)
00044 
00045 
00046 
00047 def main():
00048     rospy.init_node('static_tf_publisher')
00049     s = StaticPublisher()
00050     r = rospy.Rate(5)
00051     while not rospy.is_shutdown():
00052         s.publish()
00053         r.sleep()
00054 
00055 
00056 if __name__ == '__main__':
00057     main()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08