# ============================================================================
# gc_nav2_with_amcl.launch.py
# 功能：基于 AMCL（自适应蒙特卡洛定位）的导航启动文件
#
# 架构：
#   Gazebo 仿真环境
#   ├── robot_state_publisher  — 发布机器人 TF 树（URDF 模型）
#   └── nav2_bringup_launch    — Nav2 一体化启动（包含以下子模块）：
#         ├── map_server       — 加载 my_map.yaml,发布全量静态地图到 /map
#         ├── AMCL             — 粒子滤波定位,发布 map→odom TF
#         │                      需要手动设置 /initialpose 初始位姿
#         ├── planner_server   — 全局/局部路径规划器（SmacHybrid）
#         ├── controller_server — 路径跟踪控制器（MPPI Ackermann）
#         ├── behavior_server  — 恢复行为服务器（spin/backup/wait）
#         └── bt_navigator     — 行为树导航编排器
#
# 与 SLAM 定位方案的区别：
#   AMCL：      粒子滤波定位,需要手动设置初始位姿,地图固定不更新
#   SLAM定位：  激光扫描匹配定位,自动定位,可加载 .posegraph 序列化地图
# ============================================================================

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction
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 + AMCL定位 + Nav2 导航"""
    ld = LaunchDescription()

    # =============================1.定位到包的地址=============================================================
    # 通过 ament_index 获取各功能包的安装路径
    gc_navigation_fish_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 文件路径
    #               传给 bringup_launch → map_server 加载并发布到 /map
    map_yaml_path = LaunchConfiguration('map', default=os.path.join(
        gc_navigation_fish_dir, 'maps', 'my_map.yaml'))

    # nav2_param_path: Nav2 导航栈参数文件（含 AMCL 粒子滤波配置）
    #   注意：此处用 gc_navigation_amcl.yaml,包含 AMCL 参数（粒子数、运动模型等）
    #         SLAM 方案则用 gc_navigation_slam.yaml（不含 AMCL）
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
        gc_navigation_fish_dir, 'params', 'gc_navigation_amcl.yaml'))

    # =============================3.声明启动launch文件==========================================================
    # bringup_launch.py 是 Nav2 的一体化启动入口,内部自动处理：
    #   1. 声明所有 launch 参数（map, slam, use_sim_time, params_file 等）
    #   2. 根据 slam 参数决定启动模式：
    #      slam=False（默认）→ localization_launch（map_server + AMCL）
    #      slam=True         → slam_launch（slam_toolbox 在线建图）
    #   3. 启动 navigation_launch（planner + controller + behavior + bt_navigator）
    #   4. 通过 RewrittenYaml 实现参数文件中的变量替换
    #
    # 传入参数：
    #   map:        地图 yaml 文件路径（map_server 加载,AMCL 定位）
    #   use_sim_time: 仿真时间模式（Gazebo 下必须为 true）
    #   params_file: Nav2 全部节点的参数配置
    nav2_bringup_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
        launch_arguments={
            'map': map_yaml_path,
            'use_sim_time': use_sim_time,
            'params_file': nav2_param_path}.items(),
    )

    # =============================4.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 启动参数,支持命令行覆盖：ros2 launch ... model:=/path/to/custom.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: 加载模型生成（spawn_entity）插件
    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so',
             '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
        output='screen')

    # 在 Gazebo 世界中生成机器人模型
    #   -entity: 模型在 Gazebo 中的实例名（mycar）
    #   -file:   要加载的 URDF 模型文件
    #   生成后 Gazebo 会为模型创建对应的关节状态话题等
    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: 将 XACRO/URDF 模型字符串设为 ROS 参数
    #   robot_state_publisher 读取此参数发布各连杆的 TF 坐标变换
    robot_description = ParameterValue(
        Command(["xacro ", LaunchConfiguration("model")]), value_type=str)

    # robot_state_publisher: 发布机器人各关节、连杆的 TF 变换
    #   - 订阅 joint_states 话题获取关节角度
    #   - 根据 URDF 模型计算 base_link → laser_link, wheel_link 等变换
    #   - publish_frequency=30Hz 确保 TF 更新平滑
    #   - use_sim_time=True 使用 /clock 仿真时间
    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}]
    )

    # joint_state_publisher: 发布非固定关节的默认状态
    #   如果 Gazebo 已经发布 joint_states,此节点可以注释掉避免冲突
    # joint_state_publisher = Node(
    #     package="joint_state_publisher",
    #     executable="joint_state_publisher"
    # )

    # =============================5.将所有 Action 添加到 LaunchDescription ==================================
    # 注意：nav2_bringup_launch 必须在 Gazebo 启动之后,
    #       否则会出现 /clock 话题未就绪的问题
    #       可通过 TimerAction 添加延迟,或依赖 launch 系统的自动排序
    ld.add_action(model)                     # 1. 声明模型路径参数
    ld.add_action(nav2_bringup_launch)       # 2. 启动 Nav2 一体化（map_server + AMCL + 导航）
    ld.add_action(start_gazebo_cmd)          # 3. 启动 Gazebo 仿真器
    ld.add_action(spawn_entity_cmd)          # 4. 在 Gazebo 中生成机器人
    ld.add_action(robot_state_publisher)     # 5. 发布机器人 TF 树
    # ld.add_action(joint_state_publisher)   # 6. (可选) 默认关节状态发布

    return ld