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

scripts/transform_to_tf.py

@@ -1,32 +1,44 @@
-#!/usr/bin/env python
-import rospy
-import tf
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
 from geometry_msgs.msg import TransformStamped
+from tf2_ros import TransformBroadcaster
 
-def callback(transform):
-    global br
-    global frame_id
-    global child_frame_id
-    local_frame_id = transform.header.frame_id
-    local_child_frame_id = transform.child_frame_id
-    if not local_frame_id:
-        local_frame_id = frame_id
-    if not local_child_frame_id:
-        local_child_frame_id = child_frame_id
-    br.sendTransform(
-        (transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z),
-        (transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w),
-        transform.header.stamp,
-        child_frame_id,
-        frame_id)
+class TransformToTf(Node):
+
+    def __init__(self):
+        super().__init__('transform_to_tf')
+        
+        self.declare_parameter('frame_id', 'world')
+        self.declare_parameter('child_frame_id', 'transform')
+        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
+        self.child_frame_id = self.get_parameter('child_frame_id').get_parameter_value().string_value
+
+        self.tf_broadcaster = TransformBroadcaster(self)
+
+        self.subscription = self.create_subscription(
+            TransformStamped,
+            'transform',
+            self.callback,
+            1)
+        self.subscription  # prevent unused variable warning
+
+    def callback(self, t):
+        if not t.header.frame_id:
+            t.header.frame_id = self.frame_id
+        if not t.child_frame_id:
+            t.child_frame_id = self.child_frame_id
+
+        self.tf_broadcaster.sendTransform(t)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    transform_to_tf = TransformToTf()
+    rclpy.spin(transform_to_tf)
+    transform_to_tf.destroy_node()
+    rclpy.shutdown()
 
 if __name__ == "__main__":
-
-    rospy.init_node("transform_to_tf", anonymous=True)
-
-    frame_id = rospy.get_param('~frame_id', 'world')
-    child_frame_id = rospy.get_param('~child_frame_id', 'transform')
-
-    br = tf.TransformBroadcaster()
-    rospy.Subscriber("transform", TransformStamped, callback, queue_size=1)
-    rospy.spin()
+    main()
+   

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