Turn Desire Bug 2 + Force Stop

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -6,6 +6,7 @@ from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
 from openpilot.starpilot.controls.lib.starpilot_vcruise import (
+  FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
   StarPilotVCruise,
   get_active_slc_control_target,
   get_slc_lead_drop_relaxed_target,
@@ -368,7 +369,8 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
 
 
 def test_force_stop_turn_scene_veto_blocks_new_activation():
-  _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False)
+  # No stop seen: a wound wheel is a turn instead of a stop -> veto still blocks.
+  _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=False)
   sm = make_sm(standstill=False)
   sm["carState"].leftBlinker = True
   sm["carState"].steeringAngleDeg = 30.0
@@ -381,7 +383,8 @@ def test_force_stop_turn_scene_veto_blocks_new_activation():
 
 
 def test_force_stop_curve_veto_blocks_new_activation():
-  _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False, road_curvature=0.005)
+  # No stop seen: road curvature is a genuine curve -> curve veto still blocks.
+  _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=False, road_curvature=0.005)
   sm = make_sm(standstill=False)
   toggles = make_toggles()
 
@@ -393,6 +396,63 @@ def test_force_stop_curve_veto_blocks_new_activation():
   assert not vcruise.forcing_stop
 
 
+def test_force_stop_turn_scene_veto_yields_to_stop_then_turn():
+  # Low Speed Turn Assist winds the wheel into the turn while approaching a red light.
+  # The wound wheel must NOT block Force Stop when the model saw a stop -> stop-then-turn.
+  _, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=False)
+  sm = make_sm(standstill=False)
+  sm["carState"].leftBlinker = True
+  sm["carState"].steeringAngleDeg = 30.0
+  toggles = make_toggles()
+
+  for frame in range(12):
+    result = update_vcruise(vcruise, sm, toggles, now=frame * 0.05, v_ego=7.0)
+
+  assert 0.0 < result < 20.0
+  assert vcruise.force_stop_timer >= 0.5
+  assert vcruise.forcing_stop
+
+
+def test_force_stop_curve_veto_yields_to_stop_then_turn():
+  # Path curvature bends into the turn on a stop-then-turn approach -> curve veto must yield.
+  _, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=False, road_curvature=0.05)
+  sm = make_sm(standstill=False)
+  sm["carState"].rightBlinker = True
+  sm["carState"].steeringAngleDeg = -40.0
+  toggles = make_toggles()
+
+  for frame in range(12):
+    result = update_vcruise(vcruise, sm, toggles, now=frame * 0.05, v_ego=7.0)
+
+  assert 0.0 < result < 20.0
+  assert vcruise.force_stop_timer >= 0.5
+  assert vcruise.forcing_stop
+
+
+def test_stop_then_turn_override_releases_after_stop_seen_window_expires():
+  # Once the model stops seeing a stop and the hold window lapses, the veto resumes so a
+  # real mid-intersection turn isn't force-stopped.
+  _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=False)
+  planner = vcruise.starpilot_planner
+  sm = make_sm(standstill=False)
+  sm["carState"].leftBlinker = True
+  sm["carState"].steeringAngleDeg = 30.0
+  toggles = make_toggles()
+
+  # Stop seen briefly on approach (seeds the stop_then_turn latch), then it disappears.
+  planner.starpilot_cem.stop_light_detected = True
+  update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=7.0)
+  planner.starpilot_cem.stop_light_detected = False
+
+  # Past the hold window with no stop -> veto active again, no new activation.
+  now = FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME + 0.5
+  for frame in range(12):
+    result = update_vcruise(vcruise, sm, toggles, now=now + frame * 0.05, v_ego=7.0)
+
+  assert result == pytest.approx(20.0)
+  assert not vcruise.forcing_stop
+
+
 def test_force_stop_still_activates_for_straight_red_light_approach():
   _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False, road_curvature=0.001)
   sm = make_sm(standstill=False)

starpilot/controls/lib/starpilot_vcruise.py

