#!/usr/bin/env python3
# ============================================================================
# real_slam_mapping.launch.py
# 功能：实车 SLAM 建图启动文件（无 Gazebo 仿真）
#
# 适用场景：
#   - 在实际场地遥控机器人建立环境地图
#   - 在已有地图基础上增量更新
#   - 单独调试 slam_toolbox 建图参数
#
# 架构：
#   origincar_base (bringup)    — 底盘串口驱动 + EKF 里程计 + IMU 滤波 + TF
#   lslidar_driver             — 镭神激光雷达驱动 → /scan
#   async_slam_toolbox_node    — 激光 SLAM 建图（mapping 模式）
#                                 发布 map→odom_combined TF
#                                 发布 /map 话题（实时地图）
#
# 使用方式：
#   ros2 launch gc_navigation2_real real_slam_mapping.launch.py
#
# 保存地图：
#   ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
#     "{filename: '/home/guoch/test_ws/src/gc_navigation2_real/maps/my_map'}"
#   ros2 run nav2_map_server map_saver_cli -f <map_name>
# ============================================================================

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


def generate_launch_description():
    """生成 LaunchDescription，启动实车底盘 + 雷达 + slam_toolbox 建图"""

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

    # ============================ 2. 声明启动参数 ====================================
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    # slam_params_file: slam_toolbox 配置文件
    slam_params_file = LaunchConfiguration(
        'slam_params_file',
        default=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'))

    # map_file_name: 可选，加载已有序列化地图继续建图
    map_file_name = LaunchConfiguration('map_file_name', default='')

    # ============================ 3. 实车底盘 bringup ===============================
    # 启动 origincar_base：串口驱动 + EKF + IMU 滤波 + TF 发布
    # 注：origincar_bringup 已包含 robot_state_publisher + joint_state_publisher + 静态 TF
    origincar_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [origincar_base_dir, '/launch', '/origincar_bringup.launch.py']),
    )

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

    # ============================ 5. slam_toolbox 建图节点 ============================
    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,
                'map_file_name': map_file_name,
            },
        ],
    )

    # ============================ 6. 组装 LaunchDescription =========================
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='使用仿真时间（实车必须为 false）'),
        DeclareLaunchArgument(
            'slam_params_file',
            default_value=os.path.join(pkg_dir, 'config', 'slam_toolbox_async_real.yaml'),
            description='slam_toolbox 参数配置文件路径'),
        DeclareLaunchArgument(
            'map_file_name',
            default_value='',
            description='已有序列化地图路径（.posegraph），留空则从零开始建图'),

        origincar_bringup,
        lslidar_launch,
        slam_toolbox_node,
    ])
