Turn Desire Bug

Blinker turn desire fed the model a turnLeft/turnRight input below lane-change
speed, inflating model_length so the car rolled past stop lines. Gate it in
desire_helper on prior-frame starpilotPlan.redLight/forcingStop/stopSignConfirmed
until standstill, then release so the model still turns through. "Stop first,
then turn."

selfdrive/controls/lib/desire_helper.py

@@ -61,6 +61,11 @@ class DesireHelper:
     self.prev_one_blinker = False
     self.desire = log.Desire.none
 
+    # Suppress the turn desire while stopping for a light/sign so the model keeps its
+    # stop plan (a turn desire extends model_length and rolls the car past the stop
+    # line). Released once the car has actually stopped: "stop first, then turn."
+    self.turn_stop_hold = False
+
     self.lane_change_completed = False
 
     self.lane_change_wait_timer = 0.0
@@ -237,6 +242,18 @@ class DesireHelper:
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < starpilot_toggles.minimum_lane_change_speed
+
+    # Hold the turn desire while a stop is in progress, release it once stopped. The
+    # plan message is a cycle behind the model, so its stop flags reflect the model's
+    # stop intent from before this frame's desire could inflate model_length.
+    stop_imminent = (bool(getattr(starpilotPlan, "redLight", False))
+                     or bool(getattr(starpilotPlan, "forcingStop", False))
+                     or bool(getattr(starpilotPlan, "stopSignConfirmed", False)))
+    if carstate.standstill or not one_blinker:
+      self.turn_stop_hold = False
+    elif stop_imminent:
+      self.turn_stop_hold = True
+
     cruise_state = getattr(carstate, "cruiseState", None)
     controls_enabled = bool(getattr(cruise_state, "enabled", False)) if controls_enabled is None else bool(controls_enabled)
     nudgeless_enabled = self._nudgeless_enabled(starpilot_toggles, controls_enabled)
@@ -316,7 +333,8 @@ class DesireHelper:
 
     self.prev_one_blinker = one_blinker
 
-    if lateral_active and one_blinker and below_lane_change_speed and not carstate.standstill and starpilot_toggles.use_turn_desires:
+    if lateral_active and one_blinker and below_lane_change_speed and not carstate.standstill \
+        and starpilot_toggles.use_turn_desires and not self.turn_stop_hold:
       self.turn_direction = TurnDirection.turnLeft if carstate.leftBlinker else TurnDirection.turnRight
       self.desire = TURN_DESIRES[self.turn_direction]
     else:

selfdrive/controls/tests/test_navigation_desires.py

@@ -448,6 +448,53 @@ def test_nav_desires_nudgeless_only_when_engaged_blocks_keep_when_aol_only():
   assert helper.desire == log.Desire.none
 
 
+def test_turn_desire_fires_below_lane_change_speed_when_no_stop():
+  helper = DesireHelper()
+
+  helper.update(
+    make_car_state(vEgo=5.0, rightBlinker=True),
+    True,
+    0.0,
+    make_plan(),
+    make_toggles(use_turn_desires=True, minimum_lane_change_speed=10.0),
+  )
+
+  assert helper.desire == log.Desire.turnRight
+
+
+def test_turn_desire_held_while_stopping_for_red_light():
+  helper = DesireHelper()
+
+  helper.update(
+    make_car_state(vEgo=5.0, rightBlinker=True),
+    True,
+    0.0,
+    make_plan(redLight=True),
+    make_toggles(use_turn_desires=True, minimum_lane_change_speed=10.0),
+  )
+
+  # A turn desire here would extend the model past the stop line, so it is withheld.
+  assert helper.turn_stop_hold
+  assert helper.desire == log.Desire.none
+
+
+def test_turn_desire_released_after_stop_completes():
+  helper = DesireHelper()
+  toggles = make_toggles(use_turn_desires=True, minimum_lane_change_speed=10.0)
+
+  # Approaching the stop with the blinker on -> desire held.
+  helper.update(make_car_state(vEgo=5.0, rightBlinker=True), True, 0.0, make_plan(redLight=True), toggles)
+  assert helper.desire == log.Desire.none
+
+  # Car reaches the stop line -> hold clears (still standstill, so no desire yet).
+  helper.update(make_car_state(vEgo=0.0, rightBlinker=True, standstill=True), True, 0.0, make_plan(redLight=True), toggles)
+  assert not helper.turn_stop_hold
+
+  # Pulling away through the turn, blinker still on, stop cleared -> turn desire resumes.
+  helper.update(make_car_state(vEgo=2.0, rightBlinker=True), True, 0.0, make_plan(), toggles)
+  assert helper.desire == log.Desire.turnRight
+
+
 def test_nav_desires_disabled_leave_desire_unchanged():
   helper = DesireHelper()
   helper.nav_desires_allowed = False