"""
sensors.launch.py — All sensor drivers + static TF

Brings up both sensors together with placeholder static transforms.
Use this to verify sensor data before running full SLAM stack.

Static TF (placeholder — update with real measurements after physical mount):
  base_link → laser         x=0.00  y=0.00  z=0.10  (RPLIDAR: 10cm above base)
  base_link → camera_link   x=0.05  y=0.00  z=0.15  (D435i: 5cm fwd, 15cm up)

Published topics summary:
  /scan                                LaserScan    ~5.5 Hz   (RPLIDAR)
  /camera/color/image_raw              Image        15 Hz     (D435i RGB)
  /camera/depth/image_rect_raw         Image        15 Hz     (D435i depth)
  /camera/aligned_depth_to_color/image_raw  Image  15 Hz     (D435i aligned)
  /camera/imu                          Imu          ~200 Hz   (D435i IMU)

Quick verification:
  ros2 topic list
  ros2 topic hz /scan                         # expect ~5.5 Hz
  ros2 topic hz /camera/color/image_raw       # expect 15 Hz
  ros2 topic hz /camera/imu                   # expect ~200 Hz
  ros2 run tf2_tools view_frames              # check TF tree
"""

import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    bringup_share = get_package_share_directory('saltybot_bringup')

    realsense_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_share, 'launch', 'realsense.launch.py')
        ),
    )

    rplidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_share, 'launch', 'rplidar.launch.py')
        ),
    )

    # Static TF: base_link → laser (RPLIDAR A1M8 mount position)
    # TODO: update x/y/z/yaw with real measurements from robot chassis
    laser_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='base_link_to_laser',
        arguments=[
            '--x',   '0.00',
            '--y',   '0.00',
            '--z',   '0.10',   # 10cm above base_link
            '--roll',  '0',
            '--pitch', '0',
            '--yaw',   '0',
            '--frame-id',       'base_link',
            '--child-frame-id', 'laser',
        ],
        output='screen',
    )

    # Static TF: base_link → camera_link (RealSense D435i mount position)
    # TODO: update x/y/z with real measurements from robot chassis
    camera_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='base_link_to_camera',
        arguments=[
            '--x',   '0.05',   # 5cm forward of base_link center
            '--y',   '0.00',
            '--z',   '0.15',   # 15cm above base_link
            '--roll',  '0',
            '--pitch', '0',
            '--yaw',   '0',
            '--frame-id',       'base_link',
            '--child-frame-id', 'camera_link',
        ],
        output='screen',
    )

    return LaunchDescription([
        realsense_launch,
        rplidar_launch,
        laser_tf,
        camera_tf,
    ])