#!/usr/bin/env python3
"""
Simple relay node: /odom_wheels -> /odom
Provides standard /odom topic for Nav2 from wheel odometry source.
No external topic_tools package dependency required.
"""
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.qos import qos_profile_sensor_data


class OdomRelayNode(Node):
    def __init__(self):
        super().__init__('odom_relay')
        self.pub = self.create_publisher(Odometry, '/odom', 10)
        self.sub = self.create_subscription(
            Odometry, '/odom_wheels', self.callback, qos_profile_sensor_data
        )
        self.get_logger().info('OdomRelay: /odom_wheels -> /odom')

    def callback(self, msg):
        # Ensure frame_id is 'odom' for Nav2 compatibility
        msg.header.frame_id = 'odom'
        msg.child_frame_id = 'base_link'
        self.pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = OdomRelayNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
