force stop: fast approach 2

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

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: