Reduce lane-centering correction during planner recovery

openpilot/starpilot/lateral/lane_centering.py

@@ -297,7 +297,7 @@ class LaneCenteringController:
       self._correction = 0.0
     return self._result(base, reason)
 
-  def _tracking_correction(self, base: float, correction: float, measured: float, speed: float, delay: float) -> float:
+  def _tracking_correction(self, base: float, correction: float, measured: float, speed: float, delay: float, *, damp_recovery: bool = False) -> float:
     if self._tracker.path is None:
       return correction
     x, y = self._tracker.path
@@ -306,7 +306,9 @@ class LaneCenteringController:
     if self._tracker.side is None and self._tracker.width is not None:
       bound = min(MAX_OFFSET, max(0., self._tracker.width * .5 - MIN_CENTER_TO_LINE))
       offset = clip_scalar(offset, -bound, bound)
-    return limit_tracking((x, y + offset), base, correction, measured, speed, delay)
+    spacing = self._tracker.width * .5 if self._tracker.side is None and self._tracker.width is not None else self._tracker.spacing
+    budget = max(0., spacing - MIN_CENTER_TO_LINE - abs(offset)) if damp_recovery and spacing is not None else 0.
+    return limit_tracking((x, y + offset), base, correction, measured, speed, delay, recovery_budget=budget)
 
   def guard_applied(self, model, base: float, candidate: float, measured: float, speed: float, delay) -> float:
     if self._body_required and (self._body is None or not self._body.valid):
@@ -394,7 +396,7 @@ class LaneCenteringController:
     measured = _finite_number(observation.measured_curvature)
     delay = _finite_number(observation.lateral_delay_s)
     if measured is not None and delay is not None:
-      target = self._tracking_correction(base, target, measured, speed, delay)
+      target = self._tracking_correction(base, target, measured, speed, delay, damp_recovery=True)
     step = (target - self._correction) * (1.0 - math.exp(-dt / ACQUIRE_TAU))
     max_step = MAX_CORRECTION_JERK * dt / speed / speed
     self._correction += clip_scalar(step, -max_step, max_step)

openpilot/starpilot/lateral/lane_clearance.py

@@ -136,7 +136,7 @@ def _limit_body_envelope(edges, base, correction, measured, speed, delay, body):
   return 0.
 
 
-def limit_tracking(path, base: float, correction: float, measured: float, speed: float, delay: float) -> float:
+def limit_tracking(path, base: float, correction: float, measured: float, speed: float, delay: float, *, recovery_budget: float = 0.) -> float:
   """A line-tracking contribution must improve its predicted spacing objective."""
   if not all(math.isfinite(v) for v in (base, correction, measured, speed, delay)) or speed < 5.0 or not 0.0 <= delay <= 1.0:
     return 0.0
@@ -156,6 +156,12 @@ def limit_tracking(path, base: float, correction: float, measured: float, speed:
   # Project onto the spacing objective instead of stepping between four fixed
   # correction fractions. Check the resulting nonlinear motion before use.
   factor = min(1.0, max(0.0, -float(error @ delta) / sensitivity))
+  if recovery_budget > 0.:
+    near, far = float(error[0]), float(error[-1])
+    if near != 0.:
+      reserve = max(0., 1. - float(np.max(np.abs(error))) / recovery_budget)
+      recovery = min(1., max(0., far / near))
+      factor *= 1. - reserve * min(1., abs(near) / .08) * (1. - recovery)
   if factor <= 0.0:
     return 0.0
   if factor == 1.0:

openpilot/starpilot/lateral/tests/test_lane_centering.py

@@ -738,6 +738,42 @@ class TestSingleLineTracking(unittest.TestCase):
       previous = result.correction
 
 
+class TestTrackingRecovery(unittest.TestCase):
+  def test_planner_recovery_reduces_the_added_nudge(self):
+    from openpilot.starpilot.lateral.lane_clearance import limit_tracking
+    source = observation(model(left=-1.5, right=2.1), base_curvature=.0007,
+                         measured_curvature=.0007, lateral_delay_s=.1)
+    controller = LaneCenteringController()
+    result = controller.update(source)
+    path = controller._tracker.path
+    assert path is not None
+    raw = raw_correction(source.model, 20., 0., 0.)
+    assert raw is not None
+    full = limit_tracking(path, .0007, min(MAX_CORRECTION, raw * .3 * 1.5), .0007, 20., .1)
+    self.assertGreater(result.correction, 0.)
+    self.assertLess(result.correction, full * (1. - math.exp(-.01 / .4)) * .95)
+    assert result.candidate_curvature is not None
+    self.assertAlmostEqual(controller.guard_applied(source.model, .0007, result.candidate_curvature,
+                                                   .0007, 20., .1) - .0007, result.correction, delta=1e-12)
+
+  def test_boundary_approach_preserves_nudge_and_immediate_override(self):
+    from openpilot.starpilot.lateral.lane_clearance import limit_tracking
+    source = observation(model(left=-.9, right=2.7), base_curvature=.0007,
+                         measured_curvature=.0007, lateral_delay_s=.1)
+    controller = LaneCenteringController()
+    result = controller.update(source)
+    path = controller._tracker.path
+    assert path is not None
+    raw = raw_correction(source.model, 20., 0., 0.)
+    assert raw is not None
+    full = limit_tracking(path, .0007, min(MAX_CORRECTION, raw * .3 * 1.5), .0007, 20., .1)
+    expected = min(full * (1. - math.exp(-.01 / .4)), .01 / 20. / 20.)
+    self.assertGreater(result.correction, 0.)
+    self.assertAlmostEqual(result.correction, expected, delta=1e-12)
+    self.assertEqual(controller.update(replace(source, driver_override=True)).correction, 0.)
+    self.assertEqual(controller.update(replace(source, lateral_active=False)).correction, 0.)
+
+
 class TestVehicleClearance(unittest.TestCase):
   def test_required_unknown_body_preserves_nominal_and_clears_retained_state(self):
     from openpilot.starpilot.lateral.lane_clearance import BodyDimensions

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "5c25380412476e1691f77f810294cb97a6ea89dafbdf9104394ff6a8b17a9f52", "schema_version": 1}
+{"protected_inventory_sha256": "804bd9a568a41527027706e09515eb565c59f01481dd70d12e3173fa3191f1f9", "schema_version": 1}