diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 8ba40d945..b5ff8fd0e 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/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) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 9d11b6cd4..56a6f45e2 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/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