From 713d7701a6afe9dd0d65b77fb080b96cc1714ccf Mon Sep 17 00:00:00 2001
From: chyoo <chyoo@futuredrive.net>
Date: Wed, 29 Jul 2026 15:42:37 +0900
Subject: [PATCH] fix: bridge mkmini odometry into tf for 3d slam

---
 .../launch/perception.launch.py               |  7 ++++
 .../perception3d_pkg/odom_tf.py               | 40 +++++++++++++++++++
 src/perception3d_pkg/setup.py                 |  3 +-
 3 files changed, 49 insertions(+), 1 deletion(-)
 create mode 100644 src/perception3d_pkg/perception3d_pkg/odom_tf.py

diff --git a/src/perception3d_pkg/launch/perception.launch.py b/src/perception3d_pkg/launch/perception.launch.py
index 4420be3..e9e4067 100755
--- a/src/perception3d_pkg/launch/perception.launch.py
+++ b/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',
diff --git a/src/perception3d_pkg/perception3d_pkg/odom_tf.py b/src/perception3d_pkg/perception3d_pkg/odom_tf.py
new file mode 100644
index 0000000..d2f7b69
--- /dev/null
+++ b/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()
+
diff --git a/src/perception3d_pkg/setup.py b/src/perception3d_pkg/setup.py
index 02e8e8e..0917fed 100755
--- a/src/perception3d_pkg/setup.py
+++ b/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',
         ],
     },
 )