# ============================================================================
# gc_nav2_with_slam.launch.py
# 功能：基于 slam_toolbox 定位的导航启动文件
#
# 架构：
#   Gazebo 仿真环境
#   ├── robot_state_publisher  — 发布机器人 TF 树（URDF 模型）
#   ├── map_server             — 加载 my_map.yaml，发布全量静态地图到 /map
#   ├── slam_toolbox           — 加载 my_map.posegraph，激光扫描匹配定位
#   │                            发布 map→odom TF，替代 AMCL 的粒子滤波
#   │                            ⚠️ /map 话题重映射为 /slam_map，避免冲突
#   └── navigation_launch      — Nav2 导航栈（路径规划 + 运动控制）
#                                global_costmap 从 /map 获取全量静态地图
#
# 与 AMCL 方案的区别：
#   AMCL：      粒子滤波定位，需要手动设置初始位姿
#   SLAM定位：  激光扫描匹配定位，自动根据地图特征定位
# ============================================================================

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_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command
from launch.actions import ExecuteProcess


def generate_launch_description():
    """生成 LaunchDescription，启动 Gazebo + SLAM定位 + Nav2 导航"""
    ld = LaunchDescription()

    # =============================1.定位到包的地址=============================================================
    # 通过 ament_index 获取各功能包的安装路径
    pkg_dir = get_package_share_directory('gc_navigation2_slamtoolbox')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    origincar_urdf_dir = get_package_share_directory('origincar_description')

    # =============================2.声明参数===============================================================
    # use_sim_time: 仿真时间开关。Gazebo 下必须为 True，
    #               因为仿真环境通过 /clock 话题提供时间，而非系统时间
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    # map_yaml_path: 全量静态地图 yaml 文件路径
    #               由 map_server 加载，发布到 /map 供 global_costmap 使用
    map_yaml_path = LaunchConfiguration('map', default=os.path.join(
        pkg_dir, 'maps', 'my_map.yaml'))

    # nav2_param_path: Nav2 导航栈参数文件（不含 AMCL，含 slam_toolbox）
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        pkg_dir, 'params', 'gc_navigation_slam.yaml'))

    # slam_params_file: slam_toolbox 定位模式的参数文件
    #   mode: localization — 只定位不建图
    #   包含 solver 配置、激光匹配参数、TF 帧定义等
    slam_params_file = os.path.join(
        pkg_dir, 'config', 'mapper_params_localization.yaml')

    # posegraph_path: slam_toolbox 定位所需的序列化地图文件
    #   .posegraph 文件保存了 slam_toolbox 建图时的位姿图
    #   配合 .data 文件，slam_toolbox 可以还原完整地图用于激光匹配
    posegraph_path = os.path.join(
        pkg_dir, 'maps', 'my_map.posegraph')

    # =============================3.slam_toolbox 定位模式（替代 AMCL）=========================================
    # localization_slam_toolbox_node:
    #   - 加载 .posegraph 序列化地图用于激光扫描匹配
    #   - 发布 map→odom 坐标变换（替代 AMCL 的粒子滤波定位）
    #   - 发布 /pose 话题（当前定位位姿）
    #
    # ⚠️ 关键：将 slam_toolbox 的 /map 重映射为 /slam_map
    #    避免覆盖 map_server 发布到 /map 的全量静态地图
    #    确保 global_costmap 的 static_layer 能获取完整地图边界
    slam_toolbox_node = Node(
        package='slam_toolbox',
        executable='localization_slam_toolbox_node',
        name='slam_toolbox',
        output='screen',
        parameters=[slam_params_file,
                    {'use_sim_time': True,
                     'map_file_name': posegraph_path}],
        remappings=[('/map', '/slam_map')],
    )

    # =============================4.map_server 提供全量静态地图给 global_costmap =============================
    # map_server:
    #   - 加载 my_map.yaml + my_map.pgm 格式的传统静态地图
    #   - 发布完整地图到 /map 话题（199×266 像素，分辨率 0.05 m）
    #   - 这是 global_costmap 静态层的唯一数据来源
    #   - 是 lifecycle 节点，需要 lifecycle_manager 激活
    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': True}],
    )

    # lifecycle_manager: 管理 map_server 的生命周期（configure → activate）
    #   autostart=True 表示启动后自动激活 map_server
    map_server_lifecycle = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_map',
        output='screen',
        parameters=[{'use_sim_time': True,
                     'autostart': True,
                     'node_names': ['map_server']}],
    )

    # =============================5.仅启动 nav2 navigation（不含 localization）=================================
    # 为什么用 navigation_launch 而不是 bringup_launch？
    #   bringup_launch 包含 localization_launch（AMCL + map_server）
    #   由于 slam_toolbox 已经替代了 AMCL，只需 navigation_launch 的：
    #     - planner_server   — 全局/局部路径规划
    #     - controller_server — 路径跟踪控制（MPPI）
    #     - behavior_server  — 行为树（spin/backup/wait 恢复）
    #     - bt_navigator     — 行为树导航编排
    #     - costmap 层       — global_costmap + local_costmap
    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(),
    )

    # =============================6.Gazebo仿真设置==========================================================
    # robot_name_in_model: Gazebo 中机器人的模型名称，需与 URDF 中一致
    robot_name_in_model = 'mycar'

    # default_model_path: 机器人 URDF 模型文件路径
    #   使用 xacro 预处理生成最终的 URDF
    default_model_path = os.path.join(
        origincar_urdf_dir, "urdf", "origincar.urdf")

    # 声明 model 启动参数，支持命令行覆盖 URDF 路径
    model = DeclareLaunchArgument(
        name="model", default_value=default_model_path)

    # Gazebo 世界文件路径
    gazebo_world_path = os.path.join(origincar_urdf_dir, 'world/test.world')

    # 启动 Gazebo 仿真器
    #   --verbose: 详细日志输出
    #   -s libgazebo_ros_init.so: 加载 ROS-Gazebo 初始化插件
    #   -s libgazebo_ros_factory.so: 加载模型生成插件
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
             '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
        output='screen')

    # 在 Gazebo 中生成机器人模型
    #   -entity: 模型实例名称
    #   -file: URDF 模型文件
    spawn_entity_cmd = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,
                   '-file', default_model_path],
        output='screen'
    )

    # robot_description: 将 URDF/XACRO 模型转换为 ROS 参数
    #   robot_state_publisher 订阅此参数发布 TF 树
    robot_description = ParameterValue(
        Command(["xacro ", LaunchConfiguration("model")]), value_type=str)

    # robot_state_publisher: 发布机器人各连杆的 TF 变换
    #   base_link → laser_link, wheel_link 等关节状态
    #   publish_frequency=30Hz 保证平滑的 TF 更新
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description,
                     'use_sim_time': True, 'publish_frequency': 30.0}]
    )

    # =============================7.将所有 Action 添加到 LaunchDescription ==================================
    # 启动顺序由 Launch 系统自动管理依赖
    ld.add_action(model)                     # 1. 声明模型参数
    ld.add_action(start_gazebo_cmd)          # 2. 启动 Gazebo 仿真器
    ld.add_action(spawn_entity_cmd)          # 3. 生成机器人模型
    ld.add_action(robot_state_publisher)     # 4. 发布 TF 树
    ld.add_action(map_server_node)           # 5. 加载全量静态地图 → /map
    ld.add_action(map_server_lifecycle)      # 6. 激活 map_server
    ld.add_action(slam_toolbox_node)         # 7. 启动 SLAM 定位 → map→odom TF
    ld.add_action(navigation_launch)         # 8. 启动 Nav2 导航栈

    return ld
    

