sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,211 @@
|
||||
"""
|
||||
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 openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \
|
||||
AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY
|
||||
|
||||
AUTO_LANE_CHANGE_TIMER_COMBOS = [
|
||||
(AutoLaneChangeMode.NUDGELESS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]),
|
||||
(AutoLaneChangeMode.HALF_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.HALF_SECOND]),
|
||||
(AutoLaneChangeMode.ONE_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.ONE_SECOND]),
|
||||
(AutoLaneChangeMode.TWO_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.TWO_SECONDS]),
|
||||
(AutoLaneChangeMode.THREE_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.THREE_SECONDS])
|
||||
]
|
||||
|
||||
|
||||
class TestAutoLaneChangeController:
|
||||
def setup_method(self):
|
||||
self.DH = DesireHelper()
|
||||
self.alc = AutoLaneChangeController(self.DH)
|
||||
|
||||
def _reset_states(self):
|
||||
self.alc.lane_change_bsm_delay = False
|
||||
self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
|
||||
self.lane_change_wait_timer = 0.0
|
||||
self.prev_brake_pressed = False
|
||||
self.prev_lane_change = False
|
||||
|
||||
def test_reset(self):
|
||||
"""Test that reset correctly sets timers back to default."""
|
||||
# Set some non-default values
|
||||
self.alc.lane_change_wait_timer = 2.0
|
||||
self.alc.prev_brake_pressed = True
|
||||
|
||||
# Set the DesireHelper to trigger a reset
|
||||
self.DH.lane_change_state = LaneChangeState.off
|
||||
self.DH.lane_change_direction = LaneChangeDirection.none
|
||||
|
||||
# Call reset
|
||||
self.alc.reset()
|
||||
|
||||
# Check values were reset
|
||||
assert self.alc.lane_change_wait_timer == 0.0
|
||||
assert not self.alc.prev_brake_pressed
|
||||
|
||||
@parameterized.expand([(AutoLaneChangeMode.OFF, ), (AutoLaneChangeMode.NUDGE, )])
|
||||
|
||||
def test_off_and_nudge_mode(self, timer_state):
|
||||
"""Test the default OFF and NUDGE mode behavior."""
|
||||
self._reset_states()
|
||||
# Setup mode
|
||||
self.alc.lane_change_bsm_delay = False # BSM delay off
|
||||
self.alc.lane_change_set_timer = timer_state
|
||||
|
||||
# Update controller
|
||||
num_updates = int(5.0 / DT_MDL)
|
||||
for _ in range(num_updates): # Run for 5 seconds
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# Mode should not allow lane change immediately
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
def test_nudgeless_mode(self):
|
||||
"""Test the NUDGELESS mode behavior."""
|
||||
self._reset_states()
|
||||
# Setup NUDGELESS mode
|
||||
self.alc.lane_change_bsm_delay = False # BSM delay off
|
||||
self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
|
||||
|
||||
# Update controller once to read params
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# Update multiple times to exceed the timer threshold
|
||||
for _ in range(1): # Should exceed 0.1s with multiple DT_MDL updates
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# Now lane change should be allowed
|
||||
assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
|
||||
assert self.alc.auto_lane_change_allowed
|
||||
|
||||
@parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
|
||||
def test_timers(self, timer_state, timer_delay):
|
||||
self._reset_states()
|
||||
self.alc.lane_change_bsm_delay = False # BSM delay off
|
||||
self.alc.lane_change_set_timer = timer_state
|
||||
|
||||
# Update controller once
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# The timer should still be below the threshold after one update
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
# Update enough times to exceed the threshold (seconds / DT_MDL)
|
||||
num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# Now lane change should be allowed
|
||||
assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
|
||||
assert self.alc.auto_lane_change_allowed
|
||||
|
||||
@parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
|
||||
def test_brake_pressed_disables_auto_lane_change(self, timer_state, timer_delay):
|
||||
"""Test that pressing the brake disables auto lane change."""
|
||||
self._reset_states()
|
||||
# Setup auto lane change mode
|
||||
self.alc.lane_change_bsm_delay = False
|
||||
self.alc.lane_change_set_timer = timer_state
|
||||
num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold
|
||||
|
||||
# Update with brake pressed for 1 second
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=True)
|
||||
|
||||
# Even though it is an auto lane change mode, lane change should be disallowed due to brake pressed prior initiating lane change
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
# Check that prev_brake_pressed is saved
|
||||
assert self.alc.prev_brake_pressed
|
||||
|
||||
# Even releasing brake shouldn't allow auto lane change
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
@parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
|
||||
def test_blindspot_detected_with_bsm_delay(self, timer_state, timer_delay):
|
||||
"""Test behavior when blindspot is detected with BSM delay enabled."""
|
||||
# Blindspot detected - should prevent auto lane change
|
||||
self._reset_states()
|
||||
self.alc.lane_change_bsm_delay = True # BSM delay on
|
||||
self.alc.lane_change_set_timer = timer_state
|
||||
|
||||
# Update with blindspot detected - this should prevent auto lane change
|
||||
self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
# Keep updating with blindspot detected - should still prevent auto lane change
|
||||
num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
@parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
|
||||
def test_blindspot_detected_then_undetected_with_bsm_delay(self, timer_state, timer_delay):
|
||||
"""Test behavior when blindspot is detected then undetected with BSM delay enabled."""
|
||||
# Blindspot clears - should allow auto lane change after sufficient time
|
||||
self._reset_states()
|
||||
self.alc.lane_change_bsm_delay = True
|
||||
self.alc.lane_change_set_timer = timer_state
|
||||
|
||||
# First update with blindspot detected to set the negative timer
|
||||
self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
# Now update with blindspot cleared - should start incrementing timer from negative value
|
||||
num_updates = int((timer_delay + abs(ONE_SECOND_DELAY)) / DT_MDL) + 1
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# After sufficient updates with no blindspot, auto lane change should be allowed
|
||||
assert self.alc.auto_lane_change_allowed
|
||||
|
||||
@parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
|
||||
def test_disallow_continuous_auto_lane_change(self, timer_state, timer_delay):
|
||||
self._reset_states()
|
||||
self.alc.lane_change_bsm_delay = False # BSM delay off
|
||||
self.alc.lane_change_set_timer = timer_state
|
||||
num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold
|
||||
|
||||
# Update enough times to exceed the threshold (seconds / DT_MDL)
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# Now lane change should be allowed
|
||||
assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
|
||||
assert self.alc.auto_lane_change_allowed
|
||||
|
||||
# Simulate lane change is initiated
|
||||
self.DH.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
self.alc.update_state()
|
||||
|
||||
# Simulate lane change is completed, and one_blinker stays on
|
||||
self.DH.lane_change_state = LaneChangeState.preLaneChange
|
||||
self.alc.update_state()
|
||||
|
||||
# Update enough times to exceed the threshold (seconds / DT_MDL)
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
|
||||
def test_auto_lane_change_mode_off_disallows_lane_change(self):
|
||||
"""Test that OFF mode never allows auto lane change."""
|
||||
self._reset_states()
|
||||
self.alc.lane_change_bsm_delay = False
|
||||
self.alc.lane_change_set_timer = AutoLaneChangeMode.OFF
|
||||
|
||||
# Simulate updates for a long period of time (e.g., 10 seconds)
|
||||
num_updates = int(10.0 / DT_MDL)
|
||||
for _ in range(num_updates):
|
||||
self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
|
||||
|
||||
# Lane change should never be allowed
|
||||
assert not self.alc.auto_lane_change_allowed
|
||||
@@ -0,0 +1,144 @@
|
||||
"""
|
||||
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 opendbc.car.structs import car
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
|
||||
|
||||
|
||||
class TestBlinkerPauseLateral:
|
||||
|
||||
def setup_method(self):
|
||||
self.blinker_pause_lateral = BlinkerPauseLateral()
|
||||
self._reset_states()
|
||||
|
||||
def _reset_states(self):
|
||||
self.blinker_pause_lateral.enabled = True
|
||||
self.blinker_pause_lateral.is_metric = False
|
||||
self.blinker_pause_lateral.min_speed = 20 # MPH
|
||||
self.blinker_pause_lateral.reengage_delay = 0
|
||||
self.blinker_pause_lateral.blinker_off_timer = 0.0
|
||||
|
||||
self.CS = car.CarState.new_message()
|
||||
self.CS.vEgo = 0
|
||||
self.CS.leftBlinker = False
|
||||
self.CS.rightBlinker = False
|
||||
|
||||
def _test_should_blinker_pause_lateral(self, expected_results) -> None:
|
||||
for left in (True, False):
|
||||
for right in (True, False):
|
||||
self.CS.leftBlinker = left
|
||||
self.CS.rightBlinker = right
|
||||
|
||||
result = self.blinker_pause_lateral.update(self.CS)
|
||||
assert result == expected_results[(left, right)]
|
||||
|
||||
def test_below_min_speed_blinker(self):
|
||||
self.CS.vEgo = 4.5 # ~10 MPH
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): True,
|
||||
(False, True): True,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
def test_reengage_delay(self):
|
||||
self.blinker_pause_lateral.reengage_delay = 2 # seconds
|
||||
self.CS.vEgo = 4.5 # ~10 MPH
|
||||
|
||||
expected_results = {
|
||||
(False, False): True,
|
||||
(True, False): True,
|
||||
(False, True): True,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
def test_above_min_speed_blinker(self):
|
||||
self.CS.vEgo = 13.4 # ~30 MPH
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): False,
|
||||
(False, True): False,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
def test_just_below_min_speed(self):
|
||||
self.CS.vEgo = (20 * CV.MPH_TO_MS) - 0.01
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): True,
|
||||
(False, True): True,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
def test_disabled(self):
|
||||
self.blinker_pause_lateral.enabled = False
|
||||
self.CS.vEgo = 4.5 # ~10 MPH
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): False,
|
||||
(False, True): False,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
def test_metric_units_below_min_speed(self):
|
||||
self.blinker_pause_lateral.is_metric = True
|
||||
self.CS.vEgo = 5.0 # ~18 km/h
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): True,
|
||||
(False, True): True,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
def test_metric_units_above_threshold(self):
|
||||
self.blinker_pause_lateral.is_metric = True
|
||||
self.CS.vEgo = 6.0 # ~21.6 km/h
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): False,
|
||||
(False, True): False,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
def test_change_min_speed_threshold(self):
|
||||
self.blinker_pause_lateral.min_speed = 30 # MPH
|
||||
|
||||
# below min speed
|
||||
self.CS.vEgo = 11.2 # ~25 MPH
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): True,
|
||||
(False, True): True,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
|
||||
# above min speed
|
||||
self.CS.vEgo = 15.6 # ~35 MPH
|
||||
|
||||
expected_results = {
|
||||
(False, False): False,
|
||||
(True, False): False,
|
||||
(False, True): False,
|
||||
(True, True): False
|
||||
}
|
||||
self._test_should_blinker_pause_lateral(expected_results)
|
||||
@@ -0,0 +1,113 @@
|
||||
import pytest
|
||||
from openpilot.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
|
||||
|
||||
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),
|
||||
])
|
||||
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
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
@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
|
||||
])
|
||||
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
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def set_lane_turn_params():
|
||||
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!
|
||||
])
|
||||
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
|
||||
Reference in new issue
Block a user