From 73c0af39e884fc33374632e425bf4020abae1559 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 24 Aug 2026 20:13:13 -0500 Subject: [PATCH] force stop: fast approach 2 --- .../controls/tests/test_starpilot_vcruise.py | 19 +++++++++++++++++++ starpilot/controls/lib/starpilot_vcruise.py | 10 ++++++++++ 2 files changed, 29 insertions(+) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 2c4b58632..fa444e63b 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -7,6 +7,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_GLOW_HOLD_TIME, CSC_GLOW_ON_DELTA from openpilot.starpilot.controls.lib.starpilot_vcruise import ( + FORCE_STOP_CAP_SLACK_M, FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME, StarPilotVCruise, @@ -57,6 +58,8 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False vcruise.forcing_stop = forcing_stop vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0 vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length + # what the not-committed branch would have left behind on the frame before commit + vcruise.force_stop_distance_cap = planner.model_length + FORCE_STOP_CAP_SLACK_M return planner, vcruise @@ -753,6 +756,7 @@ def test_force_stop_reanchors_when_model_reopens_path_without_stop_action(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 90.0 vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 90.0 + FORCE_STOP_CAP_SLACK_M sm = make_sm(standstill=False) sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) @@ -774,6 +778,21 @@ def test_force_stop_does_not_reanchor_inside_reanchor_floor(): assert vcruise.tracked_model_length < 25.0 +def test_force_stop_reanchor_bounded_by_distance_driven(): + # The line can't recede: a ballooning horizon may not push the stop past where it was at + # commit minus the distance driven since. + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + planner.model_length = 200.0 + vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 70.0 + sm = make_sm(standstill=False) + sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) + + update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.0) + + assert vcruise.tracked_model_length <= 70.0 + + def test_force_stop_does_not_reanchor_committed_model_stop(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 40.0 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index f583ae179..45824af02 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -83,6 +83,9 @@ FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0 FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0 # m — ignore small model-horizon noise FORCE_STOP_REANCHOR_MIN_M = 40.0 # m — inside this only ratchet down; shouldStop doesn't # assert until ~10 m, so horizon jitter would release the stop +FORCE_STOP_CAP_SLACK_M = 15.0 # m — the line can't move away, so tracked can never exceed + # what it was at commit minus distance driven. Slack covers an + # under-read at commit; without it that would stop us short. # Knob bounds (mirror of UI slider; defense in depth) OFFSET_FT_MIN = -20 @@ -189,6 +192,7 @@ class StarPilotVCruise: # Kinematic distance estimator. Same attribute also published as # starpilotPlan.forcingStopLength, so the existing reader keeps working. self.tracked_model_length = 0.0 + self.force_stop_distance_cap = 0.0 # odometry ceiling, re-seeded until commit self.stop_sign_confirmed = False self.stop_seen_on_approach_at = None @@ -675,6 +679,11 @@ class StarPilotVCruise: self.tracked_model_length = model_length else: self.tracked_model_length = min(self.tracked_model_length, model_length) + # Odometry ceiling: the line can't recede, so a re-anchor may never exceed what we + # had at commit minus what we've driven. Bounds a ballooning horizon (seen +95 m) + # that the REANCHOR_MIN floor can't catch, since that floor trusts the estimate. + self.force_stop_distance_cap = max(self.force_stop_distance_cap - (v_ego * DT_MDL), 0.0) + self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap) if dash_active: if model_length < DASH_MODEL_AGREE_M: self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) @@ -707,6 +716,7 @@ class StarPilotVCruise: self.stop_sign_confirmed = False self.tracked_model_length = self.starpilot_planner.model_length + self.force_stop_distance_cap = self.tracked_model_length + FORCE_STOP_CAP_SLACK_M targets = [v_cruise] if self.csc_target >= CSC_MIN_SPEED: