#!/usr/bin/env python3
# ============================================================================
# real_nav2_slam_online.launch.py
# 功能：实车导航 — 已知全局地图 + SLAM 在线更新 + Nav2 导航
#
# 架构：
#                        ┌─ /map（全量静态地图）
#   map_server ──────────┤
#                        └─ 来自 my_map.yaml + my_map.pgm
#
#                        ┌─ map→odom_combined TF（激光扫描匹配定位）
#   slam_toolbox ────────┤
#   (async mapping)      └─ /slam_map（实时建图结果，重映射避免冲突）
#
#   navigation           ┌─ global_costmap/static_layer ← /map（全量地图）
#                        ├─ local_costmap/obstacle_layer ← /scan（实时避障）
#                        └─ planner + controller + behavior
#
# 关键设计：
#   1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局
#   2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果
#   3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新）
#   4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图
#
# 与仿真版的区别：
#   - 无 Gazebo，无 spawn_entity
#   - 使用 origincar_base bringup（串口驱动 + EKF + IMU）
#   - 使用 lslidar_driver（镭神实车雷达）
#   - use_sim_time: False
#   - TF 帧适配实车（odom_combined, base_footprint）
#
# 使用方式：
#   ros2 launch gc_navigation2_real real_nav2_slam_online.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
from launch.actions import ExecuteProcess


def generate_launch_description():
    """启动实车底盘 + 雷达 + 静态地图 + 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 = 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_async_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：定位 + 实时建图========================
    # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='async_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[slam_params_file, {'use_sim_time': False}],
        remappings=[('/map', '/slam_map')],
    )

    # =============================7. 加载已有序列化地图 =================================
    load_map_cmd = ExecuteProcess(
        cmd=[
            'bash', '-c',
            'sleep 8 && '
            'ros2 service call /slam_toolbox/deserialize_map '
            'slam_toolbox/srv/DeserializePoseGraph '
            "'{filename: \"%s\", match_type: 1}'" % posegraph_path,
        ],
        output='screen',
    )

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

    # =============================9. 组装============================================
    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(load_map_cmd)
    ld.add_action(navigation_launch)

    return ld
