ported rtabmap_ros_pkg_split to ros2

rtabmap_util/scripts/transform_to_tf.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TransformStamped
+from tf2_ros import TransformBroadcaster
+
+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__":
+    main()
+