yaml_to_camera_info.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import rospy
3 import yaml
4 from sensor_msgs.msg import CameraInfo
5 from sensor_msgs.msg import Image
6 
7 def yaml_to_CameraInfo(yaml_fname):
8  with open(yaml_fname, "r") as file_handle:
9  calib_data = yaml.load(file_handle)
10 
11  camera_info_msg = CameraInfo()
12  camera_info_msg.width = calib_data["image_width"]
13  camera_info_msg.height = calib_data["image_height"]
14  camera_info_msg.K = calib_data["camera_matrix"]["data"]
15  camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
16  camera_info_msg.R = calib_data["rectification_matrix"]["data"]
17  camera_info_msg.P = calib_data["projection_matrix"]["data"]
18  camera_info_msg.distortion_model = calib_data["distortion_model"]
19  return camera_info_msg
20 
21 def callback(image):
22  global publisher
23  global camera_info_msg
24  global frameId
25  camera_info_msg.header = image.header
26  if frameId:
27  camera_info_msg.header.frame_id = frameId
28  publisher.publish(camera_info_msg)
29 
30 if __name__ == "__main__":
31 
32  rospy.init_node("yaml_to_camera_info", anonymous=True)
33 
34  yaml_path = rospy.get_param('~yaml_path', '')
35  if not yaml_path:
36  print('yaml_path parameter should be set to path of the calibration file!')
37  sys.exit(1)
38 
39  frameId = rospy.get_param('~frame_id', '')
40  camera_info_msg = yaml_to_CameraInfo(yaml_path)
41 
42  publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1)
43  rospy.Subscriber("image", Image, callback, queue_size=1)
44  rospy.spin()
def yaml_to_CameraInfo(yaml_fname)


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40