#!/usr/bin/env python3
# ============================================================================
# real_nav2_slam.launch.py
# 功能：实车导航 — 已知静态地图 + SLAM 定位 + Nav2 导航
#
# 架构：
#   origincar_base (bringup)    — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF
#   lslidar_driver             — 镭神激光雷达驱动 → /scan
#   map_server                 — 加载 yaml+pgm 全量静态地图 → /map
#   slam_toolbox (localization) — 加载 .posegraph，激光扫描匹配定位
#                                 发布 map→odom_combined TF（替代 AMCL）
#                                 ⚠️ /map 话题重映射为 /slam_map，避免冲突
#   navigation_launch          — Nav2 导航栈
#                                 global_costmap 从 /map 获取全量静态地图
#
# 使用方式：
#   ros2 launch gc_navigation2_real real_nav2_slam.launch.py
# ============================================================================

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():
    """生成 LaunchDescription，启动实车底盘 + 雷达 + SLAM定位 + Nav2 导航"""
    ld = LaunchDescription()

    # =============================1. 包路径===========================================
    pkg_dir = get_package_share_directory('gc_navigation2_real')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    origincar_base_dir = get_package_share_directory('origincar_base')

    # =============================2. 参数=============================================
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    map_yaml_path = LaunchConfiguration('map', default=os.path.join(
        pkg_dir, 'maps', 'my_map.yaml'))
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        pkg_dir, 'params', 'gc_navigation_slam_real.yaml'))
    slam_params_file = os.path.join(
        pkg_dir, 'config', 'slam_toolbox_localization_real.yaml')
    posegraph_path = os.path.join(
        pkg_dir, 'maps', 'my_map.posegraph')

    # =============================3. 实车底盘 bringup ================================
    origincar_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
    )

    # =============================4. 镭神激光雷达驱动 ================================
    lslidar_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [get_package_share_directory('lslidar_driver'),
             '/launch', '/lsn10_launch.py']),
    )

    # =============================5. map_server：全量静态地图 ==========================
    map_server_node = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': False}],
    )
    map_server_lifecycle = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_map',
        output='screen',
        parameters=[{'use_sim_time': False, 'autostart': True,
                     'node_names': ['map_server']}],
    )

    # =============================6. slam_toolbox 定位模式（替代 AMCL）==================
    # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='localization_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[slam_params_file,
                    {'use_sim_time': False,
                     'map_file_name': posegraph_path}],
        remappings=[('/map', '/slam_map')],
    )

    # =============================7. 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(),
    )

    # =============================8. 组装=============================================
    ld.add_action(origincar_bringup)
    ld.add_action(lslidar_launch)
    ld.add_action(map_server_node)
    ld.add_action(map_server_lifecycle)
    ld.add_action(slam_toolbox_node)
    ld.add_action(navigation_launch)

    return ld
