diff --git a/selfdrive/ui/lib/speed_limit_pulse.py b/selfdrive/ui/lib/speed_limit_pulse.py new file mode 100644 index 0000000000..ce05506bfa --- /dev/null +++ b/selfdrive/ui/lib/speed_limit_pulse.py @@ -0,0 +1,32 @@ +import math + + +class SpeedLimitPulse: + def __init__(self): + self.last_limit = 0.0 + self.start_time = -math.inf + self._started_frame: int | None = None + + def clear(self) -> None: + self.start_time = -math.inf + + def reset(self) -> None: + self.last_limit = 0.0 + self._started_frame = None + self.clear() + + def update(self, source: str, limit: float, speed_conversion: float, now: float, started_frame: int) -> None: + if started_frame != self._started_frame: + self.reset() + self._started_frame = started_frame + + if not math.isfinite(limit) or limit <= 0: + self.clear() + return + + if source != "Vision": + self.clear() + elif round(limit * speed_conversion) != round(self.last_limit * speed_conversion): + self.start_time = now + + self.last_limit = limit diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index b48020759d..ab087addcb 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -6,6 +6,7 @@ from openpilot.common.constants import CV from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display_speed_limit_ms +from openpilot.selfdrive.ui.lib.speed_limit_pulse import SpeedLimitPulse from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.selfdrive.ui.onroad.exp_button import get_wheel_tint @@ -133,9 +134,7 @@ class HudRenderer(Widget): self._show_speed_limit_offset: bool = False self._speed_limit_overridden: bool = False self._pending_speed_limit: float = 0.0 - self._vision_speed_limit_active: bool = False - self._last_vision_speed_limit: float = 0.0 - self._vision_speed_limit_pulse_start: float = -VISION_SPEED_LIMIT_PULSE_SECONDS + self._speed_limit_pulse = SpeedLimitPulse() self._prompt_visible: bool = False self._prompt_card_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) self._prompt_sign_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0) @@ -256,12 +255,9 @@ class HudRenderer(Widget): primary_priority=primary_priority, secondary_priority=secondary_priority, ) - vision_speed_limit_active = starpilot_plan.slcSpeedLimitSource == "Vision" and resolved_speed_limit > 0.0 - vision_speed_limit_changed = abs(resolved_speed_limit - self._last_vision_speed_limit) >= 0.1 - if vision_speed_limit_active and (not self._vision_speed_limit_active or vision_speed_limit_changed): - self._vision_speed_limit_pulse_start = rl.get_time() - self._vision_speed_limit_active = vision_speed_limit_active - self._last_vision_speed_limit = resolved_speed_limit if vision_speed_limit_active else 0.0 + self._speed_limit_pulse.update( + starpilot_plan.slcSpeedLimitSource, resolved_speed_limit, speed_conversion, rl.get_time(), ui_state.started_frame, + ) display_speed_limit = starpilot_plan.slcOverriddenSpeed if starpilot_plan.slcOverriddenSpeed > 0 else resolved_speed_limit self._speed_limit = max(0.0, display_speed_limit * speed_conversion) @@ -276,8 +272,7 @@ class HudRenderer(Widget): self._show_speed_limit_offset = False self._speed_limit_overridden = False self._pending_speed_limit = 0.0 - self._vision_speed_limit_active = False - self._last_vision_speed_limit = 0.0 + self._speed_limit_pulse.clear() self._prompt_visible = self._pending_speed_limit > 0 else: self._show_speed_limit = False @@ -286,8 +281,7 @@ class HudRenderer(Widget): self._show_speed_limit_offset = False self._speed_limit_overridden = False self._pending_speed_limit = 0.0 - self._vision_speed_limit_active = False - self._last_vision_speed_limit = 0.0 + self._speed_limit_pulse.clear() self._prompt_visible = False def prepare(self, rect: rl.Rectangle) -> None: @@ -514,7 +508,7 @@ class HudRenderer(Widget): def _speed_limit_pulse_color(self, base_color, alpha: int) -> rl.Color: base = rl.Color(base_color.r, base_color.g, base_color.b, alpha) - elapsed = rl.get_time() - self._vision_speed_limit_pulse_start + elapsed = rl.get_time() - self._speed_limit_pulse.start_time if elapsed < 0.0 or elapsed >= VISION_SPEED_LIMIT_PULSE_SECONDS: return base diff --git a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py index 4b7174dad5..22dcfb59bc 100644 --- a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py +++ b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py @@ -17,6 +17,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import ( source_content_metrics, source_value_text, visible_source_rows, ) from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state +from openpilot.selfdrive.ui.lib.speed_limit_pulse import SpeedLimitPulse _WHITE = rl.Color(255, 255, 255, 255) @@ -53,41 +54,16 @@ FONT_EU_OFFSET = 40 # is "Vision" and the resolved value just changed. VISION_SPEED_LIMIT_PULSE_SECONDS = 1.0 VISION_SPEED_LIMIT_PULSE_COLOR = rl.Color(188, 132, 255, 255) -VISION_SPEED_LIMIT_CHANGE_THRESHOLD = 0.1 # m/s - - -# ── Vision speed-limit pulse state (one-shot highlight) ──────────────── -# Persists across frames so we can detect a just-changed vision limit and -# animate the sign colors toward VISION_SPEED_LIMIT_PULSE_COLOR for -# VISION_SPEED_LIMIT_PULSE_SECONDS. Held at module scope because this file -# is a procedural API consumed once per frame by StarPilotOnroadView. - -_pulse = { - "last": 0.0, # last resolved vision limit (m/s) seen while active - "start": -VISION_SPEED_LIMIT_PULSE_SECONDS, # get_time() stamp of the last change -} +_pulse = SpeedLimitPulse() def _reset_pulse() -> None: - """Clear pulse state when SLC goes hidden or stale.""" - _pulse["last"] = 0.0 - _pulse["start"] = -VISION_SPEED_LIMIT_PULSE_SECONDS + _pulse.reset() def _tick_pulse(source: str, resolved_ms: float) -> None: - """Update pulse state once per frame from the resolved speed limit. - - The pulse fires when the active source is "Vision" and its resolved value - changed by at least VISION_SPEED_LIMIT_CHANGE_THRESHOLD (m/s). - """ - vision_active = source == "Vision" and resolved_ms > 0.0 - if not vision_active: - _pulse["start"] = -VISION_SPEED_LIMIT_PULSE_SECONDS - return - - if abs(resolved_ms - _pulse["last"]) >= VISION_SPEED_LIMIT_CHANGE_THRESHOLD: - _pulse["start"] = rl.get_time() - _pulse["last"] = resolved_ms + speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH + _pulse.update(source, resolved_ms, speed_conversion, rl.get_time(), ui_state.started_frame) def _speed_limit_pulse_color(base: rl.Color, alpha: int) -> rl.Color: @@ -98,7 +74,7 @@ def _speed_limit_pulse_color(base: rl.Color, alpha: int) -> rl.Color: where t is elapsed / VISION_SPEED_LIMIT_PULSE_SECONDS. """ base_with_alpha = rl.Color(base.r, base.g, base.b, alpha) - elapsed = rl.get_time() - _pulse["start"] + elapsed = rl.get_time() - _pulse.start_time if elapsed < 0.0 or elapsed >= VISION_SPEED_LIMIT_PULSE_SECONDS: return base_with_alpha @@ -118,7 +94,7 @@ def _get_slc_state(): """Extract SLC state from SubMaster. Returns dict or None if stale/hidden.""" sm = ui_state.sm if sm.recv_frame["starpilotPlan"] < ui_state.started_frame: - _reset_pulse() + _pulse.clear() return None plan = sm["starpilotPlan"] @@ -129,7 +105,7 @@ def _get_slc_state(): unconfirmed_valid = plan.unconfirmedSlcSpeedLimit > 1 if not show_slc: - _reset_pulse() + _pulse.clear() return None speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH diff --git a/selfdrive/ui/tests/test_speed_limit_pulse.py b/selfdrive/ui/tests/test_speed_limit_pulse.py new file mode 100644 index 0000000000..f008d69d73 --- /dev/null +++ b/selfdrive/ui/tests/test_speed_limit_pulse.py @@ -0,0 +1,139 @@ +import math +from types import SimpleNamespace + +import pyray as rl +import pytest + +from openpilot.common.constants import CV +from openpilot.selfdrive.ui.lib.speed_limit_pulse import SpeedLimitPulse + + +@pytest.mark.parametrize("conversion", [CV.MS_TO_MPH, CV.MS_TO_KPH]) +def test_pulse_tracks_visible_number_without_delaying_real_changes(conversion): + pulse = SpeedLimitPulse() + pulse.update("Vision", 35 / conversion, conversion, 0.0, 1) + + for frame in range(1, 100): + limit = (35.3 if frame % 2 else 35.0) / conversion + pulse.update("Vision", limit, conversion, frame / 20, 1) + assert pulse.start_time == 0.0 + + pulse.update("Vision", 30 / conversion, conversion, 5.0, 1) + assert pulse.start_time == 5.0 + + +def test_source_reacquisition_does_not_repeat_an_already_visible_limit(): + pulse = SpeedLimitPulse() + for index, source in enumerate(["Map Data", "Vision", "Dashboard", "Vision"] * 10): + pulse.update(source, 15.6464, CV.MS_TO_MPH, float(index), 1) + assert pulse.start_time == -math.inf + + +def test_new_drive_resets_pulse_and_unit_change_does_not(): + pulse = SpeedLimitPulse() + pulse.update("Vision", 15.6464, CV.MS_TO_MPH, 1.0, 10) + pulse.update("Vision", 15.6464, CV.MS_TO_KPH, 2.0, 10) + assert pulse.start_time == 1.0 + + pulse.update("Vision", 15.6464, CV.MS_TO_KPH, 3.0, 20) + assert pulse.start_time == 3.0 + + +@pytest.mark.parametrize("missing", [0.0, float("nan"), float("inf")]) +def test_missing_reading_cancels_pulse_without_forgetting_limit(missing): + pulse = SpeedLimitPulse() + pulse.update("Vision", 15.6464, CV.MS_TO_MPH, 0.0, 1) + pulse.update("Vision", missing, CV.MS_TO_MPH, 0.2, 1) + pulse.update("Vision", 15.6464, CV.MS_TO_MPH, 0.3, 1) + assert pulse.start_time == -math.inf + + +class FakeParams(dict): + def get(self, key, encoding=None): + return super().get(key) + + def get_bool(self, key): + return bool(self.get(key)) + + +class FakeSubMaster(dict): + recv_frame = {"carState": 20, "starpilotPlan": 20} + valid = {"starpilotCarState": True} + + +@pytest.fixture(params=["big", "small"]) +def renderer(request, monkeypatch): + from openpilot.selfdrive.ui.mici.onroad import hud_renderer + from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit + + plan = SimpleNamespace( + slcSpeedLimit=15.6464, slcSpeedLimitSource="Vision", slcOverriddenSpeed=0.0, slcSpeedLimitOffset=0.0, + slcMapSpeedLimit=15.6464, slcMapboxSpeedLimit=0.0, slcNextSpeedLimit=0.0, + unconfirmedSlcSpeedLimit=0.0, speedLimitChanged=False, + ) + sm = FakeSubMaster({ + "starpilotPlan": plan, + "starpilotCarState": SimpleNamespace(dashboardSpeedLimit=15.6464), + "carState": SimpleNamespace(vCruiseCluster=100.0, vEgoCluster=0.0, vEgo=0.0, standstill=True), + "controlsState": SimpleNamespace(), + "selfdriveState": SimpleNamespace(enabled=False), + }) + sm.recv_frame = sm.recv_frame.copy() + params = FakeParams({"ShowSpeedLimits": True, "ShowSLCOffset": True, "VisionSpeedLimitDetection": True, + "SLCPriority1": "Vision", "SLCPriority2": "Map Data"}) + ui = SimpleNamespace(sm=sm, started_frame=10, is_metric=False, ui_params=params, + params_memory=SimpleNamespace(get_float=lambda _: plan.slcSpeedLimit)) + pulse = SpeedLimitPulse() + clock = [0.0] + monkeypatch.setattr(rl, "get_time", lambda: clock[0]) + + if request.param == "big": + monkeypatch.setattr(slc_speed_limit, "ui_state", ui) + monkeypatch.setattr(slc_speed_limit, "_pulse", pulse) + update = slc_speed_limit._get_slc_state + else: + monkeypatch.setattr(hud_renderer, "ui_state", ui) + monkeypatch.setattr(hud_renderer, "rivian_lateral_mode", SimpleNamespace(update=lambda: None, wheel_tint=None)) + hud = object.__new__(hud_renderer.HudRenderer) + hud._engaged = False + hud.set_speed = 100.0 + hud.v_ego_cluster_seen = False + hud._speed_limit_pulse = pulse + update = hud._update_state + + return SimpleNamespace(update=update, pulse=pulse, clock=clock, plan=plan, sm=sm, params=params) + + +@pytest.mark.parametrize("gap", ["hidden", "stale", "source"]) +def test_both_renderers_preserve_pulse_history_at_standstill(renderer, gap): + renderer.update() + assert renderer.pulse.start_time == 0.0 + renderer.clock[0] = 0.5 + + if gap == "hidden": + renderer.params["ShowSpeedLimits"] = False + elif gap == "stale": + renderer.sm.recv_frame["starpilotPlan"] = 9 + else: + renderer.plan.slcSpeedLimitSource = "Map Data" + renderer.update() + + renderer.params["ShowSpeedLimits"] = True + renderer.sm.recv_frame["starpilotPlan"] = 20 + renderer.plan.slcSpeedLimitSource = "Vision" + renderer.clock[0] = 0.6 + renderer.update() + assert renderer.pulse.start_time == -math.inf + + renderer.plan.slcSpeedLimit = 30 * CV.MPH_TO_MS + renderer.clock[0] = 0.7 + renderer.update() + assert renderer.pulse.start_time == 0.7 + + +def test_both_renderers_do_not_pulse_for_jitter_with_same_sign_number(renderer): + renderer.update() + renderer.plan.slcSpeedLimit = 35.3 * CV.MPH_TO_MS + renderer.clock[0] = 2.0 + renderer.update() + assert renderer.pulse.start_time == 0.0 diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py index ed730322ad..e33ac64e7f 100644 --- a/selfdrive/ui/ui.py +++ b/selfdrive/ui/ui.py @@ -45,7 +45,7 @@ def _stall_context() -> dict[str, object]: def main(): cores = {5, } - config_realtime_process(0, Priority.CTRL_HIGH) + config_realtime_process(0, Priority.UI) stall_monitor = UIStallMonitor("raylib_ui") stall_monitor.progress("ui.before_init_window")