From a06366ce9246962f313380e4ebe15955e720e7a5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 25 Oct 2026 22:48:26 -0400 Subject: [PATCH] controls: decouple RELC from BSM blocking, allow nudge override (#2049) * controls: decouple RELC from BSM blocking, allow nudge override * nah --- .../selfdrive/controls/lib/desire_helper.py | 11 +- .../controls/lib/auto_lane_change.py | 25 +++- .../lib/tests/test_auto_lane_change.py | 109 +++++++++++++++--- .../lib/tests/test_lane_turn_desire.py | 48 +++++++- 4 files changed, 169 insertions(+), 24 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/desire_helper.py b/openpilot/selfdrive/controls/lib/desire_helper.py index 334b360a62..ed8be505f8 100644 --- a/openpilot/selfdrive/controls/lib/desire_helper.py +++ b/openpilot/selfdrive/controls/lib/desire_helper.py @@ -64,16 +64,19 @@ class DesireHelper: ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) - blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or - ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right)) + blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or + (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - self.alc.update_lane_change(blindspot_detected, carstate.brakePressed) + edge_detected = ((left_edge_detected and self.lane_change_direction == LaneChangeDirection.left) or + (right_edge_detected and self.lane_change_direction == LaneChangeDirection.right)) + + self.alc.update_lane_change(blindspot_detected, edge_detected, carstate.brakePressed, torque_applied) if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 - elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected: + elif (torque_applied or self.alc.auto_lane_change_allowed) and not self.alc.lane_change_blocked: self.lane_change_state = LaneChangeState.laneChangeStarting self.lane_change_timer = 0.0 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/auto_lane_change.py b/openpilot/sunnypilot/selfdrive/controls/lib/auto_lane_change.py index 39909ac7b1..7f557e50b0 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/auto_lane_change.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/auto_lane_change.py @@ -10,6 +10,8 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL + + class AutoLaneChangeMode: OFF = -1 NUDGE = 0 # default @@ -31,6 +33,7 @@ AUTO_LANE_CHANGE_TIMER = { } ONE_SECOND_DELAY = -1 +EDGE_DELAY = -2 class AutoLaneChangeController: @@ -48,6 +51,7 @@ class AutoLaneChangeController: self.prev_brake_pressed = False self.auto_lane_change_allowed = False self.prev_lane_change = False + self.lane_change_blocked = False self.read_params() @@ -68,7 +72,7 @@ class AutoLaneChangeController: self.read_params() self.param_read_counter += 1 - def update_lane_change_timers(self, blindspot_detected: bool) -> None: + def update_lane_change_timers(self, blindspot_detected: bool, edge_detected: bool) -> None: self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE]) @@ -80,6 +84,12 @@ class AutoLaneChangeController: else: self.lane_change_wait_timer = self.lane_change_delay + ONE_SECOND_DELAY + if edge_detected and self.lane_change_delay > 0: + if self.lane_change_delay == AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]: + self.lane_change_wait_timer = min(self.lane_change_wait_timer, EDGE_DELAY) + else: + self.lane_change_wait_timer = min(self.lane_change_wait_timer, self.lane_change_delay + EDGE_DELAY) + def update_allowed(self) -> bool: # Auto lane change allowed if: # 1. A valid delay is set (non-zero) @@ -97,14 +107,23 @@ class AutoLaneChangeController: return bool(self.lane_change_wait_timer > self.lane_change_delay) - def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None: + def update_lane_change(self, blindspot_detected: bool, edge_detected: bool, brake_pressed: bool, + torque_applied: bool = False) -> None: if brake_pressed and not self.prev_brake_pressed: self.prev_brake_pressed = brake_pressed - self.update_lane_change_timers(blindspot_detected) + self.update_lane_change_timers(blindspot_detected, edge_detected) self.auto_lane_change_allowed = self.update_allowed() + # BSM always blocks. Edge blocks auto mode but nudge can override. + if blindspot_detected: + self.lane_change_blocked = True + elif edge_detected: + self.lane_change_blocked = not torque_applied + else: + self.lane_change_blocked = False + def update_state(self): if self.DH.lane_change_state == log.LaneChangeState.laneChangeStarting: self.prev_lane_change = True diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py index 0d95550412..4daadf73dc 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py @@ -8,7 +8,7 @@ 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, ONE_SECOND_DELAY, EDGE_DELAY from openpilot.common.test import OpenpilotTestCase AUTO_LANE_CHANGE_TIMER_COMBOS = [ @@ -61,7 +61,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase): # 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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) # Mode should not allow lane change immediately assert not self.alc.auto_lane_change_allowed @@ -74,11 +74,11 @@ class TestAutoLaneChangeController(OpenpilotTestCase): 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) + self.alc.update_lane_change(blindspot_detected=False, edge_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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) # Now lane change should be allowed assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay @@ -91,7 +91,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase): self.alc.lane_change_set_timer = timer_state # Update controller once - self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) # The timer should still be below the threshold after one update assert not self.alc.auto_lane_change_allowed @@ -99,7 +99,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase): # 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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) # Now lane change should be allowed assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay @@ -116,7 +116,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase): # Update with brake pressed for 1 second for _ in range(num_updates): - self.alc.update_lane_change(blindspot_detected=False, brake_pressed=True) + self.alc.update_lane_change(blindspot_detected=False, edge_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 @@ -126,7 +126,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase): # 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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) assert not self.alc.auto_lane_change_allowed @@ -139,13 +139,13 @@ class TestAutoLaneChangeController(OpenpilotTestCase): 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) + self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, 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) + self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False) assert not self.alc.auto_lane_change_allowed @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) @@ -157,13 +157,13 @@ class TestAutoLaneChangeController(OpenpilotTestCase): 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) + self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, 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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) # After sufficient updates with no blindspot, auto lane change should be allowed assert self.alc.auto_lane_change_allowed @@ -177,7 +177,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase): # 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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) # Now lane change should be allowed assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay @@ -193,7 +193,7 @@ class TestAutoLaneChangeController(OpenpilotTestCase): # 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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) assert not self.alc.auto_lane_change_allowed @@ -206,7 +206,86 @@ class TestAutoLaneChangeController(OpenpilotTestCase): # 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) + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) # Lane change should never be allowed assert not self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_edge_detected_delays_auto_lane_change(self, timer_state, timer_delay): + """Test that road edge detection adds delay to auto lane change.""" + self._reset_states() + self.alc.lane_change_bsm_delay = False + self.alc.lane_change_set_timer = timer_state + + # Update with edge detected - should prevent auto lane change + self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False) + assert not self.alc.auto_lane_change_allowed + + # Keep updating with edge detected - timer keeps resetting, should stay disallowed + num_updates = int(timer_delay / DT_MDL) + 1 + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False) + assert not self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_edge_detected_then_cleared_allows_after_delay(self, timer_state, timer_delay): + """Test that auto lane change is allowed after edge clears plus extra delay.""" + self._reset_states() + self.alc.lane_change_bsm_delay = False + self.alc.lane_change_set_timer = timer_state + + # First update with edge detected to set the negative timer + self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False) + assert not self.alc.auto_lane_change_allowed + + # Now update with edge cleared - should start incrementing timer from negative value + num_updates = int((timer_delay + abs(EDGE_DELAY)) / DT_MDL) + 1 + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) + + # After sufficient updates with no edge, auto lane change should be allowed + assert self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_edge_and_bsm_combined_uses_worst_delay(self, timer_state, timer_delay): + """Test that when both BSM and edge are detected, the worst (longest) delay wins.""" + self._reset_states() + self.alc.lane_change_bsm_delay = True + self.alc.lane_change_set_timer = timer_state + + # Update with both blindspot and edge detected + self.alc.update_lane_change(blindspot_detected=True, edge_detected=True, brake_pressed=False) + assert not self.alc.auto_lane_change_allowed + + # Clear both - need to wait out the worst delay + worst_delay = max(abs(ONE_SECOND_DELAY), abs(EDGE_DELAY)) + num_updates = int((timer_delay + worst_delay) / DT_MDL) + 1 + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, edge_detected=False, brake_pressed=False) + + assert self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_nudge_overrides_edge_in_lane_change_blocked(self, timer_state, timer_delay): + """Test that lane_change_blocked is False when nudge applied with edge-only.""" + self._reset_states() + self.alc.lane_change_set_timer = timer_state + + # Edge detected, no nudge - should be blocked + self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False, torque_applied=False) + assert self.alc.lane_change_blocked + + # Edge detected, nudge applied - should NOT be blocked + self.alc.update_lane_change(blindspot_detected=False, edge_detected=True, brake_pressed=False, torque_applied=True) + assert not self.alc.lane_change_blocked + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_nudge_does_not_override_bsm_in_lane_change_blocked(self, timer_state, timer_delay): + """Test that lane_change_blocked stays True when BSM detected even with nudge.""" + self._reset_states() + self.alc.lane_change_set_timer = timer_state + + # BSM detected with nudge - should still be blocked + self.alc.update_lane_change(blindspot_detected=True, edge_detected=False, brake_pressed=False, torque_applied=True) + assert self.alc.lane_change_blocked diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py index 52c6cecbfc..c59f3edca6 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -113,12 +113,56 @@ class TestDesireHelperIntegration(OpenpilotTestCase): left_edge_detected=False, right_edge_detected=False) assert dh.desire == expected_desire - def test_edge_blocks_lane_change(self, set_lane_turn_params): + def test_nudge_overrides_edge_detection(self, set_lane_turn_params): + """Manual steering nudge should override road edge detection and allow lane change.""" dh = DesireHelper() dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE carstate = DummyCarState(vEgo=15, leftBlinker=True, steeringPressed=True, steeringTorque=1) for _ in range(10): dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False) - assert dh.lane_change_state == LaneChangeState.preLaneChange + assert dh.lane_change_state == LaneChangeState.laneChangeStarting assert dh.lane_change_direction == LaneChangeDirection.left + assert dh.desire == log.Desire.laneChangeLeft + + def test_nudge_does_not_override_bsm(self, set_lane_turn_params): + """Manual steering nudge must NOT override BSM blindspot detection.""" + dh = DesireHelper() + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE + carstate = DummyCarState(vEgo=15, leftBlinker=True, leftBlindspot=True, steeringPressed=True, steeringTorque=1) + for _ in range(10): + dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=False) + assert dh.lane_change_state == LaneChangeState.preLaneChange assert dh.desire == log.Desire.none + + def test_edge_blocks_auto_lane_change(self, set_lane_turn_params): + """Road edge detection should still block auto (nudgeless) lane changes.""" + dh = DesireHelper() + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS + dh.alc.param_read_counter = 1 + carstate = DummyCarState(vEgo=15, leftBlinker=True) + for _ in range(10): + dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False) + assert dh.lane_change_state == LaneChangeState.preLaneChange + assert dh.desire == log.Desire.none + + def test_auto_lane_change_allowed_without_edge(self, set_lane_turn_params): + """Auto lane change should proceed when no edge and no BSM detected.""" + dh = DesireHelper() + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS + dh.alc.param_read_counter = 1 + carstate = DummyCarState(vEgo=15, leftBlinker=True) + for _ in range(10): + dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=False) + assert dh.lane_change_state == LaneChangeState.laneChangeStarting + assert dh.desire == log.Desire.laneChangeLeft + + def test_right_edge_does_not_block_left_lane_change(self, set_lane_turn_params): + """Edge on opposite side should not affect lane change direction.""" + dh = DesireHelper() + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS + dh.alc.param_read_counter = 1 + carstate = DummyCarState(vEgo=15, leftBlinker=True) + for _ in range(10): + dh.update(carstate, True, 1.0, left_edge_detected=False, right_edge_detected=True) + assert dh.lane_change_state == LaneChangeState.laneChangeStarting + assert dh.desire == log.Desire.laneChangeLeft