# ============================================================================
# planner.launch.py
# Nav2 最小全局规划模块
# 启动: slam_toolbox + planner_server + lifecycle_manager
# costmap 由 planner_server 内部实例化，不需要单独启动
# 不启动: controller / behavior / bt_navigator / velocity_smoother / smoother
#
# 参考: gc_nav2_with_slam_online_real.launch.py
# ============================================================================

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    """启动 Nav2 最小规划栈：slam + planner"""
    ld = LaunchDescription()

    # === 包路径 ===
    pkg_dir = get_package_share_directory('planner')

    # === 参数 ===
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    params_file = LaunchConfiguration(
        'params_file',
        default=os.path.join(pkg_dir, 'config', 'planner.yaml'))

    declare_params = DeclareLaunchArgument(
        'params_file',
        default_value=os.path.join(pkg_dir, 'config', 'planner.yaml'),
        description='Path to the planner parameters file')
    ld.add_action(declare_params)

    # =====================================================================
    # 1. slam_toolbox — 异步建图 (online_async)，发布 /map + map→odom
    # =====================================================================
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[params_file,
                     {'use_sim_time': use_sim_time}],
    )
    ld.add_action(slam_toolbox_node)

    # =====================================================================
    # 2. planner_server — SmacPlannerHybrid，ComputePathToPose action
    #    planner_server 内部实例化 global_costmap（从 planner.yaml 读取参数）
    # =====================================================================
    planner_server_node = Node(
        package='nav2_planner',
        executable='planner_server',
        name='planner_server',
        output='screen',
        parameters=[params_file],
    )
    ld.add_action(planner_server_node)

    # =====================================================================
    # 3. lifecycle_manager — 自动激活 planner_server (lifecycle 节点)
    # =====================================================================
    lifecycle_manager_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_planner',
        output='screen',
        parameters=[{'autostart': True},
                    {'node_names': ['planner_server']}],
    )
    ld.add_action(lifecycle_manager_node)

    return ld
