diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index a77997ffbb..c20bf923be 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -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; diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index f0921b6723..7df58a0ba9 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -179,7 +179,10 @@ inline static std::unordered_map 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"}}, diff --git a/openpilot/selfdrive/controls/lib/desire_helper.py b/openpilot/selfdrive/controls/lib/desire_helper.py index df4e5c56ab..334b360a62 100644 --- a/openpilot/selfdrive/controls/lib/desire_helper.py +++ b/openpilot/selfdrive/controls/lib/desire_helper.py @@ -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) diff --git a/openpilot/selfdrive/modeld/modeld.py b/openpilot/selfdrive/modeld/modeld.py index 1ca1a71f35..5dc8712112 100755 --- a/openpilot/selfdrive/modeld/modeld.py +++ b/openpilot/selfdrive/modeld/modeld.py @@ -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 diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 2bc0574e81..8035b2934e 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -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) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/display.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/display.py index 8ba5663662..897d34085a 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/display.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/display.py @@ -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) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py index fbb9ce7cf7..82419a5567 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py @@ -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 diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index 4828f37103..3f2889de9a 100644 --- a/openpilot/selfdrive/ui/sunnypilot/ui_state.py +++ b/openpilot/selfdrive/ui/sunnypilot/ui_state.py @@ -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: diff --git a/openpilot/selfdrive/ui/ui_state.py b/openpilot/selfdrive/ui/ui_state.py index 59742edfed..67845bdc87 100644 --- a/openpilot/selfdrive/ui/ui_state.py +++ b/openpilot/selfdrive/ui/ui_state.py @@ -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) diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index b53ab18c73..9f3a75a662 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py index 1ba5ab0618..fb854edae8 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -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. diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/relc.py b/openpilot/sunnypilot/selfdrive/controls/lib/relc.py new file mode 100644 index 0000000000..031e751b43 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/relc.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py index c3e96fd778..434b12110b 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_relc.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_relc.py new file mode 100644 index 0000000000..71b153c8b7 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_relc.py @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/selfdrived/events.py b/openpilot/sunnypilot/selfdrive/selfdrived/events.py index 2001d0dbee..3c010cc776 100644 --- a/openpilot/sunnypilot/selfdrive/selfdrived/events.py +++ b/openpilot/sunnypilot/selfdrive/selfdrived/events.py @@ -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), + }, } diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 1e6422ac84..63bf342fcb 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -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 + } + ] } ] } diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/display.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/display.yaml index 39a8cbaf80..3e3b16c374 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/display.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/display.yaml @@ -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 diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml index a09796ab7a..73d46681a6 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml @@ -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' diff --git a/openpilot/system/manager/process_config.py b/openpilot/system/manager/process_config.py index a72675385d..08e1aabef3 100644 --- a/openpilot/system/manager/process_config.py +++ b/openpilot/system/manager/process_config.py @@ -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: diff --git a/openpilot/system/ui/sunnypilot/widgets/screen_saver.py b/openpilot/system/ui/sunnypilot/widgets/screen_saver.py new file mode 100644 index 0000000000..bf218306d8 --- /dev/null +++ b/openpilot/system/ui/sunnypilot/widgets/screen_saver.py @@ -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