Merge branch 'master' into ccnc-port

This commit is contained in:
royjr
2026-08-11 11:56:46 -04:00
15 changed files with 332 additions and 10 deletions
+3
View File
@@ -351,6 +351,7 @@ struct OnroadEventSP @0xda96579883444c35 {
speedLimitChanged @21;
speedLimitPending @22;
e2eChime @23;
laneChangeRoadEdge @24;
}
}
@@ -457,6 +458,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
struct ModelDataV2SP @0xa1680744031fdb2d {
laneTurnDirection @0 :TurnDirection;
leftLaneChangeEdgeBlock @1 :Bool;
rightLaneChangeEdgeBlock @2 :Bool;
enum TurnDirection {
none @0;
+1
View File
@@ -179,6 +179,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"}},
{"ScreenSaverEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
{"ScreenSaverTimeout", {PERSISTENT | BACKUP, INT, "300"}},
@@ -33,7 +33,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=False, right_edge_detected=False):
self.alc.update_params()
self.lane_turn_controller.update_params()
v_ego = carstate.vEgo
@@ -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))
self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
+4 -1
View File
@@ -29,6 +29,7 @@ from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
PROCESS_NAME = "openpilot.selfdrive.modeld.modeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -213,6 +214,7 @@ def main(demo=False):
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
RELC = RoadEdgeLaneChangeController()
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -313,7 +315,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)
left_edge, right_edge = RELC.update_and_fill(modelv2_send.modelV2, mdv2sp_send.modelDataV2SP, v_ego)
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge)
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
@@ -327,9 +327,16 @@ 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):
self.events.add(EventName.laneChangeBlocked)
elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
(mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
@@ -51,11 +51,18 @@ 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("Blocks the lane change if the model sees a road edge on your signaled side."),
)
items = [
self._lane_change_timer,
LineSeparatorSP(40),
self._bsm_delay,
LineSeparatorSP(40),
self._road_edge_block,
]
return items
+4 -1
View File
@@ -49,6 +49,7 @@ from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, mak
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.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad"
@@ -367,6 +368,7 @@ def main(demo=False):
DH = DesireHelper()
meta_constants = load_meta_constants()
RELC = RoadEdgeLaneChangeController()
while True:
# Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -479,7 +481,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)
left_edge, right_edge = RELC.update_and_fill(modelv2_send.modelV2, mdv2sp_send.modelDataV2SP, v_ego)
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge)
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
@@ -1,5 +1,5 @@
"""
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
Copyright (c) 2021-, rav4kumar, 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.
@@ -0,0 +1,98 @@
"""
Copyright (c) 2021-, rav4kumar, 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 openpilot.common.constants import CV
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
EDGE_CLEAR_TIME = 0.3
MIN_SPEED = 20 * CV.MPH_TO_MS
VEHICLE_EDGE_MARGIN = 1.08
EDGE_CLEARANCE = 3.7
class RoadEdgeLaneChangeController:
def __init__(self):
self.params = Params()
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
self.param_read_counter = 0
self.left_edge_detected = False
self.right_edge_detected = False
self.left_edge_timer = 0.0
self.right_edge_timer = 0.0
self.left_clear_timer = 0.0
self.right_clear_timer = 0.0
def read_params(self) -> None:
self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
def update_params(self) -> None:
if self.param_read_counter % 50 == 0:
self.read_params()
self.param_read_counter += 1
def reset(self) -> None:
self.left_edge_detected = False
self.right_edge_detected = False
self.left_edge_timer = 0.0
self.right_edge_timer = 0.0
self.left_clear_timer = 0.0
self.right_clear_timer = 0.0
def update(self, road_edge_stds, lane_line_probs, v_ego: float, road_edges=None) -> None:
self.update_params()
if not self.enabled or v_ego < MIN_SPEED:
self.reset()
return
left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
left_lane_prob = lane_line_probs[0]
right_lane_prob = lane_line_probs[3]
if road_edges is not None and len(road_edges) == 2 and len(road_edges[0].y) > 0 and len(road_edges[1].y) > 0:
left_clearance = abs(road_edges[0].y[0]) - VEHICLE_EDGE_MARGIN
right_clearance = abs(road_edges[1].y[0]) - VEHICLE_EDGE_MARGIN
else:
left_clearance = 0.0
right_clearance = 0.0
left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and left_clearance < EDGE_CLEARANCE
right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and right_clearance < EDGE_CLEARANCE
if left_cond:
self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
self.left_clear_timer = 0.0
if self.left_edge_timer > EDGE_REACTION_TIME:
self.left_edge_detected = True
else:
self.left_clear_timer += DT_MDL
if self.left_clear_timer > EDGE_CLEAR_TIME:
self.left_edge_timer = 0.0
self.left_edge_detected = False
if right_cond:
self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
self.right_clear_timer = 0.0
if self.right_edge_timer > EDGE_REACTION_TIME:
self.right_edge_detected = True
else:
self.right_clear_timer += DT_MDL
if self.right_clear_timer > EDGE_CLEAR_TIME:
self.right_edge_timer = 0.0
self.right_edge_detected = False
def update_and_fill(self, modelv2, mdv2sp, v_ego):
self.update(modelv2.roadEdgeStds, modelv2.laneLineProbs, v_ego, modelv2.roadEdges)
mdv2sp.leftLaneChangeEdgeBlock = self.left_edge_detected
mdv2sp.rightLaneChangeEdgeBlock = self.right_edge_detected
return self.left_edge_detected, self.right_edge_detected
@@ -2,10 +2,11 @@ import pytest
from openpilot.cereal import log, custom
from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
TurnDirection = custom.ModelDataV2SP.TurnDirection
@@ -109,5 +110,17 @@ def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, e
dh = DesireHelper()
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
for _ in range(10):
dh.update(carstate, lateral_active, lane_change_prob)
assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
dh.update(carstate, lateral_active, lane_change_prob,
left_edge_detected=False, right_edge_detected=False)
assert dh.desire == expected_desire
def test_edge_blocks_lane_change(set_lane_turn_params):
dh = DesireHelper()
dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
carstate = DummyCarState(vEgo=15, leftBlinker=True, steeringPressed=True, steeringTorque=1)
for _ in range(10):
dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False)
assert dh.lane_change_state == LaneChangeState.preLaneChange
assert dh.lane_change_direction == LaneChangeDirection.left
assert dh.desire == log.Desire.none
@@ -0,0 +1,169 @@
"""
Copyright (c) 2021-, rav4kumar, 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 openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
VEHICLE_EDGE_MARGIN, EDGE_CLEARANCE,
)
V_HIGH = MIN_SPEED + 2.0
V_LOW = MIN_SPEED - 1.0
class MockEdge:
def __init__(self, y_val):
self.y = [y_val] * 33
def edges(left_y, right_y):
return [MockEdge(left_y), MockEdge(right_y)]
CLOSE_EDGES = edges(-2.0, 1.5)
FAR_EDGES = edges(-10.0, 10.0)
@pytest.fixture
def relc(mocker):
mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
controller = RoadEdgeLaneChangeController()
controller.enabled = True
return controller
def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH, road_edges=CLOSE_EDGES):
for _ in range(int(seconds / DT_MDL) + 1):
controller.update(road_edge_stds, lane_line_probs, v_ego, road_edges)
@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
])
def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
assert getattr(relc, attr)
def test_edge_detection_requires_time(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
assert not relc.left_edge_detected
def test_both_edges_detected(relc):
drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
assert relc.right_edge_detected
def test_noise_doesnt_clear(relc):
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
relc.update(*clear, V_HIGH, CLOSE_EDGES)
relc.update(*edge, V_HIGH, CLOSE_EDGES)
assert relc.left_edge_detected
def test_clears_after_window(relc):
edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
assert not relc.left_edge_detected
assert relc.left_edge_timer == 0.0
def test_low_speed_skips(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
assert not relc.left_edge_detected
assert relc.left_edge_timer == 0.0
def test_speed_drop_resets(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW, CLOSE_EDGES)
assert not relc.left_edge_detected
def test_param_off_resets(relc):
drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
assert relc.left_edge_detected
relc.params.get_bool.return_value = False
relc.read_params()
relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH, CLOSE_EDGES)
assert not relc.left_edge_detected
assert not relc.right_edge_detected
def test_lane_line_prevents_detection(relc):
drive(relc, [0.0, 0.9], [0.8, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
assert not relc.left_edge_detected
def test_one_side_blocks_other_allows(relc):
drive(relc, [0.9, 0.0], [0.8, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
assert relc.right_edge_detected
assert not relc.left_edge_detected
def test_disabled_no_detection(relc):
relc.enabled = False
relc.params.get_bool.return_value = False
drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
assert not relc.left_edge_detected
assert not relc.right_edge_detected
def test_far_edge_no_block(relc):
drive(relc, [0.0, 0.9], [0.05, 0.5, 0.5, 0.08], EDGE_REACTION_TIME + 0.1, road_edges=FAR_EDGES)
assert not relc.left_edge_detected
def test_close_edge_blocks(relc):
drive(relc, [0.9, 0.0], [0.05, 0.8, 0.8, 0.05], EDGE_REACTION_TIME + 0.1,
road_edges=edges(-8.0, 1.5))
assert relc.right_edge_detected
assert not relc.left_edge_detected
def test_wide_road_no_lines_no_block(relc):
drive(relc, [0.0, 0.0], [0.05, 0.4, 0.4, 0.05], EDGE_REACTION_TIME + 0.1,
road_edges=edges(-8.0, 8.0))
assert not relc.left_edge_detected
assert not relc.right_edge_detected
def test_narrow_road_both_block(relc):
drive(relc, [0.0, 0.0], [0.02, 0.4, 0.4, 0.02], EDGE_REACTION_TIME + 0.1,
road_edges=edges(-2.5, 2.5))
assert relc.left_edge_detected
assert relc.right_edge_detected
def test_clearance_boundary(relc):
boundary = VEHICLE_EDGE_MARGIN + EDGE_CLEARANCE # 4.78m
drive(relc, [0.0, 0.9], [0.05, 0.5, 0.5, 0.08], EDGE_REACTION_TIME + 0.1,
road_edges=edges(-(boundary - 0.1), 10.0))
assert relc.left_edge_detected
relc.reset()
drive(relc, [0.0, 0.9], [0.05, 0.5, 0.5, 0.08], EDGE_REACTION_TIME + 0.1,
road_edges=edges(-(boundary + 0.1), 10.0))
assert not relc.left_edge_detected
@@ -244,4 +244,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),
},
}
@@ -545,6 +545,12 @@
}
]
},
{
"key": "RoadEdgeLaneChangeEnabled",
"widget": "toggle",
"title": "Block Lane Change: Road Edge Detection",
"description": "Blocks lane change when the model sees a road edge on the side you signal."
},
{
"key": "AutoLaneChangeBsmDelay",
"widget": "toggle",
@@ -257,6 +257,10 @@ sections:
label: 2 seconds
- value: 5
label: 3 seconds
- key: RoadEdgeLaneChangeEnabled
widget: toggle
title: 'Block Lane Change: Road Edge Detection'
description: Blocks lane change when the model sees a road edge on the side you signal.
- key: AutoLaneChangeBsmDelay
widget: toggle
title: 'Auto Lane Change: Delay with Blind Spot'
+1 -1
View File
@@ -88,7 +88,7 @@ def use_sunnylink_uploader_shim(started, params, CP: car.CarParams) -> bool:
return use_sunnylink_uploader(params)
def is_tinygrad_model(started, params, CP: car.CarParams) -> bool:
"""Check if the active model runner is SNPE."""
"""Check if the active model runner is tinygrad."""
return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.tinygrad)
def is_stock_model(started, params, CP: car.CarParams) -> bool: