From 3afee818e80567844748b49744c21572190ae074 Mon Sep 17 00:00:00 2001
From: Orange <2314753575@qq.com>
Date: Mon, 22 Jun 2026 18:06:53 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86bttree=E5=92=8C?=
 =?UTF-8?q?=E8=A7=84=E5=88=92=E9=80=BB=E8=BE=91=EF=BC=8C=E6=AF=8F=E7=A7=92?=
 =?UTF-8?q?=E6=89=A7=E8=A1=8C=E4=B8=80=E6=AC=A1=E8=A7=84=E5=88=92?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../config/nav_through_poses_ackermann.xml    | 29 +++++++++++++++++++
 .../config/nav_to_pose_ackermann.xml          | 18 +++---------
 .../params/gc_navigation_slam.yaml            | 16 ++++------
 3 files changed, 38 insertions(+), 25 deletions(-)
 create mode 100644 src/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml

diff --git a/src/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml b/src/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml
new file mode 100644
index 0000000..30531ad
--- /dev/null
+++ b/src/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml
@@ -0,0 +1,29 @@
+<!--
+  Ackermann navigate-through-poses behavior tree.
+  Keeps periodic replanning while following. Recovery is intentionally short:
+  local controller clear on path-follow failure, then BackUp only at the top level.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <ReactiveSequence>
+            <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
+            <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
+          </ReactiveSequence>
+        </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">
+          <BackUp backup_dist="0.80" backup_speed="0.18"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>
diff --git a/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml b/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
index 5b2cc52..8746cd1 100644
--- a/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+++ b/src/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
@@ -1,16 +1,14 @@
 <!--
-  阿克曼底盘自定义行为树：移除了 Spin（原地旋转），
-  恢复行为改为：清除代价地图 → 后退 → 等待
+  Ackermann navigation behavior tree.
+  Keeps 1 Hz replanning while following. Recovery is intentionally short:
+  local controller clear on path-follow failure, then BackUp only at the top level.
 -->
 <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>
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
           <FollowPath path="{path}" controller_id="FollowPath"/>
@@ -20,15 +18,7 @@
       <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.8m，给重新规划留空间（阿克曼无法原地旋转） -->
           <BackUp backup_dist="0.80" backup_speed="0.18"/>
-          <!-- 3. 短暂等待 0.3s，让新路径规划完成 -->
-          <Wait wait_duration="0.3"/>
         </RoundRobin>
       </ReactiveFallback>
     </RecoveryNode>
diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
index 4d4e494..1b9ce2c 100644
--- a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
+++ b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
@@ -10,6 +10,7 @@ bt_navigator:
     bt_loop_duration: 50
     default_server_timeout: 20
     default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+    default_nav_through_poses_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_through_poses_ackermann.xml
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node
@@ -82,11 +83,11 @@ controller_server:
       cost_scaling_gain: 1.0
       regulated_linear_scaling_min_radius: 0.35
       regulated_linear_scaling_min_speed: 0.1
-      use_rotate_to_heading: True
+      use_rotate_to_heading: False
       rotate_to_heading_min_angle: 0.5
       max_robot_pose_search_dist: 10.0
       use_interpolation: False
-      allow_reversing: False
+      allow_reversing: True
 controller_server_rclcpp_node:
   ros__parameters:
     use_sim_time: False
@@ -110,7 +111,7 @@ local_costmap:
       - inflation_layer
       inflation_layer:
         plugin: nav2_costmap_2d::InflationLayer
-        cost_scaling_factor: 3.0
+        cost_scaling_factor: 5.0
         inflation_radius: 0.3
       voxel_layer:
         plugin: nav2_costmap_2d::VoxelLayer
@@ -177,7 +178,7 @@ global_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: nav2_costmap_2d::InflationLayer
-        cost_scaling_factor: 3.0
+        cost_scaling_factor: 5.0
         inflation_radius: 0.3
       always_send_full_costmap: True
   global_costmap_client:
@@ -247,18 +248,11 @@ behavior_server:
     footprint_topic: local_costmap/published_footprint
     cycle_frequency: 10.0
     behavior_plugins:
-    - spin
     - backup
-    - wait
-    spin:
-      plugin: nav2_behaviors/Spin
     backup:
       plugin: nav2_behaviors/BackUp
       backup_dist: 0.8
       backup_speed: 0.18
-    wait:
-      plugin: nav2_behaviors/Wait
-      wait_duration: 0.5
     global_frame: odom
     robot_base_frame: base_link
     transform_tolerance: 2.0