From 9590900658d7b724c94633832a45ee6b4df567cd Mon Sep 17 00:00:00 2001
From: cyy <cyy@robot.local>
Date: Fri, 3 Jul 2026 08:30:17 +0000
Subject: [PATCH] Ackermann MPPI: v+kappa sampling, no reverse, progress
 critic, safety stop-only

---
 src/laser_safety.cpp  |  4 ++--
 src/local_planner.cpp | 35 +++++++++++++++++++++++------------
 2 files changed, 25 insertions(+), 14 deletions(-)

diff --git a/src/laser_safety.cpp b/src/laser_safety.cpp
index a62dbf5..41e16e5 100644
--- a/src/laser_safety.cpp
+++ b/src/laser_safety.cpp
@@ -37,9 +37,9 @@ Twist LaserSafetyFilter::filter(const Twist& cmd, const LaserScan& scan) const {
     double right_avg = right_n > 0 ? right_sum / right_n : 0;
 
     if (front_min < stop) {
-        // Emergency: stop and turn away
+        // Ackermann: cannot turn in place. Stop means stop.
         out.vx = 0.0;
-        out.wz = (left_avg > right_avg) ? turn_wz : -turn_wz;
+        out.wz = 0.0;
     } else if (front_min < slow) {
         out.vx *= 0.3;
     }
diff --git a/src/local_planner.cpp b/src/local_planner.cpp
index 1edb3dc..e02571a 100644
--- a/src/local_planner.cpp
+++ b/src/local_planner.cpp
@@ -77,8 +77,20 @@ double LocalPlanner::evaluateTrajectory(
     double end_to_goal = std::hypot(goal.x - sx, goal.y - sy);
     cost += W_FOLLOW * end_to_goal;
 
-    // ── Reverse penalty (mild) ──
-    if (vx < 0) cost += std::abs(vx) * 3.0;
+    // ── Progress critic: reward forward progress along path ──
+    if (total_path_len > 0.5) {
+        // Find start and end path indices
+        size_t s0 = 0, s1 = 0;
+        double d0 = 1e9, d1 = 1e9;
+        for (size_t i = 0; i < path.size(); ++i) {
+            double ds = std::hypot(pose.x - path[i].x, pose.y - path[i].y);
+            if (ds < d0) { d0 = ds; s0 = i; }
+            double de = std::hypot(sx - path[i].x, sy - path[i].y);
+            if (de < d1) { d1 = de; s1 = i; }
+        }
+        double progress = std::max(0.0, path_dists[s1] - path_dists[s0]);
+        cost += 20.0 * std::max(0.0, 0.25 - progress);  // penalty for too little progress
+    }
 
     return cost;
 }
@@ -107,20 +119,22 @@ Twist LocalPlanner::compute(const Pose2D& pose,
     for (int k = 0; k < K; ++k) {
         double vx, wz;
         if (k < exploit_n) {
-            // Exploit: sample around previous optimal
+            // Exploit: sample v + kappa (Ackermann-compatible)
             vx = prev_vx_ + VX_STD * randNormal();
-            wz = prev_wz_ + WZ_STD * randNormal();
+            double kappa = (prev_vx_ > 0.01) ? (prev_wz_ / prev_vx_ + WZ_STD * randNormal()) : (WZ_STD * randNormal());
+            wz = vx * kappa;
         } else {
-            // Explore: uniform random across full range (including reverse)
+            // Explore: uniform vx + kappa (Ackermann-compatible)
             uint32_t x = rng_state_;
             x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
-            vx = (double)(x & 0xFFFF) / 65535.0 * (ms_ + 0.5) - 0.5;
+            vx = (double)(x & 0xFFFF) / 65535.0 * ms_;
             x ^= x << 13; x ^= x >> 17; x ^= x << 5; rng_state_ = x;
-            wz = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * 3.0;
+            double kappa = ((double)(x & 0xFFFF) / 65535.0 - 0.5) * (2.0 / mr_);
+            wz = vx * kappa;
         }
 
-        // Clamp + Ackermann (no hard wz limit — Ackermann constraint is enough)
-        vx = limit(vx, -0.5, ms_);
+        // Clamp + Ackermann: no reverse, curvature-based wz
+        vx = limit(vx, 0.0, ms_);
         double max_wz = std::abs(vx) / mr_;
         wz = limit(wz, -max_wz, max_wz);
 
@@ -152,9 +166,6 @@ Twist LocalPlanner::compute(const Pose2D& pose,
     cmd.vx = weighted_vx / sum_weights;
     cmd.wz = weighted_wz / sum_weights;
 
-    // Enforce minimum reverse speed
-    if (cmd.vx < 0 && cmd.vx > -0.3) cmd.vx = -0.3;
-
     cmd.wz = limit(cmd.wz, -std::abs(cmd.vx) / mr_, std::abs(cmd.vx) / mr_);
 
     prev_vx_ = cmd.vx;