Implement nudge mode for lane turn desires in LaneTurnController and DesireHelper

selfdrive/controls/lib/desire_helper.py

@@ -31,12 +31,6 @@ DESIRES = {
   },
 }
 
-TURN_DESIRES = {
-  custom.TurnDirection.none: log.Desire.none,
-  custom.TurnDirection.turnLeft: log.Desire.turnLeft,
-  custom.TurnDirection.turnRight: log.Desire.turnRight,
-}
-
 
 class DesireHelper:
   def __init__(self):
@@ -121,8 +115,10 @@ class DesireHelper:
 
     self.prev_one_blinker = one_blinker
 
-    if self.lane_turn_direction != custom.TurnDirection.none:
-      self.desire = TURN_DESIRES[self.lane_turn_direction]
+    lane_change_nudge_mode = self.alc.lane_change_set_timer == AutoLaneChangeMode.NUDGE
+    turn_desire = self.lane_turn_controller.get_desire(lane_change_nudge_mode)
+    if turn_desire != log.Desire.none:
+      self.desire = turn_desire
     else:
       self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
 

sunnypilot/selfdrive/controls/lib/lane_turn_desire.py

@@ -4,13 +4,20 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
-from cereal import custom
+from cereal import custom, log
 
 from openpilot.common.constants import CV
 from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 
+TURN_DESIRES = {
+  custom.TurnDirection.none: log.Desire.none,
+  custom.TurnDirection.turnLeft: log.Desire.turnLeft,
+  custom.TurnDirection.turnRight: log.Desire.turnRight,
+}
+
 
 class LaneTurnController:
   def __init__(self, desire_helper):
@@ -20,6 +27,7 @@ class LaneTurnController:
     self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
     self.param_read_counter = 0
     self.enabled = self.params.get_bool("LaneTurnDesire")
+    self.lane_turn_nudge_timer = 0.0
 
   def read_params(self):
     self.enabled = self.params.get_bool("LaneTurnDesire")
@@ -43,3 +51,24 @@ class LaneTurnController:
     if not self.enabled:
       return custom.TurnDirection.none
     return self.turn_direction
+
+  def get_desire(self, nudge_mode: bool) -> log.Desire:
+    # Return the current Desire for lane-turns.
+    # disabled or no turn -> reset timer and return none
+    if not self.enabled or self.turn_direction == custom.TurnDirection.none:
+      self.lane_turn_nudge_timer = 0.0
+      return log.Desire.none
+
+    # Normal (non-nudge) behavior: persistent turn desire
+    if not nudge_mode:
+      return TURN_DESIRES[self.turn_direction]
+
+    # Nudge mode: pulse a keep desire once per second
+    self.lane_turn_nudge_timer += DT_MDL
+    if self.lane_turn_nudge_timer > 1.0:
+      # reset and show the short pulse
+      self.lane_turn_nudge_timer = 0.0
+      return TURN_DESIRES[self.turn_direction]
+
+    # most of the second the desire is cleared
+    return log.Desire.none

sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -21,7 +21,8 @@ class TurnDirection:
     (False, True, 6, False, True, TurnDirection.none),
     (False, False, 5, False, False, TurnDirection.none),
     (True, True, 5, False, False, TurnDirection.none),
-])
+], ids=["left blinker", "right blinker", "no blinkers", "left blindspot w/ left blinker", "right blindspot w/ left blinker",
+        "no blinkers", "both blinkers"])
 def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected):
     dh = DesireHelper()
     controller = LaneTurnController(dh)
@@ -63,7 +64,7 @@ def test_lane_turn_overrides_lane_change():
     (8.93, TurnDirection.turnLeft),   # just below threshold
     (8.96, TurnDirection.none),       # above threshold
     (8.95, TurnDirection.none),       # just above threshold
-])
+], ids=["below threshold", "above threshold", "just above threshold"])
 def test_lane_turn_desire_speed_boundary(v_ego, expected):
     dh = DesireHelper()
     controller = LaneTurnController(dh)
@@ -91,6 +92,7 @@ def set_lane_turn_params():
     params = Params()
     params.put("LaneTurnDesire", True)
     params.put("LaneTurnValue", 20.0)
+    params.put("AutoLaneChangeTimer", 1)
 
 @pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [
     # Lane turn desire overrides lane change desire
@@ -104,10 +106,15 @@ def set_lane_turn_params():
     # No desire (inactive)
     (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none),
     (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none),  # No blinkers? no desire!
-])
+], ids=["turn left", "turn right", "lane change left", "lane change right", "inactive", "no blinkers"])
 def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
     dh = DesireHelper()
+
+    dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+    dh.update(carstate, lateral_active, lane_change_prob)
+
     dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
-    for _ in range(10):
-        dh.update(carstate, lateral_active, lane_change_prob)
-    assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
+    for _ in range(20):  # nudge mode requires 20 steps (.05 * 20 = 1 second) to simulate one second to enter true state
+      dh.update(carstate, lateral_active, lane_change_prob)
+
+    assert dh.desire == expected_desire  # This tests the integration in desire helpers