mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-20 05:03:45 +08:00
Merge remote-tracking branch 'sunnypilot/sunnypilot/master' into hkg-angle-steering-2025
This commit is contained in:
Jason Wen
@@ -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;
|
||||
|
||||
@@ -179,7 +179,10 @@ 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"}},
|
||||
{"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -9,7 +9,7 @@ from enum import IntEnum
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.widgets.scroller_tici import Scroller
|
||||
from openpilot.system.ui.sunnypilot.widgets.list_view import option_item_sp
|
||||
from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp, option_item_sp
|
||||
from openpilot.sunnypilot.system.params_migration import ONROAD_BRIGHTNESS_TIMER_VALUES
|
||||
|
||||
|
||||
@@ -61,10 +61,26 @@ class DisplayLayout(Widget):
|
||||
f"{value} s" if value < 60 else f"{int(value/60)} m"),
|
||||
inline=True
|
||||
)
|
||||
self._screensaver_toggle = toggle_item_sp(
|
||||
param="ScreenSaverEnabled",
|
||||
title=lambda: tr("Screen Saver"),
|
||||
description=lambda: tr("Show a screen saver when the device is offroad and idle, instead of turning the screen off."),
|
||||
)
|
||||
self._screensaver_timeout = option_item_sp(
|
||||
param="ScreenSaverTimeout",
|
||||
title=lambda: tr("Screen Saver Duration"),
|
||||
description=lambda: tr("How long the screen saver runs before the screen turns off."),
|
||||
min_value=60,
|
||||
max_value=600,
|
||||
value_change_step=60,
|
||||
label_callback=lambda value: f"{int(value/60)} m"
|
||||
)
|
||||
items = [
|
||||
self._onroad_brightness,
|
||||
self._onroad_brightness_timer,
|
||||
self._interactivity_timeout,
|
||||
self._screensaver_toggle,
|
||||
self._screensaver_timeout,
|
||||
]
|
||||
return items
|
||||
|
||||
@@ -87,6 +103,8 @@ class DisplayLayout(Widget):
|
||||
brightness_val = self._onroad_brightness.action_item.current_value
|
||||
self._onroad_brightness_timer.action_item.set_enabled(brightness_val not in (OnroadBrightness.AUTO, OnroadBrightness.AUTO_DARK))
|
||||
|
||||
self._screensaver_timeout.set_visible(self._screensaver_toggle.action_item.get_state())
|
||||
|
||||
def _render(self, rect):
|
||||
self._scroller.render(rect)
|
||||
|
||||
|
||||
+7
@@ -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
|
||||
|
||||
@@ -12,6 +12,7 @@ from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.display import OnroadBrightness
|
||||
from openpilot.sunnypilot.sunnylink.sunnylink_state import SunnylinkState
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.sunnypilot.widgets.screen_saver import ScreenSaverSP
|
||||
|
||||
OpenpilotState = log.SelfdriveState.OpenpilotState
|
||||
MADSState = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState
|
||||
@@ -38,6 +39,9 @@ class UIStateSP:
|
||||
|
||||
self.sunnylink_state = SunnylinkState()
|
||||
|
||||
self.screensaver = ScreenSaverSP(params=self.params)
|
||||
self.screensaver_enabled: bool = False
|
||||
|
||||
self.active_bundle = None
|
||||
self.blindspot: bool = False
|
||||
self.chevron_metrics = None
|
||||
@@ -170,6 +174,7 @@ class UIStateSP:
|
||||
self.turn_signals = self.params.get_bool("ShowTurnSignals")
|
||||
self.boot_offroad_mode = self.params.get("DeviceBootMode", return_default=True)
|
||||
self.always_offroad = self.params.get_bool("OffroadMode")
|
||||
self.screensaver_enabled = self.params.get_bool("ScreenSaverEnabled")
|
||||
|
||||
if not self._sp_initialized:
|
||||
self._sp_initialized = True
|
||||
@@ -230,10 +235,26 @@ class UIStateSP:
|
||||
|
||||
|
||||
class DeviceSP:
|
||||
def __init__(self):
|
||||
self._blocked_by_screensaver: bool = False
|
||||
|
||||
def _set_awake(self, on: bool, _ui_state=None):
|
||||
self._blocked_by_screensaver = False
|
||||
|
||||
if _ui_state.boot_offroad_mode == 1 and not on:
|
||||
_ui_state.params.put_bool("OffroadMode", True)
|
||||
|
||||
if not on and _ui_state.screensaver_enabled:
|
||||
if _ui_state.screensaver.was_dismissed:
|
||||
if gui_app.get_active_widget() == _ui_state.screensaver:
|
||||
gui_app.pop_widget()
|
||||
elif _ui_state.screensaver.is_active:
|
||||
self._blocked_by_screensaver = True
|
||||
else:
|
||||
_ui_state.screensaver.initialize()
|
||||
gui_app.push_widget(_ui_state.screensaver)
|
||||
self._blocked_by_screensaver = True
|
||||
|
||||
@staticmethod
|
||||
def set_onroad_brightness(_ui_state, awake: bool, cur_brightness: float) -> float:
|
||||
if not awake or not _ui_state.started:
|
||||
|
||||
@@ -340,6 +340,8 @@ class Device(DeviceSP):
|
||||
def _set_awake(self, on: bool, _ui_state=None):
|
||||
if on != self._awake:
|
||||
super()._set_awake(on, _ui_state or ui_state)
|
||||
if self._blocked_by_screensaver:
|
||||
return
|
||||
self._awake = on
|
||||
cloudlog.debug(f"setting display power {int(on)}")
|
||||
HARDWARE.set_display_power(on)
|
||||
|
||||
@@ -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",
|
||||
@@ -1286,6 +1292,67 @@
|
||||
"label": "2 m"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ScreenSaverEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Screen Saver",
|
||||
"description": "Show a screen saver when the device is offroad and idle, instead of turning the screen off."
|
||||
},
|
||||
{
|
||||
"key": "ScreenSaverTimeout",
|
||||
"widget": "multiple_button",
|
||||
"title": "Screen Saver Duration",
|
||||
"description": "How long the screen saver runs before the screen turns off.",
|
||||
"options": [
|
||||
{
|
||||
"value": 60,
|
||||
"label": "1 m"
|
||||
},
|
||||
{
|
||||
"value": 120,
|
||||
"label": "2 m"
|
||||
},
|
||||
{
|
||||
"value": 180,
|
||||
"label": "3 m"
|
||||
},
|
||||
{
|
||||
"value": 240,
|
||||
"label": "4 m"
|
||||
},
|
||||
{
|
||||
"value": 300,
|
||||
"label": "5 m"
|
||||
},
|
||||
{
|
||||
"value": 360,
|
||||
"label": "6 m"
|
||||
},
|
||||
{
|
||||
"value": 420,
|
||||
"label": "7 m"
|
||||
},
|
||||
{
|
||||
"value": 480,
|
||||
"label": "8 m"
|
||||
},
|
||||
{
|
||||
"value": 540,
|
||||
"label": "9 m"
|
||||
},
|
||||
{
|
||||
"value": 600,
|
||||
"label": "10 m"
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "param",
|
||||
"key": "ScreenSaverEnabled",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -128,3 +128,36 @@ sections:
|
||||
label: 1 m
|
||||
- value: 120
|
||||
label: 2 m
|
||||
- key: ScreenSaverEnabled
|
||||
widget: toggle
|
||||
title: Screen Saver
|
||||
description: Show a screen saver when the device is offroad and idle, instead of turning the screen off.
|
||||
- key: ScreenSaverTimeout
|
||||
widget: multiple_button
|
||||
title: Screen Saver Duration
|
||||
description: How long the screen saver runs before the screen turns off.
|
||||
options:
|
||||
- value: 60
|
||||
label: 1 m
|
||||
- value: 120
|
||||
label: 2 m
|
||||
- value: 180
|
||||
label: 3 m
|
||||
- value: 240
|
||||
label: 4 m
|
||||
- value: 300
|
||||
label: 5 m
|
||||
- value: 360
|
||||
label: 6 m
|
||||
- value: 420
|
||||
label: 7 m
|
||||
- value: 480
|
||||
label: 8 m
|
||||
- value: 540
|
||||
label: 9 m
|
||||
- value: 600
|
||||
label: 10 m
|
||||
enablement:
|
||||
- type: param
|
||||
key: ScreenSaverEnabled
|
||||
equals: true
|
||||
|
||||
@@ -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'
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -0,0 +1,118 @@
|
||||
"""
|
||||
Copyright (c) 2021-, 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 os
|
||||
import time
|
||||
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.common.hardware import HARDWARE
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
|
||||
class ScreenSaverSP(Widget):
|
||||
def __init__(self, params: Params | None = None):
|
||||
super().__init__()
|
||||
self.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
|
||||
self._params = params or Params()
|
||||
self._is_mici = HARDWARE.get_device_type() == 'mici' or (HARDWARE.get_device_type() == "pc" and os.getenv("BIG") != "1")
|
||||
|
||||
self.x = 0.0
|
||||
self.y = 100.0
|
||||
self.vx = 120.0 if self._is_mici else 300.0
|
||||
self.vy = 70.0 if self._is_mici else 200.0
|
||||
self._hue = 150
|
||||
self.color = rl.color_from_hsv(self._hue, 1, 1)
|
||||
|
||||
self.text = "sunnypilot"
|
||||
self.font_size = 50 if self._is_mici else 200
|
||||
self._start_time = None
|
||||
self._dismiss = False
|
||||
self._screensaver_timeout = 300
|
||||
self._hit_last_frame = False
|
||||
|
||||
@property
|
||||
def is_active(self) -> bool:
|
||||
return self._start_time is not None and not self._dismiss
|
||||
|
||||
@property
|
||||
def was_dismissed(self) -> bool:
|
||||
return self._dismiss
|
||||
|
||||
def initialize(self):
|
||||
self._screensaver_timeout = self._params.get("ScreenSaverTimeout", return_default=True)
|
||||
if self._start_time is None:
|
||||
self._start_time = time.monotonic()
|
||||
self._dismiss = False
|
||||
|
||||
def hide_event(self):
|
||||
super().hide_event()
|
||||
self._dismiss = False
|
||||
self._start_time = None
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos):
|
||||
self._dismiss = True
|
||||
self._start_time = None
|
||||
gui_app.pop_widget()
|
||||
return super()._handle_mouse_release(mouse_pos)
|
||||
|
||||
def _update_state(self):
|
||||
super()._update_state()
|
||||
|
||||
self.font = gui_app.font(FontWeight.AUDIOWIDE)
|
||||
text_size = measure_text_cached(self.font, self.text, self.font_size, 0)
|
||||
self.logo_width = text_size.x
|
||||
self.logo_height = text_size.y
|
||||
|
||||
if self._start_time and time.monotonic() - self._start_time > self._screensaver_timeout:
|
||||
self._dismiss = True
|
||||
self._start_time = None
|
||||
|
||||
dt = rl.get_frame_time()
|
||||
|
||||
self.x += self.vx * dt
|
||||
self.y += self.vy * dt
|
||||
|
||||
hit_x = hit_y = False
|
||||
if self.x + self.logo_width > self.rect.width:
|
||||
self.vx *= -1
|
||||
self.x = self.rect.width - self.logo_width
|
||||
hit_x = True
|
||||
elif self.x < 0:
|
||||
self.vx *= -1
|
||||
self.x = 0
|
||||
hit_x = True
|
||||
|
||||
if self.y + self.logo_height > self.rect.height:
|
||||
self.vy *= -1
|
||||
self.y = self.rect.height - self.logo_height
|
||||
hit_y = True
|
||||
elif self.y < 0:
|
||||
self.vy *= -1
|
||||
self.y = 0
|
||||
hit_y = True
|
||||
|
||||
hit = hit_x or hit_y
|
||||
if hit and not self._hit_last_frame:
|
||||
while self._hue_dist((new_hue := rl.get_random_value(0, 360)), self._hue) < 120:
|
||||
pass
|
||||
self._hue = new_hue
|
||||
self.color = rl.color_from_hsv(self._hue, 1, 1)
|
||||
self._hit_last_frame = hit
|
||||
|
||||
@staticmethod
|
||||
def _hue_dist(a, b):
|
||||
d = abs(a - b)
|
||||
return min(d, 360 - d)
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
self.set_rect(rect)
|
||||
rl.clear_background(rl.BLACK)
|
||||
rl.draw_text_ex(self.font, self.text, rl.Vector2(int(self.x), int(self.y)), self.font_size, 0, self.color)
|
||||
return -1
|
||||
Reference in New Issue
Block a user