"""
slam.launch.py — Full SLAM stack (sensors + slam_toolbox)

Entry point referenced by docker-compose.yml saltybot-ros2 service.

Stack:
  sensors.launch.py  (RPLIDAR + RealSense + static TF)
  slam_toolbox       online_async — 2D LIDAR SLAM from /scan

SLAM input:
  /scan              LaserScan from RPLIDAR A1M8
  /tf                base_link → laser (from sensors.launch.py)

SLAM output:
  /map               OccupancyGrid  (2D occupancy map)
  /slam_toolbox/...  Internal slam_toolbox topics

Note: slam_toolbox uses LIDAR-only SLAM (no RGB-D from D435i at this stage).
For RGB-D SLAM (RTAB-Map), use a separate launch file once slam_toolbox
mapping is validated — see SLAM-SETUP-PLAN.md (bd-wax) for full plan.

Config: /config/slam_toolbox_params.yaml (mounted from jetson/config/)

Verify:
  ros2 topic hz /map          # expect update every ~5s (map_update_interval)
  ros2 run rviz2 rviz2        # visualize map + scan
"""

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


SLAM_PARAMS_FILE = '/config/slam_toolbox_params.yaml'


def generate_launch_description():
    bringup_share = get_package_share_directory('saltybot_bringup')
    slam_share = get_package_share_directory('slam_toolbox')
    mapping_share = get_package_share_directory('saltybot_mapping')

    sensors_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_share, 'launch', 'sensors.launch.py')
        ),
    )

    slam_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(slam_share, 'launch', 'online_async_launch.py')
        ),
        launch_arguments={
            'params_file':  SLAM_PARAMS_FILE,
            'use_sim_time': 'false',
        }.items(),
    )

    # Map persistence service for SLAM Toolbox
    persistence_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(mapping_share, 'launch', 'slam_toolbox_persistence.launch.py')
        ),
    )

    return LaunchDescription([
        sensors_launch,
        slam_launch,
        persistence_launch,
    ])