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

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

src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam.launch.py

@@ -1,3 +1,22 @@
+# ============================================================================
+# 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
@@ -11,27 +30,50 @@ 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')
-    # slam_toolbox 定位所需的序列化地图
+
+    # 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）=========================================
-    # 注意：slam_toolbox 的 /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
+    # 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',
@@ -44,6 +86,11 @@ def generate_launch_description():
     )
 
     # =============================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',
@@ -53,6 +100,8 @@ def generate_launch_description():
                      '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',
@@ -64,7 +113,14 @@ def generate_launch_description():
     )
 
     # =============================5.仅启动 nav2 navigation（不含 localization）=================================
-    # 因为 slam_toolbox 已经替代了 AMCL，不需要再跑 localization_launch
+    # 为什么用 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']),
@@ -73,19 +129,34 @@ def generate_launch_description():
             'params_file': nav2_param_path}.items(),
     )
 
-    # =============================5.Gazebo仿真设置==========================================================
+    # =============================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',
@@ -94,9 +165,14 @@ def generate_launch_description():
         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",
@@ -104,13 +180,17 @@ def generate_launch_description():
                      'use_sim_time': True, 'publish_frequency': 30.0}]
     )
 
-    ld.add_action(model)
-    ld.add_action(start_gazebo_cmd)
-    ld.add_action(spawn_entity_cmd)
-    ld.add_action(robot_state_publisher)
-    ld.add_action(map_server_node)
-    ld.add_action(map_server_lifecycle)
-    ld.add_action(slam_toolbox_node)
-    ld.add_action(navigation_launch)
+    # =============================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
+    
+

src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py

@@ -0,0 +1,158 @@
+# ============================================================================
+# gc_nav2_with_slam_online.launch.py
+# 功能：基于 slam_toolbox 在线异步建图 + 定位的导航启动文件
+#
+# 核心能力：
+#   1. 启动时加载已有地图（my_map.posegraph），无需从零建图
+#   2. 运行中持续更新地图（新障碍物自动写入 /map）
+#   3. 激光扫描匹配定位（替代 AMCL，自动定位）
+#   4. 双层避障：obstacle_layer（/scan 瞬时避障）+ static_layer（/map 持久化障碍）
+#
+# 架构：
+#   Gazebo 仿真环境
+#   ├── robot_state_publisher  — URDF → TF 树（base_link→laser_link 等）
+#   ├── slam_toolbox           — async_slam_toolbox_node
+#   │                            ├── 加载 my_map.posegraph + my_map.data
+#   │                            ├── mode: mapping（定位 + 实时建图）
+#   │                            ├── 发布 /map（全量 + 实时更新）
+#   │                            └── 发布 map→odom TF（定位结果）
+#   └── navigation_launch      — Nav2 导航栈
+#                                ├── global_costmap  ← /map（静态层）
+#                                ├── local_costmap   ← /scan（障碍层）
+#                                ├── planner_server  — SmacHybrid 路径规划
+#                                ├── controller_server — MPPI Ackermann 控制
+#                                ├── behavior_server — spin/backup/wait 恢复
+#                                └── bt_navigator    — 行为树编排
+#
+# 与其他方案对比：
+#   gc_nav2_with_amcl.launch.py:        AMCL 粒子滤波，地图固定，需手动设初始位姿
+#   gc_nav2_with_slam.launch.py:        SLAM 定位模式，地图只读，需 map_server
+#   gc_nav2_with_slam_online.launch.py: 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():
+    """启动 Gazebo + SLAM 在线建图定位 + Nav2 导航"""
+    ld = LaunchDescription()
+
+    # =============================1.定位到包的地址=============================================================
+    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: 必须为 true，Gazebo 通过 /clock 话题提供仿真时间
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+
+    # nav2_param_path: Nav2 导航栈参数（planner/controller/costmap/behavior）
+    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(
+        pkg_dir, 'params', 'gc_navigation_slam.yaml'))
+
+    # slam 基础配置文件（异步建图 + 实时更新）
+    slam_params_file = os.path.join(
+        pkg_dir, 'config', 'slam_toolbox_async.yaml')
+
+    # 已有序列化地图（slam_toolbox 格式：.posegraph + .data）
+    posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph')
+
+    # =============================3.slam_toolbox：在线异步建图 + 定位============================================
+    # 选用 online_async_slam_toolbox_node（而非 localization_slam_toolbox_node）的原因：
+    #   - 加载已有 .posegraph 地图后，仍可继续写入新扫描数据
+    #   - 异步处理：激光数据在独立线程中建图，不阻塞里程计
+    #   - 适合导航场景：启动时还原已知区域，运行中发现新障碍物自动更新 /map
+    #
+    # 参数说明（以下参数已在 mapper_params_online_async.yaml 中配置，此处覆写确保生效）：
+    #   mode: mapping        — 定位 + 建图（覆盖 yaml）
+    #   map_file_name        — 已在 yaml 中设为源码路径，不在此覆写
+
+
+    # 已有序列化地图（slam_toolbox 格式：.posegraph + .data）
+    # 注意：此路径仅供文档参考，实际加载路径在 yaml 配置文件中
+    posegraph_path = os.path.join(pkg_dir, 'maps', 'my_map.posegraph')
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[
+            slam_params_file,  # 基础参数（solver、TF、分辨率、map_file_name 等）
+            {
+                'use_sim_time': True,
+                'mode': 'mapping',               # 覆盖 yaml：定位 + 建图
+                'map_update_interval': 3.0,      # 每 3 秒更新一次 /map
+            },
+        ],
+        # 无需 remap /map：online_async 发布的就是权威 /map
+    )
+#  Read: Failed to open requested file: / home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph.
+# Failed to read file: / home/guoch/test_ws/src/gc_navigation2_slamtoolbox/maps/my_map.posegraph.
+    # =============================4.启动 Nav2 导航栈==========================================================
+    # 只用 navigation_launch（不含 localization），因为 slam_toolbox 已承担定位
+    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(),
+    )
+
+    # =============================5.Gazebo 仿真环境==========================================================
+    robot_name_in_model = 'mycar'
+    default_model_path = os.path.join(origincar_urdf_dir, 'urdf', 'origincar.urdf')
+    model = DeclareLaunchArgument(name='model', default_value=default_model_path)
+
+    # 启动 Gazebo 仿真器（加载 world 文件和 ROS 插件）
+    gazebo_world_path = os.path.join(origincar_urdf_dir, 'world', 'test.world')
+    start_gazebo_cmd = ExecuteProcess(
+        cmd=[
+            'gazebo', '--verbose',
+            '-s', 'libgazebo_ros_init.so',      # ROS ↔ Gazebo 通信初始化
+            '-s', 'libgazebo_ros_factory.so',   # 模型生成工厂
+            gazebo_world_path,
+        ],
+        output='screen',
+    )
+
+    # 在 Gazebo 中生成机器人
+    spawn_entity_cmd = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=['-entity', robot_name_in_model, '-file', default_model_path],
+        output='screen',
+    )
+
+    # 发布机器人 TF 树
+    robot_description = ParameterValue(
+        Command(['xacro ', LaunchConfiguration('model')]), value_type=str,
+    )
+    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,
+        }],
+    )
+
+    # =============================6.组装 LaunchDescription===================================================
+    ld.add_action(model)
+    ld.add_action(start_gazebo_cmd)
+    ld.add_action(spawn_entity_cmd)
+    ld.add_action(robot_state_publisher)
+    ld.add_action(slam_toolbox_node)
+    ld.add_action(navigation_launch)
+
+    return ld