Road Edge Detection - init

common/params_keys.h

@@ -119,4 +119,5 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"Version", PERSISTENT},
     {"dp_device_last_log", CLEAR_ON_MANAGER_START},
     {"dp_device_reset_conf", CLEAR_ON_MANAGER_START},
+    {"dp_lat_road_edge_detection", PERSISTENT},
 };

selfdrive/controls/lib/desire_helper.py

@@ -40,7 +40,7 @@ class DesireHelper:
     self.prev_one_blinker = False
     self.desire = log.Desire.none
 
-  def update(self, carstate, lateral_active, lane_change_prob):
+  def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected, right_edge_detected):
     v_ego = carstate.vEgo
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
@@ -64,8 +64,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 = (((left_edge_detected or carstate.leftBlindspot) and self.lane_change_direction == LaneChangeDirection.left) or
+                              ((right_edge_detected or carstate.rightBlindspot) and self.lane_change_direction == LaneChangeDirection.right))
 
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off

selfdrive/controls/lib/road_edge_detector.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+"""
+MIT Non-Commercial License
+
+Copyright (c) 2019-, rav4kumar, Rick Lan, dragonpilot community, and a number of other of contributors.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation
+files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use,
+copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, for non-commercial purposes only,
+subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+ * Commercial use (e.g., use in a product, service, or activity intended to generate revenue) is prohibited without explicit written permission from dragonpilot. Contact ricklan@gmail.com for inquiries.
+ * Any project that uses the Software must visibly mention the following acknowledgment: "This project uses software from dragonpilot and is licensed under a custom license requiring permission for use."
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
+WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+"""
+
+import numpy as np
+
+NEARSIDE_PROB = 0.2
+EDGE_PROB = 0.35
+
+class RoadEdgeDetector:
+  def __init__(self, enabled = False):
+    self._is_enabled = enabled
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+
+  def update(self, road_edge_stds, lane_line_probs):
+    if not self._is_enabled:
+      return
+
+    left_road_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
+    left_lane_nearside_prob = lane_line_probs[0]
+
+    right_road_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
+    right_lane_nearside_prob = lane_line_probs[3]
+
+    self.left_edge_detected = (
+      left_road_edge_prob > EDGE_PROB and
+      left_lane_nearside_prob < NEARSIDE_PROB and
+      right_lane_nearside_prob >= left_lane_nearside_prob
+    )
+
+    self.right_edge_detected = (
+      right_road_edge_prob > EDGE_PROB and
+      right_lane_nearside_prob < NEARSIDE_PROB and
+      left_lane_nearside_prob >= right_lane_nearside_prob
+    )
+
+  def set_enabled(self, enabled):
+    self._is_enabled = enabled
+
+  def is_enabled(self):
+    return self._is_enabled

selfdrive/modeld/modeld.py

@@ -30,6 +30,7 @@ from openpilot.selfdrive.modeld.parse_model_outputs import Parser
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
+from openpilot.selfdrive.controls.lib.road_edge_detector import RoadEdgeDetector
 
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
@@ -213,6 +214,7 @@ def main(demo=False):
   steer_delay = CP.steerActuatorDelay + .2
 
   DH = DesireHelper()
+  RED = RoadEdgeDetector(params.get_bool("dp_lat_road_edge_detection"))
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -303,7 +305,8 @@ 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)
+      RED.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RED.left_edge_detected, RED.right_edge_detected)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state

selfdrive/selfdrived/selfdrived.py

@@ -20,6 +20,7 @@ from openpilot.selfdrive.selfdrived.events import Events, ET
 from openpilot.selfdrive.selfdrived.state import StateMachine
 from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
 from openpilot.selfdrive.controls.lib.latcontrol import MIN_LATERAL_CONTROL_SPEED
+from openpilot.selfdrive.controls.lib.road_edge_detector import RoadEdgeDetector
 
 from openpilot.system.version import get_build_metadata
 
@@ -131,6 +132,8 @@ class SelfdriveD:
     elif self.CP.passive:
       self.events.add(EventName.dashcamMode, static=True)
 
+    self.RED = RoadEdgeDetector(self.params.get_bool("dp_lat_road_edge_detection"))
+
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
 
@@ -220,9 +223,10 @@ class SelfdriveD:
 
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
+      self.RED.update(self.sm['modelV2'].roadEdgeStds, self.sm['modelV2'].laneLineProbs)
       direction = self.sm['modelV2'].meta.laneChangeDirection
-      if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-         (CS.rightBlindspot and direction == LaneChangeDirection.right):
+      if ((CS.leftBlindspot or self.RED.left_edge_detected) and direction == LaneChangeDirection.left) or \
+         ((CS.rightBlindspot or self.RED.right_edge_detected) and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
       else:
         if direction == LaneChangeDirection.left:

selfdrive/ui/qt/offroad/dp_panel.cc

@@ -106,6 +106,11 @@ void DPPanel::add_lateral_toggles() {
       QString::fromUtf8("🐉 ") + tr("Lateral Ctrl"),
       "",
     },
+    {
+      "dp_lat_road_edge_detection",
+      tr("Road Edge Detection"),
+      tr("Block lane change assist when the system detects the road edge.\nNOTE: This will show 'Car Detected in Blindspot' warning.")
+    },
   };
 
   QWidget *label = nullptr;

system/manager/manager.py

@@ -41,6 +41,7 @@ def manager_init() -> None:
     ("OpenpilotEnabledToggle", "1"),
     ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
     ("DisableLogging", "0"),
+    ("dp_lat_road_edge_detection", "0"),
   ]
 
   if params.get_bool("RecordFrontLock"):