#!/usr/bin/env python3
"""
Relay LaserScan messages while fixing header.frame_id.
Gz-sim (Harmonic) + ros_gz_bridge (Humble) may publish LaserScan
with Gazebo-scoped frame_id (e.g., 'burger/base_scan/hls_lfcd_lds')
instead of the expected 'base_scan'. This node remaps it so that
slam_toolbox / nav2 can find the correct TF.
"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan


class ScanFrameFix(Node):
    def __init__(self):
        super().__init__('scan_frame_fix')

        self.declare_parameter('input_topic', '/scan_raw')
        self.declare_parameter('output_topic', '/scan')
        self.declare_parameter('target_frame_id', 'base_scan')

        input_topic = self.get_parameter('input_topic').value
        output_topic = self.get_parameter('output_topic').value
        self.target_frame_id = self.get_parameter('target_frame_id').value

        self.pub = self.create_publisher(LaserScan, output_topic, 10)
        self.sub = self.create_subscription(
            LaserScan, input_topic, self.callback, 10
        )
        self.get_logger().info(
            f'Relaying [{input_topic}] -> [{output_topic}], '
            f'overriding frame_id -> {self.target_frame_id}'
        )

    def callback(self, msg: LaserScan):
        msg.header.frame_id = self.target_frame_id
        self.pub.publish(msg)


def main():
    rclpy.init()
    node = ScanFrameFix()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
