add：在基础地图上实现了实时避障以及实现了比较灵活的后撤，从而实现掉头

src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml

@@ -0,0 +1,36 @@
+<!--
+  阿克曼底盘自定义行为树：移除了 Spin（原地旋转），
+  恢复行为改为：清除代价地图 → 后退 → 等待
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <!-- 1. 清除代价地图，去掉短暂障碍 -->
+          <Sequence name="ClearingActions">
+            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </Sequence>
+          <!-- 2. 后退 0.5m，给重新规划留空间（阿克曼无法原地旋转） -->
+          <BackUp backup_dist="0.50" backup_speed="0.05"/>
+          <!-- 3. 等待 1s，让动态障碍物离开 -->
+          <Wait wait_duration="1"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online.launch.py

@@ -1,33 +1,25 @@
 # ============================================================================
 # gc_nav2_with_slam_online.launch.py
-# 功能：基于 slam_toolbox 在线异步建图 + 定位的导航启动文件
+# 功能：已知全局地图 + SLAM 在线更新 + 导航
-#
-# 核心能力：
-#   1. 启动时加载已有地图（my_map.posegraph），无需从零建图
-#   2. 运行中持续更新地图（新障碍物自动写入 /map）
-#   3. 激光扫描匹配定位（替代 AMCL，自动定位）
-#   4. 双层避障：obstacle_layer（/scan 瞬时避障）+ static_layer（/map 持久化障碍）
 #
 # 架构：
-#   Gazebo 仿真环境
+#                        ┌─ /map（全量静态地图，199×266）
-#   ├── robot_state_publisher  — URDF → TF 树（base_link→laser_link 等）
+#   map_server ──────────┤
-#   ├── slam_toolbox           — async_slam_toolbox_node
+#                        └─ 来自 my_map.yaml + my_map.pgm
-#   │                            ├── 加载 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    — 行为树编排
 #
-# 与其他方案对比：
+#                        ┌─ map→odom TF（激光扫描匹配定位）
-#   gc_nav2_with_amcl.launch.py:        AMCL 粒子滤波，地图固定，需手动设初始位姿
+#   slam_toolbox ────────┤
-#   gc_nav2_with_slam.launch.py:        SLAM 定位模式，地图只读，需 map_server
+#   (async mapping)      └─ /slam_map（实时建图结果，重映射避免冲突）
-#   gc_nav2_with_slam_online.launch.py: SLAM 在线模式，加载已有地图 + 实时更新（本文件）
+#
+#   navigation           ┌─ global_costmap/static_layer ← /map（全量地图）
+#                        ├─ local_costmap/obstacle_layer ← /scan（实时避障）
+#                        └─ planner + controller + behavior
+#
+# 关键设计：
+#   1. map_server 提供全量静态底图 → Foxglove 中看到完整房间布局
+#   2. slam_toolbox 定位 + 实时建图，/slam_map 可单独查看更新效果
+#   3. 实时避障通过 costmap 的 obstacle_layer 实现（不依赖地图更新）
+#   4. 定期调用 /slam_toolbox/serialize_map 保存更新后的地图
 # ============================================================================
 
 import os
@@ -43,62 +35,66 @@ from launch.actions import ExecuteProcess
 
 
 def generate_launch_description():
-    """启动 Gazebo + SLAM 在线建图定位 + Nav2 导航"""
+    """启动 Gazebo + 已知全局地图 + SLAM 在线定位建图 + Nav2 导航"""
     ld = LaunchDescription()
 
-    # =============================1.定位到包的地址=============================================================
+    # =============================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.声明路径参数===============================================================
+    # =============================2.参数==================================================================
-    # use_sim_time: 必须为 true，Gazebo 通过 /clock 话题提供仿真时间
     use_sim_time = LaunchConfiguration('use_sim_time', default='true')
