diff --git a/openpilot/starpilot/lateral/lane_centering.py b/openpilot/starpilot/lateral/lane_centering.py index 5fb4a932a0..1f0393bd05 100644 --- a/openpilot/starpilot/lateral/lane_centering.py +++ b/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) diff --git a/openpilot/starpilot/lateral/lane_clearance.py b/openpilot/starpilot/lateral/lane_clearance.py index 5d36a7454b..24171f87de 100644 --- a/openpilot/starpilot/lateral/lane_clearance.py +++ b/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: diff --git a/openpilot/starpilot/lateral/tests/test_lane_centering.py b/openpilot/starpilot/lateral/tests/test_lane_centering.py index 839c1dc772..afc5268800 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_centering.py +++ b/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 diff --git a/prebuilt.json b/prebuilt.json index 2685bf94fd..973d89cbae 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "5c25380412476e1691f77f810294cb97a6ea89dafbdf9104394ff6a8b17a9f52", "schema_version": 1} +{"protected_inventory_sha256": "804bd9a568a41527027706e09515eb565c59f01481dd70d12e3173fa3191f1f9", "schema_version": 1}