00001
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()