-
+    map_yaml_path = os.path.join(pkg_dir, 'maps', 'my_map.yaml')
-    # 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_params_file = os.path.join(pkg_dir, 'config', 'slam_toolbox_async.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：在线异步建图 + 定位============================================
+    # =============================3.map_server：全量静态地图（Foxglove 看到的全局地图）========================
-    # 选用 online_async_slam_toolbox_node（而非 localization_slam_toolbox_node）的原因：
+    map_server_node = Node(
-    #   - 加载已有 .posegraph 地图后，仍可继续写入新扫描数据
+        package='nav2_map_server',
-    #   - 异步处理：激光数据在独立线程中建图，不阻塞里程计
+        executable='map_server',
-    #   - 适合导航场景：启动时还原已知区域，运行中发现新障碍物自动更新 /map
+        name='map_server',
-    #
+        output='screen',
-    # 参数说明（以下参数已在 mapper_params_online_async.yaml 中配置，此处覆写确保生效）：
+        parameters=[{'yaml_filename': map_yaml_path, 'use_sim_time': True}],
-    #   mode: mapping        — 定位 + 建图（覆盖 yaml）
+    )
-    #   map_file_name        — 已在 yaml 中设为源码路径，不在此覆写
+    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']}],
+    )
 
-
+    # =============================4.slam_toolbox：定位 + 实时建图=============================================
-    # 已有序列化地图（slam_toolbox 格式：.posegraph + .data）
+    # ⚠️ /map 重映射到 /slam_map，避免覆盖 map_server 的全量地图
-    # 注意：此路径仅供文档参考，实际加载路径在 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=[
+        parameters=[slam_params_file, {'use_sim_time': True}],
-            slam_params_file,  # 基础参数（solver、TF、分辨率、map_file_name 等）
+        remappings=[('/map', '/slam_map')],
-            {
-                '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.
+    # =============================5.加载已有序列化地图=======================================================
-    # =============================4.启动 Nav2 导航栈==========================================================
+    # 通过 ros2 service call 在 slam_toolbox 启动后加载
-    # 只用 navigation_launch（不含 localization），因为 slam_toolbox 已承担定位
+    import launch
+    load_map_cmd = ExecuteProcess(
+        cmd=[
+            'bash', '-c',
+            # 等待 slam_toolbox 服务就绪后加载地图
+            'sleep 8 && '
+            'ros2 service call /slam_toolbox/deserialize_map '
+            'slam_toolbox/srv/DeserializePoseGraph '
+            "'{filename: \"%s\", match_type: 1}'" % posegraph_path,
+        ],
+        output='screen',
+    )
+
+    # =============================6.Nav2 导航栈============================================================
     navigation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [nav2_bringup_dir, '/launch', '/navigation_launch.py']),
@@ -108,51 +104,43 @@ def generate_launch_description():
         }.items(),
     )
 
-    # =============================5.Gazebo 仿真环境==========================================================
+    # =============================7.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=[
+        cmd=['gazebo', '--verbose',
-            'gazebo', '--verbose',
+             '-s', 'libgazebo_ros_init.so',
-            '-s', 'libgazebo_ros_init.so',      # ROS ↔ Gazebo 通信初始化
+             '-s', 'libgazebo_ros_factory.so',
-            '-s', 'libgazebo_ros_factory.so',   # 模型生成工厂
+             gazebo_world_path],
-            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=[{
+        parameters=[{'robot_description': robot_description,
-            'robot_description': robot_description,
+                     'use_sim_time': True, 'publish_frequency': 30.0}],
-            'use_sim_time': True,
-            'publish_frequency': 30.0,
-        }],
     )
 
-    # =============================6.组装 LaunchDescription===================================================
+    # =============================8.组装==================================================================
     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(load_map_cmd)
     ld.add_action(navigation_launch)
 
     return ld

src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml

@@ -10,6 +10,8 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 20
     default_server_timeout: 20
+    # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
+    default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
     plugin_lib_names:
       - nav2_compute_path_to_pose_action_bt_node
       - nav2_compute_path_through_poses_action_bt_node
@@ -269,7 +271,7 @@ planner_server:
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 3.0
       minimum_turning_radius: 0.40
-      reverse_penalty: 2.0
+      reverse_penalty: 1.3     # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
       change_penalty: 0.0
       non_straight_penalty: 1.2
       cost_penalty: 2.0
@@ -308,11 +310,13 @@ behavior_server:
     cycle_frequency: 10.0
     behavior_plugins: ["spin", "backup", "wait"]
     spin:
-      plugin: "nav2_behaviors/Spin"
+      plugin: "nav2_behaviors/Spin"    # 需保留以匹配默认 BT XML
     backup:
       plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.5
     wait:
       plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
     global_frame: odom
     robot_base_frame: base_link
     transform_tolerance: 0.5