From baf56ae32469dec73c16610a1a5f020dda4da2d5 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Mon, 18 May 2026 12:18:03 -0700 Subject: [PATCH] feat/relc --- cereal/custom.capnp | 3 + common/params_keys.h | 1 + selfdrive/controls/lib/desire_helper.py | 6 +- selfdrive/selfdrived/selfdrived.py | 9 +- .../lane_change_settings.py | 6 ++ sunnypilot/modeld_v2/modeld.py | 7 +- sunnypilot/selfdrive/controls/lib/relc.py | 84 ++++++++++++++++ .../lib/tests/test_lane_turn_desire.py | 8 +- .../selfdrive/controls/lib/tests/test_relc.py | 99 +++++++++++++++++++ sunnypilot/selfdrive/selfdrived/events.py | 8 ++ sunnypilot/sunnylink/params_metadata.json | 4 + sunnypilot/sunnylink/settings_ui.json | 6 ++ 12 files changed, 235 insertions(+), 6 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/relc.py create mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_relc.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index fe3ed9196f..237ec79e64 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -342,6 +342,7 @@ struct OnroadEventSP @0xda96579883444c35 { speedLimitChanged @21; speedLimitPending @22; e2eChime @23; + laneChangeRoadEdge @24; } } @@ -448,6 +449,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { struct ModelDataV2SP @0xa1680744031fdb2d { laneTurnDirection @0 :TurnDirection; + leftLaneChangeEdgeBlock @1 :Bool; + rightLaneChangeEdgeBlock @2 :Bool; enum TurnDirection { none @0; diff --git a/common/params_keys.h b/common/params_keys.h index edfd63ec55..84f484057a 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -179,6 +179,7 @@ inline static std::unordered_map keys = { {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}}, {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 16908fa3e3..0891edf5ad 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -56,7 +56,7 @@ class DesireHelper: def get_lane_change_direction(CS): return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right - def update(self, carstate, lateral_active, lane_change_prob): + def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False): self.alc.update_params() self.lane_turn_controller.update_params() v_ego = carstate.vEgo @@ -88,8 +88,8 @@ 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 and self.lane_change_direction == LaneChangeDirection.left) or - (carstate.rightBlindspot 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)) self.alc.update_lane_change(blindspot_detected, carstate.brakePressed) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 30605d0a27..7f3ede8402 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -321,9 +321,16 @@ class SelfdriveD(CruiseHelper): # Handle lane change if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: direction = self.sm['modelV2'].meta.laneChangeDirection + mdv2sp = self.sm['modelDataV2SP'] + if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ - (CS.rightBlindspot and direction == LaneChangeDirection.right): + (CS.rightBlindspot and direction == LaneChangeDirection.right): self.events.add(EventName.laneChangeBlocked) + + elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \ + (mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right): + self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge) + else: if direction == LaneChangeDirection.left: self.events.add(EventName.preLaneChangeLeft) diff --git a/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py b/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py index fbb9ce7cf7..0f23264623 100644 --- a/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py +++ b/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py @@ -51,11 +51,17 @@ class LaneChangeSettingsLayout(Widget): description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " + "(BSM) detects a obstructing vehicle, ensuring safe maneuvering."), ) + self._road_edge_block = toggle_item_sp( + param="RoadEdgeLaneChangeEnabled", + title=lambda: tr("Block Lane Change: Road Edge Detection"), + description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."), + ) items = [ self._lane_change_timer, LineSeparatorSP(40), self._bsm_delay, + self._road_edge_block, ] return items diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index dfff2e6c22..0eb10c1b29 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -40,6 +40,7 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp from openpilot.sunnypilot.livedelay.helpers import get_lat_delay from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad" @@ -329,6 +330,7 @@ def main(demo=False): prev_action = log.ModelDataV2.Action() DH = DesireHelper() + RELC = RoadEdgeLaneChangeController(DH) meta_constants = load_meta_constants() while True: @@ -433,7 +435,10 @@ def main(demo=False): l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob - DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) + RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego) + mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected + mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected + DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction diff --git a/sunnypilot/selfdrive/controls/lib/relc.py b/sunnypilot/selfdrive/controls/lib/relc.py new file mode 100644 index 0000000000..747b7909de --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/relc.py @@ -0,0 +1,84 @@ +""" +Copyright (c) 2021-, rav4kumar, 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. +""" +import numpy as np + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.common.params import Params + +NEARSIDE_PROB = 0.2 +EDGE_PROB = 0.35 +EDGE_REACTION_TIME = 1.0 +EDGE_CLEAR_TIME = 0.3 +MIN_SPEED = 20 * CV.MPH_TO_MS + + +class RoadEdgeLaneChangeController: + def __init__(self, desire_helper): + self.DH = desire_helper + self.params = Params() + self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled") + self.param_read_counter = 0 + self.left_edge_detected = False + self.right_edge_detected = False + self.left_edge_timer = 0.0 + self.right_edge_timer = 0.0 + self.left_clear_timer = 0.0 + self.right_clear_timer = 0.0 + + def read_params(self) -> None: + self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled") + + def update_params(self) -> None: + if self.param_read_counter % 50 == 0: + self.read_params() + self.param_read_counter += 1 + + def reset(self) -> None: + self.left_edge_detected = False + self.right_edge_detected = False + self.left_edge_timer = 0.0 + self.right_edge_timer = 0.0 + self.left_clear_timer = 0.0 + self.right_clear_timer = 0.0 + + def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None: + self.update_params() + + if not self.enabled or v_ego < MIN_SPEED: + self.reset() + return + + left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0) + right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0) + left_lane_prob = lane_line_probs[0] + right_lane_prob = lane_line_probs[3] + + left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob + right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob + + if left_cond: + self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME) + self.left_clear_timer = 0.0 + if self.left_edge_timer > EDGE_REACTION_TIME: + self.left_edge_detected = True + else: + self.left_clear_timer += DT_MDL + if self.left_clear_timer > EDGE_CLEAR_TIME: + self.left_edge_timer = 0.0 + self.left_edge_detected = False + + if right_cond: + self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME) + self.right_clear_timer = 0.0 + if self.right_edge_timer > EDGE_REACTION_TIME: + self.right_edge_detected = True + else: + self.right_clear_timer += DT_MDL + if self.right_clear_timer > EDGE_CLEAR_TIME: + self.right_edge_timer = 0.0 + self.right_edge_detected = False 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 57fe7b684f..859ebd59ac 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -5,6 +5,8 @@ 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 +from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController + TurnDirection = custom.ModelDataV2SP.TurnDirection @@ -107,7 +109,11 @@ def set_lane_turn_params(): ]) def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params): dh = DesireHelper() + relc = RoadEdgeLaneChangeController(dh) + relc.enabled = True dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE for _ in range(10): - dh.update(carstate, lateral_active, lane_change_prob) + dh.update(carstate, lateral_active, lane_change_prob, + left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected) assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers + diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_relc.py b/sunnypilot/selfdrive/controls/lib/tests/test_relc.py new file mode 100644 index 0000000000..b54435a6d5 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_relc.py @@ -0,0 +1,99 @@ +""" +Copyright (c) 2021-, rav4kumar, 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. +""" +import pytest + +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.sunnypilot.selfdrive.controls.lib.relc import ( + RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED, +) + +V_HIGH = MIN_SPEED + 2.0 +V_LOW = MIN_SPEED - 1.0 + + +@pytest.fixture +def relc(mocker): + mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params") + mock_params.return_value.get_bool.return_value = True + controller = RoadEdgeLaneChangeController(DesireHelper()) + controller.enabled = True + return controller + + +def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH): + for _ in range(int(seconds / DT_MDL) + 1): + controller.update(road_edge_stds, lane_line_probs, v_ego) + + +@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [ + ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"), + ([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"), +]) +def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr): + drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1) + assert getattr(relc, attr) + + +def test_edge_detection_requires_time(relc): + drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05) + assert not relc.left_edge_detected + + +def test_both_edges_detected(relc): + drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1) + assert relc.left_edge_detected + assert relc.right_edge_detected + + +def test_noise_doesnt_clear(relc): + edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8]) + clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8]) + + drive(relc, *edge, EDGE_REACTION_TIME + 0.1) + assert relc.left_edge_detected + + relc.update(*clear, V_HIGH) + relc.update(*edge, V_HIGH) + assert relc.left_edge_detected + + +def test_clears_after_window(relc): + edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8]) + clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8]) + + drive(relc, *edge, EDGE_REACTION_TIME + 0.1) + assert relc.left_edge_detected + + drive(relc, *clear, EDGE_CLEAR_TIME + 0.05) + assert not relc.left_edge_detected + assert relc.left_edge_timer == 0.0 + + +def test_low_speed_skips(relc): + drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW) + assert not relc.left_edge_detected + assert relc.left_edge_timer == 0.0 + + +def test_speed_drop_resets(relc): + drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1) + assert relc.left_edge_detected + + relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW) + assert not relc.left_edge_detected + + +def test_param_off_resets(relc): + drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1) + assert relc.left_edge_detected + + relc.params.get_bool.return_value = False + relc.read_params() + relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH) + assert not relc.left_edge_detected + assert not relc.right_edge_detected diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 27b44fb4fc..a5f6c9bc9d 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -243,4 +243,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { AlertStatus.normal, AlertSize.none, Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), }, + + EventNameSP.laneChangeRoadEdge: { + ET.WARNING: Alert( + "Lane Change Unavailable: Road Edge", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1), + }, } diff --git a/sunnypilot/sunnylink/params_metadata.json b/sunnypilot/sunnylink/params_metadata.json index 59afb87c19..1e95fd91aa 100644 --- a/sunnypilot/sunnylink/params_metadata.json +++ b/sunnypilot/sunnylink/params_metadata.json @@ -1118,6 +1118,10 @@ "title": "Record Front Lock", "description": "" }, + "RoadEdgeLaneChangeEnabled": { + "title": "Block Lane Change: Road Edge Detection", + "description": "" + }, "RoadName": { "title": "Road Name", "description": "" diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 1745235d96..2ef5e4e036 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -519,6 +519,12 @@ } ] }, + { + "key": "RoadEdgeLaneChangeEnabled", + "widget": "toggle", + "title": "Block Lane Change: Road Edge Detection", + "description": "Blocks lane change when the model sees a road edge on the side you signal." + }, { "key": "AutoLaneChangeBsmDelay", "widget": "toggle",