Go to the documentation of this file.00001
00002 import rospy
00003 import yaml
00004 from sensor_msgs.msg import CameraInfo
00005 from sensor_msgs.msg import Image
00006
00007 def yaml_to_CameraInfo(yaml_fname):
00008 with open(yaml_fname, "r") as file_handle:
00009 calib_data = yaml.load(file_handle)
00010
00011 camera_info_msg = CameraInfo()
00012 camera_info_msg.width = calib_data["image_width"]
00013 camera_info_msg.height = calib_data["image_height"]
00014 camera_info_msg.K = calib_data["camera_matrix"]["data"]
00015 camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
00016 camera_info_msg.R = calib_data["rectification_matrix"]["data"]
00017 camera_info_msg.P = calib_data["projection_matrix"]["data"]
00018 camera_info_msg.distortion_model = calib_data["distortion_model"]
00019 return camera_info_msg
00020
00021 def callback(image):
00022 global publisher
00023 global camera_info_msg
00024 camera_info_msg.header = image.header
00025 publisher.publish(camera_info_msg)
00026
00027 if __name__ == "__main__":
00028
00029 rospy.init_node("yaml_to_camera_info", anonymous=True)
00030
00031 yaml_path = rospy.get_param('~yaml_path', '')
00032 if not yaml_path:
00033 print 'yaml_path parameter should be set to path of the calibration file!'
00034 sys.exit(1)
00035
00036 camera_info_msg = yaml_to_CameraInfo(yaml_path)
00037
00038 publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1)
00039 rospy.Subscriber("image", Image, callback, queue_size=1)
00040 rospy.spin()