diff --git a/selfdrive/ui/onroad/starpilot/curve_speed_border.py b/selfdrive/ui/onroad/starpilot/curve_speed_border.py deleted file mode 100644 index 47d06ebab3..0000000000 --- a/selfdrive/ui/onroad/starpilot/curve_speed_border.py +++ /dev/null @@ -1,208 +0,0 @@ -import math -import pyray as rl -from openpilot.selfdrive.ui import UI_BORDER_SIZE -from openpilot.selfdrive.ui.ui_state import ui_state - -# Amber palette -AMBER = rl.Color(251, 191, 36, 255) - -# Filament -_FILAMENT_WIDTH = 3.0 -_FILAMENT_PASSES = 3 -_FILAMENT_SPEED_BASE = 2.0 -_FILAMENT_SEGMENTS = 120 - -# Glow -_GLOW_LAYERS = 8 -_GLOW_MAX_ALPHA = 60 -_BREATHING_PERIOD = 4.5 # seconds — resting heart rate ~0.22 Hz - -# Curvature mapping -_CURVATURE_MIN = 0.001 -_CURVATURE_MAX = 0.1 - - -# ── State ────────────────────────────────────────────────────────────── - -def _csc_state(): - """Read CSC state from starpilotPlan. Returns dict or None if stale/hidden.""" - sm = ui_state.sm - if sm.recv_frame["starpilotPlan"] < ui_state.started_frame: - return None - if sm.recv_frame["carState"] < ui_state.started_frame: - return None - if sm.recv_frame["controlsState"] < ui_state.started_frame: - return None - - plan = sm["starpilotPlan"] - if plan.speedLimitChanged or not ui_state.params.get_bool("ShowCSCStatus"): - return None - - car_state = sm["carState"] - v_cruise = car_state.vCruiseCluster - if v_cruise == 0.0: - v_cruise = sm["controlsState"].vCruiseDEPRECATED - is_cruise_set = 0 < v_cruise < 255 - - return { - 'training': plan.cscTraining, - 'active': is_cruise_set and plan.cscControllingSpeed, - 'curvature': plan.roadCurvature, - } - - -# ── Math ─────────────────────────────────────────────────────────────── - -def _intensity(curvature: float) -> float: - """Map abs(curvature) from [CURVATURE_MIN, CURVATURE_MAX] → [0, 1].""" - return max(0.0, min(1.0, (abs(curvature) - _CURVATURE_MIN) / (_CURVATURE_MAX - _CURVATURE_MIN))) - - -def _perimeter(r: rl.Rectangle, roundness: float) -> list[tuple[float, float]]: - """Generate evenly-spaced points around a rounded rectangle perimeter.""" - rx = roundness * min(r.width, r.height) / 2.0 - rx = min(rx, r.width / 2.0, r.height / 2.0) - x, y, w, h = r.x, r.y, r.width, r.height - - # Corner centers - corners = [ - (x + w - rx, y + rx), # top-right - (x + w - rx, y + h - rx), # bottom-right - (x + rx, y + h - rx), # bottom-left - (x + rx, y + rx), # top-left - ] - - # Straight edge lengths (connecting corner tangent points) - edges = [ - w - 2 * rx, # top - h - 2 * rx, # right - w - 2 * rx, # bottom - h - 2 * rx, # left - ] - - arc_len = math.pi * rx / 2.0 - total = 4 * arc_len + sum(edges) - if total <= 0: - return [(x, y)] * _FILAMENT_SEGMENTS - - points = [] - for i in range(_FILAMENT_SEGMENTS): - d = (i / _FILAMENT_SEGMENTS) * total - # Walk corners and edges in order - for ci in range(4): - if d < arc_len: - frac = d / arc_len - # Sweep 90° clockwise: 270→0, 0→90, 90→180, 180→270 - angle = math.radians(270 + ci * 90 + frac * 90) - cx, cy = corners[ci] - points.append((cx + rx * math.cos(angle), cy + rx * math.sin(angle))) - break - d -= arc_len - - edge = edges[ci] - if d < edge: - frac = d / edge - cx, cy = corners[ci] - # Next corner in CW order - nx, ny = corners[(ci + 1) % 4] - # Tangent exit point from current corner - dx = (1.0, 0.0, -1.0, 0.0)[ci] - dy = (0.0, 1.0, 0.0, -1.0)[ci] - ex, ey = cx + rx * dx, cy + rx * dy - # Tangent entry point to next corner - nnx, nny = nx - rx * dx, ny - rx * dy - points.append((ex + (nnx - ex) * frac, ey + (nny - ey) * frac)) - break - d -= edge - else: - points.append((x, y)) - - return points - - -# ── Public API ───────────────────────────────────────────────────────── - -def render_glow(border_rect: rl.Rectangle, border_width: float = UI_BORDER_SIZE): - """Layer 3: Amber glow behind the standard border. Call BEFORE drawing standard border.""" - state = _csc_state() - if state is None or not state['active']: - return - - intensity = _intensity(state['curvature']) - if intensity <= 0.0: - return - - phase = (rl.get_time() % _BREATHING_PERIOD) / _BREATHING_PERIOD - breath = 0.5 + 0.5 * math.sin(phase * 2 * math.pi) - alpha = _GLOW_MAX_ALPHA * intensity * (0.5 + 0.5 * breath) - - for i in range(_GLOW_LAYERS): - inset = (border_width / _GLOW_LAYERS) * i - falloff = 1.0 - (i / _GLOW_LAYERS) - a = int(alpha * falloff * falloff) - if a < 2: - continue - - glow_rect = rl.Rectangle( - border_rect.x + inset, - border_rect.y + inset, - border_rect.width - 2 * inset, - border_rect.height - 2 * inset, - ) - rl.draw_rectangle_rounded(glow_rect, 0.12, 10, rl.Color(251, 191, 36, a)) - - -def render_filament(border_rect: rl.Rectangle, border_width: float = UI_BORDER_SIZE): - """Layer 5: Amber filament on top of the standard border. Call AFTER drawing standard border.""" - state = _csc_state() - if state is None: - return - - if not state['active'] and not state['training']: - return - - curvature = state['curvature'] - inner = rl.Rectangle( - border_rect.x + border_width, - border_rect.y + border_width, - border_rect.width - 2 * border_width, - border_rect.height - 2 * border_width, - ) - - points = _perimeter(inner, 0.12) - n = len(points) - - direction = -1.0 if curvature < 0 else 1.0 - speed = _FILAMENT_SPEED_BASE + _intensity(curvature) * 3.0 - time_offset = rl.get_time() * speed * direction - - base_alpha = 120 if state['training'] else 200 - - for pass_idx in range(_FILAMENT_PASSES): - width = _FILAMENT_WIDTH + pass_idx * 1.5 - alpha_scale = 1.0 - (pass_idx / _FILAMENT_PASSES) * 0.5 - - for i in range(n): - j = (i + 1) % n - t = ((i + time_offset) / n) % 1.0 - - # Single pulse: ramp up 0→25%, hold 25→50%, ramp down 50→75%, off 75→100% - if t < 0.25: - a = t / 0.25 - elif t < 0.5: - a = 1.0 - elif t < 0.75: - a = (0.75 - t) / 0.25 - else: - continue - - alpha = int(base_alpha * a * alpha_scale) - if alpha < 2: - continue - - rl.draw_line_ex( - rl.Vector2(points[i][0], points[i][1]), - rl.Vector2(points[j][0], points[j][1]), - width, - rl.Color(251, 191, 36, alpha), - ) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_border.py b/selfdrive/ui/onroad/starpilot/starpilot_border.py new file mode 100644 index 0000000000..0d821fc492 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/starpilot_border.py @@ -0,0 +1,182 @@ +from __future__ import annotations +import math +from dataclasses import dataclass +from collections.abc import Callable +from enum import Enum +import pyray as rl +from openpilot.selfdrive.ui import UI_BORDER_SIZE +from openpilot.selfdrive.ui.lib.starpilot_status import get_screen_edge_color +from openpilot.selfdrive.ui.ui_state import ui_state + + +class BorderLayer(Enum): + BEHIND = 0 + OVERLAY = 1 + + +@dataclass +class BorderEffect: + render_fn: Callable[[rl.Rectangle, float], None] + layer: BorderLayer + name: str = "" + + +_GLOW_LAYERS = 16 +_GLOW_MAX_ALPHA = 255 +_GLOW_MIN_ALPHA = 250 +_GLOW_BASE_INTENSITY = 0.70 +_GLOW_SPAN = 3.5 +_BREATHING_PERIOD = 4.5 +_GLOW_FADE_IN_DURATION = 0.6 + +_CURVATURE_MIN = 0.0005 +_CURVATURE_MAX = 0.02 + +_GREEN = rl.Color(34, 197, 94, 255) +_AMBER = rl.Color(251, 191, 36, 255) +_ORANGE = rl.Color(234, 88, 12, 255) +_RED = rl.Color(201, 34, 49, 255) + +_last_was_active = False +_activation_start = 0.0 +_fade_out_start = 0.0 +_last_state = None + + +def _csc_state(): + sm = ui_state.sm + if sm.recv_frame["starpilotPlan"] < ui_state.started_frame: + return None + if sm.recv_frame["carState"] < ui_state.started_frame: + return None + if sm.recv_frame["controlsState"] < ui_state.started_frame: + return None + + plan = sm["starpilotPlan"] + if plan.speedLimitChanged or not ui_state.params.get_bool("ShowCSCStatus"): + return None + + car_state = sm["carState"] + v_cruise = car_state.vCruiseCluster + if v_cruise == 0.0: + v_cruise = sm["controlsState"].vCruiseDEPRECATED + is_cruise_set = 0 < v_cruise < 255 + + if not is_cruise_set: + return None + + return { + 'training': plan.cscTraining, + 'active': plan.cscControllingSpeed, + 'curvature': plan.roadCurvature, + } + + +def _intensity(curvature: float) -> float: + if abs(curvature) < _CURVATURE_MIN: + return _GLOW_BASE_INTENSITY + curve = min(1.0, (abs(curvature) - _CURVATURE_MIN) / (_CURVATURE_MAX - _CURVATURE_MIN)) + return _GLOW_BASE_INTENSITY + (1.0 - _GLOW_BASE_INTENSITY) * curve + + +def _glow_period(intensity: float) -> float: + t = max(0.0, min(1.0, (intensity - _GLOW_BASE_INTENSITY) / (1.0 - _GLOW_BASE_INTENSITY))) + return _BREATHING_PERIOD / (1.0 + 2.0 * t) + + +def _lerp_color(a: rl.Color, b: rl.Color, t: float) -> rl.Color: + t = max(0.0, min(1.0, t)) + return rl.Color( + a.r + int((b.r - a.r) * t), + a.g + int((b.g - a.g) * t), + a.b + int((b.b - a.b) * t), + 255) + + +def _glow_color(intensity: float) -> rl.Color: + t = max(0.0, min(1.0, (intensity - _GLOW_BASE_INTENSITY) / (1.0 - _GLOW_BASE_INTENSITY))) + if t < 0.5: + return _lerp_color(_GREEN, _AMBER, t / 0.5) + elif t < 0.6: + return _lerp_color(_AMBER, _ORANGE, (t - 0.5) / 0.1) + else: + return _lerp_color(_ORANGE, _RED, (t - 0.6) / 0.4) + + +def _render_csc_glow(border_rect: rl.Rectangle, border_width: float = UI_BORDER_SIZE): + global _last_was_active, _activation_start, _fade_out_start, _last_state + + state = _csc_state() + now = rl.get_time() + border_color = get_screen_edge_color(ui_state) + + if state is None or not state['active']: + if _last_was_active: + _fade_out_start = now + _last_was_active = False + if _last_state is None: + return + fade_out = max(0.0, 1.0 - (now - _fade_out_start) / _GLOW_FADE_IN_DURATION) + if fade_out <= 0: + _last_state = None + return + intensity, period, color, t_norm = _last_state + fade = fade_out + else: + if not _last_was_active: + _activation_start = now + _fade_out_start = 0.0 + _last_was_active = True + + intensity = _intensity(state['curvature']) + period = _glow_period(intensity) + color = _glow_color(intensity) + t_norm = max(0.0, min(1.0, (intensity - _GLOW_BASE_INTENSITY) / (1.0 - _GLOW_BASE_INTENSITY))) + _last_state = (intensity, period, color, t_norm) + fade = min(1.0, (now - _activation_start) / _GLOW_FADE_IN_DURATION) + + elapsed = now - _activation_start + phase = (elapsed % period) / period + amplitude = 0.3 + 0.7 * t_norm + breath = max(0.0, min(1.0, 0.5 + amplitude * math.sin(phase * 2 * math.pi))) + base_alpha = max(_GLOW_MIN_ALPHA * fade, _GLOW_MAX_ALPHA * intensity * breath * fade) + + step = border_width * _GLOW_SPAN / (_GLOW_LAYERS - 1) + base_inset = border_width * 0.5 + + for i in range(_GLOW_LAYERS): + inset = base_inset + i * step + t = i / (_GLOW_LAYERS - 1) + a = int(base_alpha * (1.0 - t) * (1.0 - t)) + if a < 3: + continue + + glow_rect = rl.Rectangle( + border_rect.x + inset, + border_rect.y + inset, + border_rect.width - 2 * inset, + border_rect.height - 2 * inset, + ) + if t < 0.4: + layer_color = border_color + else: + blend_factor = (t - 0.4) / 0.6 + layer_color = _lerp_color(border_color, color, blend_factor) + rl.draw_rectangle_rounded_lines_ex(glow_rect, 0.12, 10, int(border_width * 1.5), rl.Color(layer_color.r, layer_color.g, layer_color.b, a)) + + +_effects: list[BorderEffect] = [ + BorderEffect(_render_csc_glow, BorderLayer.BEHIND, "csc_glow"), +] + + +def render_behind(border_rect: rl.Rectangle, border_width: float): + for effect in _effects: + if effect.layer == BorderLayer.BEHIND: + effect.render_fn(border_rect, border_width) + + +def render_overlay(border_rect: rl.Rectangle, border_width: float): + for effect in _effects: + if effect.layer == BorderLayer.OVERLAY: + effect.render_fn(border_rect, border_width) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 152ef9120e..f1300ecf24 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -3,7 +3,7 @@ import time from msgq.visionipc import VisionStreamType from openpilot.common.params import Params from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView -from openpilot.selfdrive.ui.onroad.starpilot.curve_speed_border import render_glow, render_filament +from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import render_behind, render_overlay from openpilot.selfdrive.ui.onroad.starpilot.path import render_adjacent_paths, render_blind_spot_path, render_path_edges from openpilot.selfdrive.ui.onroad.starpilot.personality_button import PersonalityButton, BTN_SIZE from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import ( @@ -11,7 +11,10 @@ from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import ( SET_SPEED_WIDTH_IMP, SET_SPEED_WIDTH_MET, SET_SPEED_HEIGHT, SIGN_MARGIN, ) from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.lib.starpilot_status import get_screen_edge_color +from openpilot.selfdrive.ui.lib.starpilot_status import ( + get_screen_edge_color, CEM_OVERRIDE_COLOR, ENGAGED_COLOR, + EXPERIMENTAL_COLOR, TRAFFIC_COLOR, +) from openpilot.system.ui.lib.application import MousePos, gui_app, FontWeight from openpilot.system.ui.lib.text_measure import measure_text_cached @@ -25,9 +28,13 @@ class StarPilotOnroadView(AugmentedRoadView): self._font_bold = gui_app.font(FontWeight.BOLD) self._font_medium = gui_app.font(FontWeight.MEDIUM) self._standstill_started_at = 0.0 + self._smoothed_steer = 0.0 def _render(self, rect: rl.Rectangle): self._position_personality_button() + border_width = self._get_border_width() + border_color = get_screen_edge_color(ui_state) + rl.draw_rectangle_rounded(rect, 0.12, 10, border_color) super()._render(rect) if not ui_state.started: @@ -166,7 +173,6 @@ class StarPilotOnroadView(AugmentedRoadView): minute_size = measure_text_cached(self._font_bold, minute_text, 176) second_size = measure_text_cached(self._font_medium, second_text, 66) - from openpilot.selfdrive.ui.lib.starpilot_status import ENGAGED_COLOR, EXPERIMENTAL_COLOR, TRAFFIC_COLOR import numpy as np def blend_colors(start: rl.Color, end: rl.Color, transition: float) -> rl.Color: @@ -207,21 +213,14 @@ class StarPilotOnroadView(AugmentedRoadView): def _draw_border(self, rect: rl.Rectangle): border_width = self._get_border_width() - rl.draw_rectangle_lines_ex(rect, border_width, rl.BLACK) + rl.draw_rectangle_rounded_lines_ex(rect, 0.12, 10, border_width, rl.BLACK) border_rect = rl.Rectangle(rect.x + border_width, rect.y + border_width, rect.width - 2 * border_width, rect.height - 2 * border_width) - # Layer 3: Amber glow (behind standard border) - render_glow(border_rect, border_width) + render_behind(border_rect, border_width) - # Layer 4: Standard border - border_color = get_screen_edge_color(ui_state) - rl.draw_rectangle_rounded_lines_ex(border_rect, 0.12, 10, border_width, border_color) + render_overlay(border_rect, border_width) - # Layer 5: Amber filament (on top of standard border) - render_filament(border_rect, border_width) - - # Layer 6: Turn Signal, Blind Spot, and Steering Torque Borders (Phase 6) self._render_border_effects(rect) def _handle_mouse_press(self, mouse_pos: MousePos): @@ -330,8 +329,6 @@ class StarPilotOnroadView(AugmentedRoadView): interval = 250 if show_blindspot and (left_blindspot or right_blindspot) else 500 flicker_active = (int(rl.get_time() * 1000) % (interval * 2)) < interval - from openpilot.selfdrive.ui.lib.starpilot_status import TRAFFIC_COLOR, CEM_OVERRIDE_COLOR - def get_half_border_color(blindspot, turn_signal): if turn_signal and show_signal: if blindspot: @@ -363,9 +360,6 @@ class StarPilotOnroadView(AugmentedRoadView): torque = -car_control.actuators.torque abs_torque = abs(torque) - if not hasattr(self, "_smoothed_steer"): - self._smoothed_steer = 0.0 - self._smoothed_steer = 0.25 * abs_torque + 0.75 * self._smoothed_steer if abs(self._smoothed_steer - abs_torque) < 0.01: self._smoothed_steer = abs_torque @@ -374,8 +368,6 @@ class StarPilotOnroadView(AugmentedRoadView): x_pos = int(rect.x) if torque < 0 else int(rect.x + rect.width - border_width) y_pos = int(rect.y + rect.height - visible_height) - from openpilot.selfdrive.ui.lib.starpilot_status import TRAFFIC_COLOR, EXPERIMENTAL_COLOR, CEM_OVERRIDE_COLOR, ENGAGED_COLOR - if self._smoothed_steer < 0.25: t = self._smoothed_steer / 0.25 col = rl.color_alpha_blend(ENGAGED_COLOR, CEM_OVERRIDE_COLOR, rl.Color(255, 255, 255, int(t * 255)))