#!/usr/bin/env python3
"""
Relay camera topics to match the intel_realsense_r200 naming convention
expected by vlm_detection node.

Gz-sim + ros_gz_bridge publishes:
  /camera/color/image_raw   -> remap to /intel_realsense_r200_rgb/image_raw
  /camera/depth/image_raw   -> remap to /intel_realsense_r200_depth/depth/image_raw
  /camera/depth/camera_info -> remap to /intel_realsense_r200_depth/camera_info
  (with frame_id override to match TF tree)

Note: Depth image is now available via L16 workaround for Humble
(gz.msgs.Image type on /depth_image topic).
"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo


class CameraTopicRemap(Node):
    def __init__(self):
        super().__init__('camera_topic_remap')

        # --- RGB Image relay ---
        self.rgb_pub = self.create_publisher(Image, '/intel_realsense_r200_rgb/image_raw', 10)
        self.rgb_sub = self.create_subscription(
            Image, '/camera/color/image_raw', self.rgb_callback, 10
        )
        self.get_logger().info(
            'Relaying [/camera/color/image_raw] -> [/intel_realsense_r200_rgb/image_raw]'
        )

        # --- Depth Image relay ---
        self.depth_pub = self.create_publisher(
            Image, '/intel_realsense_r200_depth/depth/image_raw', 10
        )
        self.depth_sub = self.create_subscription(
            Image, '/camera/depth/image_raw', self.depth_callback, 10
        )
        self.get_logger().info(
            'Relaying [/camera/depth/image_raw] -> '
            '[/intel_realsense_r200_depth/depth/image_raw]'
        )

        # --- Depth CameraInfo relay (with frame_id fix) ---
        self.depth_info_pub = self.create_publisher(
            CameraInfo, '/intel_realsense_r200_depth/camera_info', 10
        )
        self.depth_info_sub = self.create_subscription(
            CameraInfo, '/camera/depth/camera_info', self.depth_info_callback, 10
        )
        self.get_logger().info(
            'Relaying [/camera/depth/camera_info] -> '
            '[/intel_realsense_r200_depth/camera_info] (frame_id overridden)'
        )

    def rgb_callback(self, msg: Image):
        """Forward RGB image as-is."""
        self.rgb_pub.publish(msg)

    def depth_callback(self, msg: Image):
        """Forward depth image as-is."""
        self.depth_pub.publish(msg)

    def depth_info_callback(self, msg: CameraInfo):
        """Forward depth CameraInfo with corrected frame_id."""
        msg.header.frame_id = 'realsense_depth_frame'
        self.depth_info_pub.publish(msg)


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


if __name__ == '__main__':
    main()
