relc

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;

common/params_keys.h

@@ -178,6 +178,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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)
 

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)

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

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

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

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

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),
+  },
 }