Added Euroc stereo dataset example launch file (ros2). Fixed identity ground_truth when it should be null.

scripts/yaml_to_camera_info.py

@@ -1,6 +1,7 @@
-#!/usr/bin/env python
-import rospy
+#!/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
 
@@ -8,37 +9,54 @@ def yaml_to_CameraInfo(yaml_fname):
     with open(yaml_fname, "r") as file_handle:
         calib_data = yaml.load(file_handle)
  
-    camera_info_msg = CameraInfo()
-    camera_info_msg.width = calib_data["image_width"]
-    camera_info_msg.height = calib_data["image_height"]
-    camera_info_msg.K = calib_data["camera_matrix"]["data"]
-    camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
-    camera_info_msg.R = calib_data["rectification_matrix"]["data"]
-    camera_info_msg.P = calib_data["projection_matrix"]["data"]
-    camera_info_msg.distortion_model = calib_data["distortion_model"]
-    return camera_info_msg
+    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
 
-def callback(image):
-    global publisher
-    global camera_info_msg
-    global frameId
-    camera_info_msg.header = image.header
-    if frameId:
-        camera_info_msg.header.frame_id = frameId
-    publisher.publish(camera_info_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__":
-
-    rospy.init_node("yaml_to_camera_info", anonymous=True)
-
-    yaml_path = rospy.get_param('~yaml_path', '')
-    if not yaml_path:
-        print('yaml_path parameter should be set to path of the calibration file!')
-        sys.exit(1)
-
-    frameId = rospy.get_param('~frame_id', '')
-    camera_info_msg = yaml_to_CameraInfo(yaml_path)
-    
-    publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1)
-    rospy.Subscriber("image", Image, callback, queue_size=1)
-    rospy.spin()
+    main()