From 4e11fbde11ac53eed41d290c22ee725b2d37d75f Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 12 May 2025 13:52:49 +0800 Subject: [PATCH] Road Edge Detection - init --- common/params_keys.h | 1 + selfdrive/controls/lib/desire_helper.py | 6 +- selfdrive/controls/lib/road_edge_detector.py | 59 ++++++++++++++++++++ selfdrive/modeld/modeld.py | 5 +- selfdrive/selfdrived/selfdrived.py | 8 ++- selfdrive/ui/qt/offroad/dp_panel.cc | 5 ++ system/manager/manager.py | 1 + 7 files changed, 79 insertions(+), 6 deletions(-) create mode 100644 selfdrive/controls/lib/road_edge_detector.py diff --git a/common/params_keys.h b/common/params_keys.h index b576f1a97..5cf79ed2c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -119,4 +119,5 @@ inline static std::unordered_map 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}, }; diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b685864..96bdac102 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/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 diff --git a/selfdrive/controls/lib/road_edge_detector.py b/selfdrive/controls/lib/road_edge_detector.py new file mode 100644 index 000000000..4179e5a59 --- /dev/null +++ b/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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index ee825d158..3e00d99d7 100755 --- a/selfdrive/modeld/modeld.py +++ b/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 diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index ecf76fe7f..c2e9689f3 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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: diff --git a/selfdrive/ui/qt/offroad/dp_panel.cc b/selfdrive/ui/qt/offroad/dp_panel.cc index 548b02be5..3f4808f8b 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.cc +++ b/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; diff --git a/system/manager/manager.py b/system/manager/manager.py index 9fe81947f..8db7bffc4 100755 --- a/system/manager/manager.py +++ b/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"):