From b9da8b0c30e1916e7fcfacbae30db4c0bbd7cb0a Mon Sep 17 00:00:00 2001
From: chyoo <chyoo@futuredrive.net>
Date: Wed, 19 Aug 2026 09:53:09 +0900
Subject: [PATCH] Tune MKMini Reeds-Shepp navigation

---
 src/planning_pkg/behavior/navigate_to_pose_no_spin.xml | 5 ++++-
 src/planning_pkg/config/planning_2d_sim_mkmini.yaml    | 8 ++++----
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml b/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml
index 13bb784..9239947 100755
--- a/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml
+++ b/src/planning_pkg/behavior/navigate_to_pose_no_spin.xml
@@ -5,7 +5,10 @@
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <PipelineSequence name="NavigateToPose">
-      <RateController hz="1.0">
+      <!-- Reeds-Shepp direction choices can flip when a car-like path is
+           recomputed every second. The static Gazebo world only needs a
+           low-rate refresh while the controller follows the current arc. -->
+      <RateController hz="0.1">
         <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
       </RateController>
       <FollowPath path="{path}" controller_id="FollowPath"/>
diff --git a/src/planning_pkg/config/planning_2d_sim_mkmini.yaml b/src/planning_pkg/config/planning_2d_sim_mkmini.yaml
index 07253d4..53a7e08 100755
--- a/src/planning_pkg/config/planning_2d_sim_mkmini.yaml
+++ b/src/planning_pkg/config/planning_2d_sim_mkmini.yaml
@@ -107,16 +107,16 @@ planner_server:
       max_iterations: 3000000
       max_on_approach_iterations: 5000
       max_planning_time: 10.0
-      # Normal navigation should remain forward-only. Reeds-Shepp creates
-      # forward/reverse cusps that Regulated Pure Pursuit can oscillate on.
-      motion_model_for_search: "DUBIN"
+      # Test car-like paths that can reverse to leave constrained spaces.
+      motion_model_for_search: "REEDS_SHEPP"
       angle_quantization_bins: 144
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 5.0
       # tricycle_drive uses a 0.72 m wheelbase with a 0.5 rad steering limit.
       # 0.72 / tan(0.5) is 1.32 m; keep a small planning margin.
       minimum_turning_radius: 1.35
-      change_penalty: 0.0
+      # Prefer a continuous arc over avoidable forward/reverse oscillation.
+      change_penalty: 1.0
       non_straight_penalty: 1.5
       cost_penalty: 5.0
       retrospective_penalty: 0.1