calibration_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 import threading
00008 from tf_conversions import posemath
00009 from geometry_msgs.msg import TransformStamped, Pose
00010 from camera_pose_calibration.msg import CameraCalibration
00011 
00012 
00013 class CameraPublisher:
00014     def __init__(self, pose, child_frame_id):
00015         self.lock = threading.Lock()
00016         self.pub = tf2_ros.TransformBroadcaster()
00017         self.set_pose(pose, child_frame_id)
00018 
00019     def set_pose(self, pose, child_frame_id):
00020         with self.lock:
00021             self.transform = TransformStamped()
00022             self.transform.header.frame_id = 'world'
00023             self.transform.child_frame_id = child_frame_id
00024             self.transform.transform.translation.x = pose.position.x
00025             self.transform.transform.translation.y = pose.position.y
00026             self.transform.transform.translation.z = pose.position.z
00027             self.transform.transform.rotation.x = pose.orientation.x
00028             self.transform.transform.rotation.y = pose.orientation.y
00029             self.transform.transform.rotation.z = pose.orientation.z
00030             self.transform.transform.rotation.w = pose.orientation.w
00031 
00032     def publish(self):
00033         with self.lock:
00034             self.transform.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
00035             self.pub.sendTransform(self.transform)
00036 
00037 
00038 
00039 class CalibrationPublishManager:
00040     def __init__(self):
00041         self.lock = threading.Lock()
00042         self.publish_list = {}
00043         self.sub = rospy.Subscriber('camera_calibration', CameraCalibration, self.cal_cb)
00044 
00045     def cal_cb(self, msg):
00046         with self.lock:
00047             self.publish_list = {}
00048             for pose, camera in zip(msg.camera_pose, msg.camera_id):
00049                 self.publish_list[camera] = CameraPublisher(pose, camera)
00050 
00051 
00052     def publish(self):
00053         with self.lock:
00054             for name, pub in self.publish_list.iteritems():
00055                 pub.publish()
00056 
00057 
00058 
00059 def main():
00060     rospy.init_node('calibration_tf_publisher')
00061     r = rospy.Rate(5)
00062     c = CalibrationPublishManager()
00063     while not rospy.is_shutdown():
00064         c.publish()
00065         try:
00066             r.sleep()
00067         except:
00068             print "Shutting down"
00069 
00070 
00071 if __name__ == '__main__':
00072     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