fix: bridge mkmini odometry into tf for 3d slam

src/perception3d_pkg/launch/perception.launch.py

@@ -60,6 +60,13 @@ def generate_launch_description():
                 arguments=['0.1', '0.0', '0.6', '0.0', '0.0', '0.0', 'base_link', 'os_lidar'],
                 condition=IfCondition(PythonExpression(["'", robot, "' == 'mkmini'"])),
             ),
+            Node(
+                package='perception3d_pkg',
+                executable='odom_tf',
+                name='odom_tf_bridge',
+                output='screen',
+                condition=IfCondition(PythonExpression(["'", robot, "' == 'mkmini'"])),
+            ),
             Node(
                 package='perception3d_pkg',
                 executable='pcd_crop',

src/perception3d_pkg/perception3d_pkg/odom_tf.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""Publish the TF representation of the platform odometry message."""
+
+import rclpy
+from geometry_msgs.msg import TransformStamped
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from tf2_ros import TransformBroadcaster
+
+
+class OdomTfBridge(Node):
+    def __init__(self):
+        super().__init__('odom_tf_bridge')
+        self.declare_parameter('odom_topic', '/odom')
+        self._tf_broadcaster = TransformBroadcaster(self)
+        self.create_subscription(Odometry, self.get_parameter('odom_topic').value, self._on_odom, 30)
+
+    def _on_odom(self, message: Odometry):
+        transform = TransformStamped()
+        transform.header = message.header
+        transform.header.frame_id = message.header.frame_id or 'odom'
+        transform.child_frame_id = message.child_frame_id or 'base_link'
+        transform.transform.translation.x = message.pose.pose.position.x
+        transform.transform.translation.y = message.pose.pose.position.y
+        transform.transform.translation.z = message.pose.pose.position.z
+        transform.transform.rotation = message.pose.pose.orientation
+        self._tf_broadcaster.sendTransform(transform)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomTfBridge()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+

src/perception3d_pkg/setup.py

@@ -30,7 +30,8 @@ setup(
     entry_points={
         'console_scripts': [
             'test = perception3d_pkg.test:main',
-            'pcd_crop = perception3d_pkg.pcd_crop:main'
+            'pcd_crop = perception3d_pkg.pcd_crop:main',
+            'odom_tf = perception3d_pkg.odom_tf:main',
         ],
     },
 )