目前正在尝试仿真中实现基础地图上实时更新地图

2026-06-04 22:07:35 +08:00
parent 82a1d44c5f
commit 5c746829db
6 changed files with 419 additions and 245 deletions
--- a/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py
+++ b/src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_amcl.launch.py
@@ -1,3 +1,24 @@
+# ============================================================================
+# 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
@@ -11,24 +32,46 @@ 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 这里要设置成true,因为gazebo是仿真环境，其时间是通过/clock话题获取，而不是系统时间
+    # =============================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文件，传入：地图路径、是否使用仿真时间以及nav2参数文件==============
+    # =============================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']),
@@ -38,20 +81,35 @@ def generate_launch_description():
            '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',
@@ -60,9 +118,16 @@ def generate_launch_description():
        output='screen'
    )

-    robot_description = ParameterValue(Command(["xacro ", LaunchConfiguration("model")]),
-                                       value_type=str)
+    # 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",
@@ -70,17 +135,22 @@ def generate_launch_description():
                     'use_sim_time': True, 'publish_frequency': 30.0}]
    )

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

-    ld.add_action(model)
-    ld.add_action(nav2_bringup_launch)
-    ld.add_action(start_gazebo_cmd)
-    ld.add_action(spawn_entity_cmd)
-    ld.add_action(robot_state_publisher)
-    # ld.add_action(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