From 734151f59bf535054719e0ffc9a9621bbe963da3 Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Tue, 14 Oct 2025 20:32:10 -0700 Subject: [PATCH] Reapply "capnp: consolidate TurnDirection enum" (#1376) (#1382) * Reapply "capnp: consolidate TurnDirection enum" (#1376) This reverts commit 339bc0b8b3c4a192aa4e982039bc2ac28246c080. * cache it * format --------- Co-authored-by: Jason Wen --- cereal/custom.capnp | 10 +- selfdrive/controls/lib/desire_helper.py | 11 +- selfdrive/selfdrived/selfdrived.py | 5 +- .../controls/lib/lane_turn_desire.py | 12 +- .../lib/tests/test_lane_turn_desire.py | 160 +++++++++--------- 5 files changed, 101 insertions(+), 97 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index e81749dbac..5c0a004fa6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -446,12 +446,12 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; -} -enum TurnDirection { - none @0; - turnLeft @1; - turnRight @2; + enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; + } } struct CustomReserved10 @0xcb9fd56c7057593a { diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index e72d464d06..16908fa3e3 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -6,6 +6,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTur LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection +TurnDirection = custom.ModelDataV2SP.TurnDirection LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. @@ -32,9 +33,9 @@ DESIRES = { } TURN_DESIRES = { - custom.TurnDirection.none: log.Desire.none, - custom.TurnDirection.turnLeft: log.Desire.turnLeft, - custom.TurnDirection.turnRight: log.Desire.turnRight, + TurnDirection.none: log.Desire.none, + TurnDirection.turnLeft: log.Desire.turnLeft, + TurnDirection.turnRight: log.Desire.turnRight, } @@ -49,7 +50,7 @@ class DesireHelper: self.desire = log.Desire.none self.alc = AutoLaneChangeController(self) self.lane_turn_controller = LaneTurnController(self) - self.lane_turn_direction = custom.TurnDirection.none + self.lane_turn_direction = TurnDirection.none @staticmethod def get_lane_change_direction(CS): @@ -126,7 +127,7 @@ class DesireHelper: self.prev_one_blinker = one_blinker - if self.lane_turn_direction != custom.TurnDirection.none: + if self.lane_turn_direction != TurnDirection.none: self.desire = TURN_DESIRES[self.lane_turn_direction] else: self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 0a4ea749e4..3bc616fb04 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -44,6 +44,7 @@ LaneChangeDirection = log.LaneChangeDirection EventName = log.OnroadEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel +TurnDirection = custom.ModelDataV2SP.TurnDirection IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) @@ -305,9 +306,9 @@ class SelfdriveD(CruiseHelper): # Handle lane turn lane_turn_direction = self.sm['modelDataV2SP'].laneTurnDirection - if lane_turn_direction == custom.TurnDirection.turnLeft: + if lane_turn_direction == TurnDirection.turnLeft: self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnLeft) - elif lane_turn_direction == custom.TurnDirection.turnRight: + elif lane_turn_direction == TurnDirection.turnRight: self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnRight) for i, pandaState in enumerate(self.sm['pandaStates']): diff --git a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py index 00ce026abb..fa35ebb125 100644 --- a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py +++ b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py @@ -9,13 +9,15 @@ from cereal import custom from openpilot.common.constants import CV from openpilot.common.params import Params +TurnDirection = custom.ModelDataV2SP.TurnDirection + LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS class LaneTurnController: def __init__(self, desire_helper): self.DH = desire_helper - self.turn_direction = custom.TurnDirection.none + self.turn_direction = TurnDirection.none self.params = Params() self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS self.param_read_counter = 0 @@ -33,13 +35,13 @@ class LaneTurnController: def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None: if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left: - self.turn_direction = custom.TurnDirection.turnLeft + self.turn_direction = TurnDirection.turnLeft elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right: - self.turn_direction = custom.TurnDirection.turnRight + self.turn_direction = TurnDirection.turnRight else: - self.turn_direction = custom.TurnDirection.none + self.turn_direction = TurnDirection.none def get_turn_direction(self): if not self.enabled: - return custom.TurnDirection.none + return TurnDirection.none return self.turn_direction diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py index 5633ed6efc..57fe7b684f 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -1,113 +1,113 @@ import pytest -from cereal import log +from cereal import log, custom from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode - -class TurnDirection: - none = 0 - turnLeft = 1 - turnRight = 2 +TurnDirection = custom.ModelDataV2SP.TurnDirection @pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [ - (True, False, 5, False, False, TurnDirection.turnLeft), - (False, True, 6, False, False, TurnDirection.turnRight), - (True, False, 9, False, False, TurnDirection.none), - (True, False, 7, True, False, TurnDirection.none), - (False, True, 6, False, True, TurnDirection.none), - (False, False, 5, False, False, TurnDirection.none), - (True, True, 5, False, False, TurnDirection.none), + (True, False, 5, False, False, TurnDirection.turnLeft), + (False, True, 6, False, False, TurnDirection.turnRight), + (True, False, 9, False, False, TurnDirection.none), + (True, False, 7, True, False, TurnDirection.none), + (False, True, 6, False, True, TurnDirection.none), + (False, False, 5, False, False, TurnDirection.none), + (True, True, 5, False, False, TurnDirection.none), ]) def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected): - dh = DesireHelper() - controller = LaneTurnController(dh) - controller.enabled = True - controller.lane_turn_value = LANE_CHANGE_SPEED_MIN - controller.turn_direction = TurnDirection.none - controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego) - assert controller.get_turn_direction() == expected + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego) + assert controller.get_turn_direction() == expected def test_lane_turn_desire_disabled(): - dh = DesireHelper() - controller = LaneTurnController(dh) - controller.enabled = False - controller.lane_turn_value = LANE_CHANGE_SPEED_MIN - controller.turn_direction = TurnDirection.none - controller.update_lane_turn(False, False, True, False, 7) - assert controller.get_turn_direction() == TurnDirection.none + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = False + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(False, False, True, False, 7) + assert controller.get_turn_direction() == TurnDirection.none def test_lane_turn_overrides_lane_change(): - dh = DesireHelper() - controller = LaneTurnController(dh) - controller.enabled = True - controller.lane_turn_value = LANE_CHANGE_SPEED_MIN - controller.turn_direction = TurnDirection.none - # left turn desire - controller.update_lane_turn(False, False, True, False, 5) - assert controller.get_turn_direction() == TurnDirection.turnLeft - # right turn desire - controller.update_lane_turn(False, False, False, True, 6) - assert controller.get_turn_direction() == TurnDirection.turnRight - # no turn - controller.update_lane_turn(False, False, False, False, 7) - assert controller.get_turn_direction() == TurnDirection.none + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + # left turn desire + controller.update_lane_turn(False, False, True, False, 5) + assert controller.get_turn_direction() == TurnDirection.turnLeft + # right turn desire + controller.update_lane_turn(False, False, False, True, 6) + assert controller.get_turn_direction() == TurnDirection.turnRight + # no turn + controller.update_lane_turn(False, False, False, False, 7) + assert controller.get_turn_direction() == TurnDirection.none @pytest.mark.parametrize("v_ego,expected", [ - (8.93, TurnDirection.turnLeft), # just below threshold - (8.96, TurnDirection.none), # above threshold - (8.95, TurnDirection.none), # just above threshold + (8.93, TurnDirection.turnLeft), # just below threshold + (8.96, TurnDirection.none), # above threshold + (8.95, TurnDirection.none), # just above threshold ]) def test_lane_turn_desire_speed_boundary(v_ego, expected): - dh = DesireHelper() - controller = LaneTurnController(dh) - controller.enabled = True - controller.lane_turn_value = LANE_CHANGE_SPEED_MIN - controller.turn_direction = TurnDirection.none - controller.update_lane_turn(False, True, True, False, v_ego) - assert controller.get_turn_direction() == expected + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(False, True, True, False, v_ego) + assert controller.get_turn_direction() == expected class DummyCarState: - def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, - steeringPressed=False, steeringTorque=0, brakePressed=False): - self.vEgo = vEgo - self.leftBlinker = leftBlinker - self.rightBlinker = rightBlinker - self.leftBlindspot = leftBlindspot - self.rightBlindspot = rightBlindspot - self.steeringPressed = steeringPressed - self.steeringTorque = steeringTorque - self.brakePressed = brakePressed + def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, + steeringPressed=False, steeringTorque=0, brakePressed=False): + self.vEgo = vEgo + self.leftBlinker = leftBlinker + self.rightBlinker = rightBlinker + self.leftBlindspot = leftBlindspot + self.rightBlindspot = rightBlindspot + self.steeringPressed = steeringPressed + self.steeringTorque = steeringTorque + self.brakePressed = brakePressed + @pytest.fixture def set_lane_turn_params(): - params = Params() - params.put("LaneTurnDesire", True) - params.put("LaneTurnValue", 20.0) + params = Params() + params.put("LaneTurnDesire", True) + params.put("LaneTurnValue", 20.0) + @pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [ - # Lane turn desire overrides lane change desire - (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnLeft), - (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnRight), - # Lane change desire only (no turn desires) - (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, - steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft), - (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False, - steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight), - # 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! + # Lane turn desire overrides lane change desire + (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, + log.Desire.turnLeft), + (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0, + log.Desire.turnRight), + # Lane change desire only (no turn desires) + (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, + steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft), + (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False, + steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight), + # 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! ]) 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.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 + dh = DesireHelper() + 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