From 729ed065f6ae3358abf15a3f5cdf920400d9f048 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 12 May 2025 13:52:49 +0800 Subject: [PATCH] Road Edge Detection - 2025-06-17 2025-06-13: [Optimization] Use custom cereal messages instead of having RED running twice. --- cereal/custom.capnp | 4 +- cereal/log.capnp | 2 +- cereal/services.py | 1 + common/params_keys.h | 1 + .../controls/lib/road_edge_detector.py | 59 +++++++++++++++++++ selfdrive/controls/lib/desire_helper.py | 6 +- selfdrive/modeld/modeld.py | 11 +++- selfdrive/selfdrived/selfdrived.py | 8 +-- selfdrive/ui/qt/offroad/dp_panel.cc | 5 ++ system/manager/manager.py | 1 + 10 files changed, 87 insertions(+), 11 deletions(-) create mode 100644 dragonpilot/selfdrive/controls/lib/road_edge_detector.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3348e859e..38e456888 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -13,7 +13,9 @@ $Cxx.namespace("cereal"); struct CustomReserved0 @0x81c2f05a394cf4af { } -struct CustomReserved1 @0xaedffd8f31e7b55d { +struct ModelExt @0xaedffd8f31e7b55d { + leftEdgeDetected @0 :Bool; + rightEdgeDetected @1 :Bool; } struct CustomReserved2 @0xf35cc4560bbf6ec2 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 9ab51e0b7..ef2237dd2 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2603,7 +2603,7 @@ struct Event { # DON'T change the ID (e.g. @107) # DON'T change which struct it points to customReserved0 @107 :Custom.CustomReserved0; - customReserved1 @108 :Custom.CustomReserved1; + modelExt @108 :Custom.ModelExt; customReserved2 @109 :Custom.CustomReserved2; customReserved3 @110 :Custom.CustomReserved3; customReserved4 @111 :Custom.CustomReserved4; diff --git a/cereal/services.py b/cereal/services.py index 82fc04bd0..50fb02a00 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -92,6 +92,7 @@ _services: dict[str, tuple] = { "customReservedRawData0": (True, 0.), "customReservedRawData1": (True, 0.), "customReservedRawData2": (True, 0.), + "modelExt": (True, 20.), } SERVICE_LIST = {name: Service(*vals) for idx, (name, vals) in enumerate(_services.items())} diff --git a/common/params_keys.h b/common/params_keys.h index 92853f92f..b618c5aa6 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -122,4 +122,5 @@ inline static std::unordered_map keys = { {"Version", PERSISTENT}, {"dp_device_last_log", CLEAR_ON_ONROAD_TRANSITION}, {"dp_device_reset_conf", CLEAR_ON_MANAGER_START}, + {"dp_lat_road_edge_detection", PERSISTENT}, }; diff --git a/dragonpilot/selfdrive/controls/lib/road_edge_detector.py b/dragonpilot/selfdrive/controls/lib/road_edge_detector.py new file mode 100644 index 000000000..46ad4dd58 --- /dev/null +++ b/dragonpilot/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 = bool( + 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 = bool( + 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/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b685864..9dc2acaf7 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 = (((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)) if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1e3d1782e..031b8e479 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -36,6 +36,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, Plan from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext +from dragonpilot.selfdrive.controls.lib.road_edge_detector import RoadEdgeDetector PROCESS_NAME = "selfdrive.modeld.modeld" @@ -228,7 +229,7 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelExt"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() @@ -260,6 +261,7 @@ def main(demo=False): prev_action = log.ModelDataV2.Action() 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 @@ -343,6 +345,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') + model_ext_send = messaging.new_message('modelExt') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action @@ -354,7 +357,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) + RED.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs) + model_ext_send.modelExt.leftEdgeDetected = RED.left_edge_detected + model_ext_send.modelExt.rightEdgeDetected = RED.right_edge_detected + 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 @@ -364,6 +370,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + pm.send('modelExt', model_ext_send) last_vipc_frame_id = meta_main.frame_id diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 3065f6b80..08499f475 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -66,7 +66,7 @@ class SelfdriveD: # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches self.car_state_sock = messaging.sub_sock('carState', timeout=20) - ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelExt'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] if REPLAY: @@ -75,7 +75,7 @@ class SelfdriveD: self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userFlag'] + \ + 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userFlag', 'modelExt'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore, ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -230,8 +230,8 @@ class SelfdriveD: # Handle lane change if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: 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.sm['modelExt'].leftEdgeDetected) and direction == LaneChangeDirection.left) or \ + ((CS.rightBlindspot or self.sm['modelExt'].rightEdgeDetected) 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..f6daf0f67 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 (RED)"), + 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 5dc08c6d3..e2ab89573 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -40,6 +40,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"):