"""
nav2.launch.py — Nav2 autonomous navigation stack for SaltyBot

Starts the full Nav2 navigation stack (controllers, planners, behavior server,
BT navigator, velocity smoother, lifecycle managers).

Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running):
  /map             — OccupancyGrid (static global costmap layer)
  /rtabmap/odom    — Odometry (velocity smoother + controller feedback)
  TF map→odom      — published by RTAB-Map

Output:
  /cmd_vel         — consumed by saltybot_bridge → STM32 over UART

Run sequence on Orin:
  1. docker compose up saltybot-ros2   # RTAB-Map + sensors
  2. docker compose up saltybot-nav2   # this launch file
"""

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


def generate_launch_description():
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    bringup_dir = get_package_share_directory('saltybot_bringup')

    nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml')
    bt_xml_file = os.path.join(
        bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml'
    )

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav2_bringup_dir, 'launch', 'navigation_launch.py')
        ),
        launch_arguments={
            'use_sim_time': 'false',
            'autostart': 'true',
            'params_file': nav2_params_file,
            'default_bt_xml_filename': bt_xml_file,
            # RTAB-Map publishes /map with transient_local QoS — must match
            'map_subscribe_transient_local': 'true',
        }.items(),
    )

    return LaunchDescription([nav2_launch])