# ============================================================================
# gc_nav2_with_slam_online_real.launch.py
# 功能：真机实时建图 — slam_toolbox 从零建图 + Nav2 导航
# 依赖：需先启动 origincar_bringup（底盘驱动 + EKF）
# ============================================================================

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    """真机实时建图：slam_toolbox 从零建图 + Nav2 导航"""
    ld = LaunchDescription()

    # === 1. 包路径 ===
    pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')

    # === 2. 参数 ===
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        pkg_dir, 'params', 'gc_navigation_slam.yaml'))
    slam_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_mapping.yaml')

    # === 3. slam_toolbox：实时建图 → 发布 map→odom ===
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[slam_params_file,
                     {'use_sim_time': use_sim_time}],
    )

    # === 4. Nav2 导航栈 ===
    navigation_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'params_file': nav2_param_path,
        }.items(),
    )

    # === 5. TF 树由底盘启动文件（origincar_base）提供，此处不再重复发布 ===

    # === 6. 组装 ===
    ld.add_action(slam_toolbox_node)
    ld.add_action(navigation_launch)

    return ld