@@ -50,6 +50,13 @@ FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS
 # for *new* activation — an in-progress stop is carried through (see force_stop_timer logic).
 FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0
 FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003
+# Low Speed Turn Assist pre-winds the wheel into an upcoming turn while the car is still
+# creeping to a stop, which crosses the veto angle before Force Stop can commit — so the
+# veto blocks the stop it was meant to carry through. When the model recently saw a stop
+# on approach, treat the wound wheel as a stop-then-turn (not turn-instead-of-stop) and
+# suppress the veto for this long after the stop was last seen, letting Force Stop arm and
+# latch; its committed hold + tracked_model_length then bridge the model dropout to the line.
+FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0
 
 # Knob bounds (mirror of UI slider; defense in depth)
 OFFSET_FT_MIN = -20
@@ -145,6 +152,9 @@ class StarPilotVCruise:
     self.tracked_model_length = 0.0
 
     self.stop_sign_confirmed = False
+    # Time a model/dash stop was last seen while approaching (moving). Suppresses the
+    # turn veto so LSTA's pre-wound wheel doesn't block a stop-then-turn.
+    self.stop_seen_on_approach_at = None
     self.nav_turn_target = 0.0
     self._nav_instruction_state_raw = None
     self._nav_instruction_state = {}
@@ -266,10 +276,30 @@ class StarPilotVCruise:
       self._applied_slc_control_target = 0.0
 
     long_control_active = sm["carControl"].longActive
+
+    # Track a model/dash stop seen while still moving (the approach). LSTA winds the wheel
+    # past the veto angle during this window; without this the veto zeroes the force-stop
+    # timer before it can commit, so the "carry an in-progress stop through" latch never arms.
+    raw_stop_seen = bool(
+      self.starpilot_planner.starpilot_cem.stop_light_detected
+      or getattr(self.starpilot_planner, "raw_model_stopped", False)
+      or sm["starpilotCarState"].dashboardStopSign > 0
+    )
+    if raw_stop_seen and not sm["carState"].standstill:
+      self.stop_seen_on_approach_at = now
+    elif sm["carState"].standstill:
+      # Stop reached: any wound wheel now is the turn itself, so let the veto resume.
+      self.stop_seen_on_approach_at = None
+    stop_then_turn = (
+      self.stop_seen_on_approach_at is not None
+      and self._elapsed_seconds(now, self.stop_seen_on_approach_at) < FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME
+    )
+
     turn_scene_active = bool(
       v_ego <= FORCE_STOP_TURN_VETO_MAX_SPEED and
       (getattr(sm["carState"], "leftBlinker", False) or getattr(sm["carState"], "rightBlinker", False)) and
-      abs(float(getattr(sm["carState"], "steeringAngleDeg", 0.0))) >= FORCE_STOP_TURN_VETO_STEERING_ANGLE
+      abs(float(getattr(sm["carState"], "steeringAngleDeg", 0.0))) >= FORCE_STOP_TURN_VETO_STEERING_ANGLE and
+      not stop_then_turn
     )
 
     # ----- Activation paths -----
@@ -280,7 +310,14 @@ class StarPilotVCruise:
     lead_present = (bool(getattr(lead, "status", False))
                     and float(getattr(lead, "dRel", float("inf"))) < ACTIVATION_M
                     and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0)
-    curved_approach_scene = abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE
+    # The road/path curvature bends toward the turn during a stop-then-turn approach, which
+    # trips this curve veto (meant for genuinely curvy roads) — same false block as the turn
+    # veto. When the model recently saw a stop here, the curvature is the turn we're stopping
+    # before, so let Force Stop arm through it. Left blinker-scoped via stop_then_turn.
+    curved_approach_scene = (
+      abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE
+      and not stop_then_turn
+    )
 
     # CEM/model path: model predicted stop within ACTIVATION_M.
     # Exclude when a lead is present (raw or filtered) — the handoff_to_stopped_lead path