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 c92a6ed89c..d7dccefda8 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -178,6 +178,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..7dde48204c 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, right_edge_detected): 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 1a10d91cd7..d18695967e 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -298,9 +298,15 @@ 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 or mdv2sp.rightLaneChangeEdgeBlock: + 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..b6dffd821a 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("Enable this toggle to block lane change when road edge is detected on the stalk actuated 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 f862286181..e76b2d24fd 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -34,6 +34,7 @@ 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.models.runners.helpers import get_model_runner +from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad" @@ -246,6 +247,9 @@ def main(demo=False): prev_action = log.ModelDataV2.Action() DH = DesireHelper() + RELC = RoadEdgeLaneChangeController(params.get_bool("RoadEdgeLaneChangeEnabled")) + + while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame @@ -349,7 +353,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) + 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..e3c14d0541 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/relc.py @@ -0,0 +1,113 @@ +""" +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 cereal import log +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 + +class RoadEdgeLaneChangeController: + def __init__(self, desire_helper): + self.desire_helper = desire_helper + self.params = Params() + self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled") + self.left_edge_detected = False + self.right_edge_detected = False + self.left_edge_timer = 0.0 + self.right_edge_timer = 0.0 + self._frame = 0 + + def set_enabled(self, enabled): + self.enabled = enabled + if not enabled: + self._reset_state() + + def _read_params(self) -> None: + if self._frame % int(1. / DT_MDL) == 0: + self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled") + + def _reset_state(self): + self.left_edge_detected = False + self.right_edge_detected = False + self.left_edge_timer = 0.0 + self.right_edge_timer = 0.0 + + def _update_edge_detection(self, road_edge_stds, lane_line_probs): + if not self.enabled: + return + + left_road_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0) + right_road_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0) + + # Lane line probabilities: [left_outer, left_inner, right_inner, right_outer] + left_lane_nearside_prob = lane_line_probs[0] if len(lane_line_probs) > 0 else 0.0 + right_lane_nearside_prob = lane_line_probs[3] if len(lane_line_probs) > 3 else 0.0 + + left_edge_conditions = ( + left_road_edge_prob > EDGE_PROB and + left_lane_nearside_prob < NEARSIDE_PROB and + (len(lane_line_probs) <= 3 or right_lane_nearside_prob >= left_lane_nearside_prob) + ) + right_edge_conditions = ( + right_road_edge_prob > EDGE_PROB and + right_lane_nearside_prob < NEARSIDE_PROB and + (len(lane_line_probs) <= 0 or left_lane_nearside_prob >= right_lane_nearside_prob) + ) + + if left_edge_conditions: + self.left_edge_timer += DT_MDL + self.left_edge_detected = self.left_edge_timer > EDGE_REACTION_TIME + else: + self.left_edge_timer = 0.0 + self.left_edge_detected = False + + if right_edge_conditions: + self.right_edge_timer += DT_MDL + self.right_edge_detected = self.right_edge_timer > EDGE_REACTION_TIME + else: + self.right_edge_timer = 0.0 + self.right_edge_detected = False + + def update(self, road_edge_stds, lane_line_probs): + self._read_params() + + if not self.enabled: + self._frame += 1 + return + + self._update_edge_detection(road_edge_stds, lane_line_probs) + self._frame += 1 + + def should_trigger_lane_change(self, carstate, lateral_active): + if not self.enabled: + return False, log.LaneChangeDirection.none + return False, log.LaneChangeDirection.none + + def is_lane_change_blocked(self, direction): + if not self.enabled: + return False + + if direction == log.LaneChangeDirection.left: + return self.left_edge_detected + elif direction == log.LaneChangeDirection.right: + return self.right_edge_detected + + return False + + def can_change_lane_left(self): + return not self.left_edge_detected if self.enabled else True + + def can_change_lane_right(self): + return not self.right_edge_detected if self.enabled else True + + @property + def edge_detected(self): + return self.left_edge_detected or self.right_edge_detected 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..3a6b29b946 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_relc.py @@ -0,0 +1,190 @@ +""" +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 cereal import log +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 + +@pytest.fixture +def relc_controller(mocker): + mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params") + mock_params.return_value.get_bool.return_value = True + + DH = DesireHelper() + relc = RoadEdgeLaneChangeController(DH) + relc.set_enabled(True) + return relc + + +def test_disable_resets_state(relc_controller): + relc = relc_controller + relc.left_edge_detected = True + relc.right_edge_detected = True + relc.left_edge_timer = 5.0 + relc.right_edge_timer = 5.0 + + relc.set_enabled(False) + + assert not relc.left_edge_detected + assert not relc.right_edge_detected + assert relc.left_edge_timer == 0.0 + assert relc.right_edge_timer == 0.0 + + +def test_lane_change_blocked_left(relc_controller): + relc = relc_controller + relc.left_edge_detected = True + assert relc.is_lane_change_blocked(log.LaneChangeDirection.left) + + +def test_lane_change_blocked_right(relc_controller): + relc = relc_controller + relc.right_edge_detected = True + assert relc.is_lane_change_blocked(log.LaneChangeDirection.right) + + +def test_lane_change_not_blocked_opposite_side(relc_controller): + relc = relc_controller + relc.left_edge_detected = True + assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right) + + relc.left_edge_detected = False + relc.right_edge_detected = True + assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left) + + +def test_lane_change_not_blocked_when_disabled(relc_controller): + relc = relc_controller + relc.set_enabled(False) + relc.left_edge_detected = True + relc.right_edge_detected = True + + assert not relc.is_lane_change_blocked(log.LaneChangeDirection.left) + assert not relc.is_lane_change_blocked(log.LaneChangeDirection.right) + + +def test_can_change_lane_left(relc_controller): + relc = relc_controller + assert relc.can_change_lane_left() + + relc.left_edge_detected = True + assert not relc.can_change_lane_left() + + +def test_can_change_lane_right(relc_controller): + relc = relc_controller + assert relc.can_change_lane_right() + + relc.right_edge_detected = True + assert not relc.can_change_lane_right() + + +def test_can_change_lane_when_disabled(relc_controller): + relc = relc_controller + relc.set_enabled(False) + relc.left_edge_detected = True + relc.right_edge_detected = True + + assert relc.can_change_lane_left() + assert relc.can_change_lane_right() + + +def test_edge_detected_property(relc_controller): + relc = relc_controller + assert not relc.edge_detected + + relc.left_edge_detected = True + assert relc.edge_detected + + relc.left_edge_detected = False + relc.right_edge_detected = True + assert relc.edge_detected + + relc.left_edge_detected = True + assert relc.edge_detected + + +def test_should_trigger_lane_change(relc_controller): + relc = relc_controller + should_trigger, direction = relc.should_trigger_lane_change(None, True) + assert not should_trigger + assert direction == log.LaneChangeDirection.none + + +def test_update_increments_frame(relc_controller): + relc = relc_controller + initial = relc._frame + relc.update([0.5, 0.5], [0.5, 0.5, 0.5, 0.5]) + assert relc._frame == initial + 1 + + +def test_left_edge_detection(relc_controller): + relc = relc_controller + road_edge_stds = [0.0, 0.9] + lane_line_probs = [0.0, 0.8, 0.8, 0.8] + + num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5 + for _ in range(num_updates): + relc.update(road_edge_stds, lane_line_probs) + + assert relc.left_edge_detected + + +def test_right_edge_detection(relc_controller): + relc = relc_controller + road_edge_stds = [0.9, 0.0] + lane_line_probs = [0.8, 0.8, 0.8, 0.0] + + num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5 + for _ in range(num_updates): + relc.update(road_edge_stds, lane_line_probs) + + assert relc.right_edge_detected + + +def test_edge_detection_requires_time(relc_controller): + relc = relc_controller + road_edge_stds = [0.0, 0.9] + lane_line_probs = [0.0, 0.8, 0.8, 0.8] + + num_updates = int(EDGE_REACTION_TIME / DT_MDL) - 1 + for _ in range(num_updates): + relc.update(road_edge_stds, lane_line_probs) + + assert not relc.left_edge_detected + + +def test_edge_detection_clears(relc_controller): + relc = relc_controller + road_edge_stds = [0.0, 0.9] + lane_line_probs = [0.0, 0.8, 0.8, 0.8] + + num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5 + for _ in range(num_updates): + relc.update(road_edge_stds, lane_line_probs) + assert relc.left_edge_detected + + road_edge_stds = [0.9, 0.9] + relc.update(road_edge_stds, lane_line_probs) + + assert not relc.left_edge_detected + assert relc.left_edge_timer == 0.0 + + +def test_both_edges_detected(relc_controller): + relc = relc_controller + road_edge_stds = [0.0, 0.0] + lane_line_probs = [0.0, 0.8, 0.8, 0.0] + + num_updates = int(EDGE_REACTION_TIME / DT_MDL) + 5 + for _ in range(num_updates): + relc.update(road_edge_stds, lane_line_probs) + + assert relc.left_edge_detected + assert relc.right_edge_detected diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index b1343b2a08..b2289c25a5 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), + }, }