ported rtabmap_ros_pkg_split to ros2

rtabmap_util/scripts/yaml_to_camera_info.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+import rclpy
+import yaml
+from rclpy.node import Node
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import Image
+
+def yaml_to_CameraInfo(yaml_fname):
+    with open(yaml_fname, "r") as file_handle:
+        calib_data = yaml.load(file_handle)
+ 
+    msg = CameraInfo()
+    msg.width = calib_data["image_width"]
+    msg.height = calib_data["image_height"]
+    msg.k = calib_data["camera_matrix"]["data"]
+    msg.d = calib_data["distortion_coefficients"]["data"]
+    msg.r = calib_data["rectification_matrix"]["data"]
+    msg.p = calib_data["projection_matrix"]["data"]
+    msg.distortion_model = calib_data["distortion_model"]
+    return msg
+
+class YamlToCameraInfo(Node):
+
+    def __init__(self):
+        super().__init__('yaml_to_camera_info')
+        
+        self.declare_parameter('yaml_path', '')
+        yaml_path = self.get_parameter('yaml_path').get_parameter_value().string_value
+
+        if not yaml_path:
+            print('yaml_path parameter should be set to path of the calibration file!')
+            sys.exit(1)
+
+        self.declare_parameter('frame_id', '')
+        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
+        
+        self.camera_info_msg = yaml_to_CameraInfo(yaml_path)
+
+        self.publisher_ = self.create_publisher(CameraInfo, 'camera_info', 1)
+        self.subscription = self.create_subscription(
+            Image,
+            'image',
+            self.callback,
+            1)
+        self.subscription  # prevent unused variable warning
+
+    def callback(self, image):
+        self.camera_info_msg.header = image.header
+        if self.frame_id:
+            self.camera_info_msg.header.frame_id = self.frame_id
+        self.publisher_.publish(self.camera_info_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    yaml_to_camera_info = YamlToCameraInfo()
+    rclpy.spin(yaml_to_camera_info)
+    yaml_to_camera_info.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()