修改了bttree和规划逻辑，每秒执行一次规划

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>

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"/>
-            <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"/>
@@ -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>

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