sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed 2026-07-21 07:44:02 +00:00
commit de77c89cf6
3995 files changed
+1260688

No files matched your search

@@ -0,0 +1,12 @@
import pyray as rl
SIDE_PANEL_WIDTH = 60
def blend_colors(a: rl.Color, b: rl.Color, f: float) -> rl.Color:
h0, s0, v0 = (hsv0 := rl.color_to_hsv(a)).x, hsv0.y, hsv0.z
h1, s1, v1 = (hsv1 := rl.color_to_hsv(b)).x, hsv1.y, hsv1.z
dh = ((h1 - h0 + 180) % 360) - 180 # shortest hue delta
return rl.color_from_hsv((h0 + f * dh) % 360,
s0 + f * (s1 - s0),
v0 + f * (v1 - v0))
@@ -0,0 +1,372 @@
import time
from enum import StrEnum
from typing import NamedTuple
import pyray as rl
import random
import string
from dataclasses import dataclass
from openpilot.cereal import messaging, log
from opendbc.car.structs import car
from openpilot.selfdrive.ui.ui_state import ui_state
from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter
from openpilot.common.hardware import TICI
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.widgets import Widget
from openpilot.system.ui.widgets.label import UnifiedLabel
from openpilot.selfdrive.ui.sunnypilot.onroad.speed_limit import SpeedLimitAlertRenderer
AlertSize = log.SelfdriveState.AlertSize
AlertStatus = log.SelfdriveState.AlertStatus
ALERT_MARGIN = 18
ALERT_FONT_SMALL = 66 - 50
ALERT_FONT_BIG = 88 - 40
SELFDRIVE_STATE_TIMEOUT = 5 # Seconds
SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds
# Constants
ALERT_COLORS = {
AlertStatus.normal: rl.Color(0, 0, 0, 255),
AlertStatus.userPrompt: rl.Color(255, 115, 0, 255),
AlertStatus.critical: rl.Color(255, 0, 21, 255),
}
TURN_SIGNAL_BLINK_PERIOD = 1 / (80 / 60) # Mazda heartbeat turn signal BPM
DEBUG = False
class IconSide(StrEnum):
left = 'left'
right = 'right'
class IconLayout(NamedTuple):
texture: rl.Texture
side: IconSide
margin_x: int
margin_y: int
alpha: float = 255.0
class AlertLayout(NamedTuple):
text_rect: rl.Rectangle
icon: IconLayout | None
@dataclass
class Alert:
text1: str = ""
text2: str = ""
size: int = 0
status: int = 0
visual_alert: int = car.CarControl.HUDControl.VisualAlert.none
alert_type: str = ""
# Pre-defined alert instances
ALERT_STARTUP_PENDING = Alert(
text1="sunnypilot Unavailable",
text2="Waiting to start",
size=AlertSize.mid,
status=AlertStatus.normal,
)
ALERT_CRITICAL_TIMEOUT = Alert(
text1="TAKE CONTROL IMMEDIATELY",
text2="System Unresponsive",
size=AlertSize.full,
status=AlertStatus.critical,
)
ALERT_CRITICAL_REBOOT = Alert(
text1="System Unresponsive",
text2="Reboot Device",
size=AlertSize.full,
status=AlertStatus.critical,
)
class AlertRenderer(Widget, SpeedLimitAlertRenderer):
def __init__(self):
Widget.__init__(self)
SpeedLimitAlertRenderer.__init__(self)
self._alert_text1_label = UnifiedLabel(text="", font_size=ALERT_FONT_BIG, font_weight=FontWeight.DISPLAY, line_height=0.86,
letter_spacing=-0.02)
self._alert_text2_label = UnifiedLabel(text="", font_size=ALERT_FONT_SMALL, font_weight=FontWeight.ROMAN, line_height=0.86,
letter_spacing=0.025)
self._prev_alert: Alert | None = None
self._text_gen_time = 0
self._alert_text2_gen = ''
# animation filters
# TODO: use 0.1 but with proper alert height calculation
self._alert_y_filter = BounceFilter(0, 0.1, 1 / gui_app.target_fps)
self._alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
self._turn_signal_timer = 0.0
self._turn_signal_alpha_filter = FirstOrderFilter(0.0, 0.3, 1 / gui_app.target_fps)
self._last_icon_side: IconSide | None = None
self._load_icons()
def _load_icons(self):
self._txt_turn_signal_left = gui_app.texture('icons_mici/onroad/turn_signal_left.png', 104, 96)
self._txt_turn_signal_right = gui_app.texture('icons_mici/onroad/turn_signal_left.png', 104, 96, flip_x=True)
self._txt_blind_spot_left = gui_app.texture('icons_mici/onroad/blind_spot_left.png', 134, 150)
self._txt_blind_spot_right = gui_app.texture('icons_mici/onroad/blind_spot_left.png', 134, 150, flip_x=True)
def get_alert(self, sm: messaging.SubMaster) -> Alert | None:
"""Generate the current alert based on selfdrive state."""
ss = sm['selfdriveState']
# Check if selfdriveState messages have stopped arriving
if not sm.updated['selfdriveState']:
recv_frame = sm.recv_frame['selfdriveState']
time_since_onroad = time.monotonic() - ui_state.started_time
# 1. Never received selfdriveState since going onroad
waiting_for_startup = recv_frame < ui_state.started_frame
if waiting_for_startup and time_since_onroad > 5:
return ALERT_STARTUP_PENDING
# 2. Lost communication with selfdriveState after receiving it
if TICI and not waiting_for_startup:
ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
if ss_missing > SELFDRIVE_STATE_TIMEOUT:
if ss.enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < SELFDRIVE_UNRESPONSIVE_TIMEOUT:
return ALERT_CRITICAL_TIMEOUT
return ALERT_CRITICAL_REBOOT
# No alert if size is none
if ss.alertSize == 0:
return None
# Return current alert
ret = Alert(text1=ss.alertText1, text2=ss.alertText2, size=ss.alertSize.raw, status=ss.alertStatus.raw,
visual_alert=ss.alertHudVisual, alert_type=ss.alertType)
self._prev_alert = ret
return ret
def will_render(self) -> tuple[Alert | None, bool]:
alert = self.get_alert(ui_state.sm)
return alert or self._prev_alert, alert is None
def _icon_helper(self, alert: Alert) -> AlertLayout:
icon_side = None
txt_icon = None
icon_alpha = 255.0
icon_margin_x = 20
icon_margin_y = 18
# alert_type format is "EventName/eventType" (e.g., "preLaneChangeLeft/warning")
event_name = alert.alert_type.split('/')[0] if alert.alert_type else ''
if event_name == 'preLaneChangeLeft':
icon_side = IconSide.left
txt_icon = self._txt_turn_signal_left
icon_margin_x = 2
icon_margin_y = 5
elif event_name == 'preLaneChangeRight':
icon_side = IconSide.right
txt_icon = self._txt_turn_signal_right
icon_margin_x = 2
icon_margin_y = 5
elif event_name == 'laneChange':
icon_side = self._last_icon_side
txt_icon = self._txt_turn_signal_left if self._last_icon_side == 'left' else self._txt_turn_signal_right
icon_margin_x = 2
icon_margin_y = 5
elif event_name == 'laneChangeBlocked':
CS = ui_state.sm['carState']
if CS.leftBlinker:
icon_side = IconSide.left
elif CS.rightBlinker:
icon_side = IconSide.right
else:
icon_side = self._last_icon_side
txt_icon = self._txt_blind_spot_left if icon_side == 'left' else self._txt_blind_spot_right
icon_margin_x = 8
icon_margin_y = 0
elif event_name == 'speedLimitPreActive':
icon_side, txt_icon, icon_alpha, icon_margin_x, icon_margin_y = SpeedLimitAlertRenderer.speed_limit_pre_active_icon_helper(self)
else:
self._turn_signal_timer = 0.0
self._last_icon_side = icon_side
# create text rect based on icon presence
text_x = self._rect.x + ALERT_MARGIN
text_width = self._rect.width - ALERT_MARGIN
if icon_side == 'left':
text_x = self._rect.x + self._txt_turn_signal_right.width
text_width = self._rect.width - ALERT_MARGIN - self._txt_turn_signal_right.width
elif icon_side == 'right':
text_x = self._rect.x + ALERT_MARGIN
text_width = self._rect.width - ALERT_MARGIN - self._txt_turn_signal_right.width
text_rect = rl.Rectangle(
text_x,
self._alert_y_filter.x,
text_width,
self._rect.height,
)
icon_layout = IconLayout(txt_icon, icon_side, icon_margin_x, icon_margin_y, icon_alpha) if txt_icon is not None and icon_side is not None else None
return AlertLayout(text_rect, icon_layout)
def _render(self, rect: rl.Rectangle) -> bool:
alert = self.get_alert(ui_state.sm)
# Animate fade and slide in/out
self._alert_y_filter.update(self._rect.y - 50 if alert is None else self._rect.y)
self._alpha_filter.update(0 if alert is None else 1)
if gui_app.sunnypilot_ui():
ui_state.onroad_brightness_handle_alerts(ui_state, alert)
if alert is None:
# If still animating out, keep the previous alert
if self._alpha_filter.x > 0.01 and self._prev_alert is not None:
alert = self._prev_alert
else:
self._prev_alert = None
return False
self._draw_background(alert)
# update speed limit UI states
SpeedLimitAlertRenderer.update(self)
alert_layout = self._icon_helper(alert)
self._draw_text(alert, alert_layout)
self._draw_icons(alert_layout)
return True
def _draw_icons(self, alert_layout: AlertLayout) -> None:
if alert_layout.icon is None:
return
if time.monotonic() - self._turn_signal_timer > TURN_SIGNAL_BLINK_PERIOD:
self._turn_signal_timer = time.monotonic()
self._turn_signal_alpha_filter.x = 255 * 2
else:
self._turn_signal_alpha_filter.update(255 * 0.2)
if alert_layout.icon.side == 'left':
pos_x = int(self._rect.x + alert_layout.icon.margin_x)
else:
pos_x = int(self._rect.x + self._rect.width - alert_layout.icon.margin_x - alert_layout.icon.texture.width)
if alert_layout.icon.texture not in (self._txt_turn_signal_left, self._txt_turn_signal_right):
icon_alpha = alert_layout.icon.alpha
else:
icon_alpha = int(min(self._turn_signal_alpha_filter.x, 255))
rl.draw_texture_ex(alert_layout.icon.texture, rl.Vector2(pos_x, self._rect.y + alert_layout.icon.margin_y), 0.0, 1.0,
rl.Color(255, 255, 255, int(icon_alpha * self._alpha_filter.x)))
def _draw_background(self, alert: Alert) -> None:
# draw top gradient for alert text at top
color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x))
translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x))
small_alert_height = round(self._rect.height * 0.583) # 140px at mici height
medium_alert_height = round(self._rect.height * 0.833) # 200px at mici height
# alert_type format is "EventName/eventType" (e.g., "preLaneChangeLeft/warning")
event_name = alert.alert_type.split('/')[0] if alert.alert_type else ''
if event_name == 'preLaneChangeLeft':
bg_height = small_alert_height
elif event_name == 'preLaneChangeRight':
bg_height = small_alert_height
elif event_name == 'laneChange':
bg_height = small_alert_height
elif event_name == 'laneChangeBlocked':
bg_height = medium_alert_height
else:
bg_height = int(self._rect.height)
solid_height = round(bg_height * 0.2)
rl.draw_rectangle(int(self._rect.x), int(self._rect.y), int(self._rect.width), solid_height, color)
rl.draw_rectangle_gradient_v(int(self._rect.x), int(self._rect.y + solid_height), int(self._rect.width),
int(bg_height - solid_height),
color, translucent_color)
def _draw_text(self, alert: Alert, alert_layout: AlertLayout) -> None:
icon_side = alert_layout.icon.side if alert_layout.icon is not None else None
# TODO: hack
alert_text1 = alert.text1.lower().replace('calibrating: ', 'calibrating:\n')
can_draw_second_line = False
# TODO: there should be a common way to determine font size based on text length to maximize rect
if len(alert_text1) <= 12:
can_draw_second_line = True
font_size = 92 - 10
elif len(alert_text1) <= 16:
can_draw_second_line = True
font_size = 70
else:
font_size = 64 - 10
if icon_side is not None:
font_size -= 10
color = rl.Color(255, 255, 255, int(255 * 0.9 * self._alpha_filter.x))
text1_y_offset = 11 if font_size >= 70 else 4
text_rect1 = rl.Rectangle(
alert_layout.text_rect.x,
alert_layout.text_rect.y - text1_y_offset,
alert_layout.text_rect.width,
alert_layout.text_rect.height,
)
self._alert_text1_label.set_text(alert_text1)
self._alert_text1_label.set_text_color(color)
self._alert_text1_label.set_font_size(font_size)
self._alert_text1_label.set_alignment(rl.GuiTextAlignment.TEXT_ALIGN_LEFT if icon_side != 'left' else rl.GuiTextAlignment.TEXT_ALIGN_RIGHT)
self._alert_text1_label.render(text_rect1)
alert_text2 = alert.text2.lower()
# randomize chars and length for testing
if DEBUG:
if time.monotonic() - self._text_gen_time > 0.5:
self._alert_text2_gen = ''.join(random.choices(string.ascii_lowercase + ' ', k=random.randint(0, 40)))
self._text_gen_time = time.monotonic()
alert_text2 = self._alert_text2_gen or alert_text2
if can_draw_second_line and alert_text2:
last_line_h = self._alert_text1_label.rect.y + self._alert_text1_label.get_content_height(int(alert_layout.text_rect.width))
last_line_h -= 4
if len(alert_text2) > 18:
small_font_size = 36
elif len(alert_text2) > 24:
small_font_size = 32
else:
small_font_size = 40
text_rect2 = rl.Rectangle(
alert_layout.text_rect.x,
last_line_h,
alert_layout.text_rect.width,
alert_layout.text_rect.height - last_line_h
)
color = rl.Color(255, 255, 255, int(255 * 0.65 * self._alpha_filter.x))
self._alert_text2_label.set_text(alert_text2)
self._alert_text2_label.set_text_color(color)
self._alert_text2_label.set_font_size(small_font_size)
self._alert_text2_label.set_alignment(rl.GuiTextAlignment.TEXT_ALIGN_LEFT if icon_side != 'left' else rl.GuiTextAlignment.TEXT_ALIGN_RIGHT)
self._alert_text2_label.render(text_rect2)
@@ -0,0 +1,379 @@
import numpy as np
import pyray as rl
from openpilot.cereal import log
from opendbc.car.structs import car
from msgq.visionipc import VisionStreamType
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer
from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer
from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent
from openpilot.system.ui.widgets.label import UnifiedLabel
from openpilot.system.ui.widgets import Widget
from openpilot.common.filter_simple import BounceFilter
from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame
from openpilot.common.transformations.orientation import rot_from_euler
from enum import IntEnum
if gui_app.sunnypilot_ui():
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
OpState = log.SelfdriveState.OpenpilotState
CALIBRATED = log.LiveCalibrationData.Status.calibrated
ROAD_CAM = VisionStreamType.VISION_STREAM_ROAD
WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD
DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"]
class BookmarkState(IntEnum):
HIDDEN = 0
DRAGGING = 1
TRIGGERED = 2
WIDE_CAM_MAX_SPEED = 5.0 # m/s (10 mph)
ROAD_CAM_MIN_SPEED = 10 # m/s (25 mph)
CAM_Y_OFFSET = 20
class BookmarkIcon(Widget):
PEEK_THRESHOLD = 50 # If icon peeks out this much, snap it fully visible
FULL_VISIBLE_OFFSET = 200 # How far onscreen when fully visible
HIDDEN_OFFSET = -50 # How far offscreen when hidden
def __init__(self, bookmark_callback):
super().__init__()
self._bookmark_callback = bookmark_callback
self._icon = gui_app.texture("icons_mici/onroad/bookmark.png", 180, 180)
self._offset_filter = BounceFilter(0.0, 0.1, 1 / gui_app.target_fps)
# State
self._interacting = False
self._state = BookmarkState.HIDDEN
self._swipe_start_x = 0.0
self._swipe_current_x = 0.0
self._is_swiping = False
self._is_swiping_left: bool = False
self._triggered_time: float = 0.0
def is_swiping_left(self) -> bool:
"""Check if currently swiping left (for scroller to disable)."""
return self._is_swiping_left
def interacting(self):
interacting, self._interacting = self._interacting, False
return interacting
def _update_state(self):
if self._state == BookmarkState.DRAGGING:
# Allow pulling past activated position with rubber band effect
swipe_offset = self._swipe_start_x - self._swipe_current_x
swipe_offset = min(swipe_offset, self.FULL_VISIBLE_OFFSET + 50)
self._offset_filter.update(swipe_offset)
elif self._state == BookmarkState.TRIGGERED:
# Continue animating to fully visible
self._offset_filter.update(self.FULL_VISIBLE_OFFSET)
# Stay in TRIGGERED state for 1 second
if rl.get_time() - self._triggered_time >= 1.5:
self._state = BookmarkState.HIDDEN
elif self._state == BookmarkState.HIDDEN:
self._offset_filter.update(self.HIDDEN_OFFSET)
if self._offset_filter.x < 1e-3:
self._interacting = False
def _handle_mouse_event(self, mouse_event: MouseEvent):
if not ui_state.started:
return
if mouse_event.left_pressed:
# Store relative position within widget
self._swipe_start_x = mouse_event.pos.x
self._swipe_current_x = mouse_event.pos.x
self._is_swiping = True
self._is_swiping_left = False
self._state = BookmarkState.DRAGGING
elif mouse_event.left_down and self._is_swiping:
self._swipe_current_x = mouse_event.pos.x
swipe_offset = self._swipe_start_x - self._swipe_current_x
self._is_swiping_left = swipe_offset > 0
if self._is_swiping_left:
self._interacting = True
elif mouse_event.left_released:
if self._is_swiping:
swipe_distance = self._swipe_start_x - self._swipe_current_x
# If peeking past threshold, transition to animating to fully visible and bookmark
if swipe_distance > self.PEEK_THRESHOLD:
self._state = BookmarkState.TRIGGERED
self._triggered_time = rl.get_time()
self._bookmark_callback()
else:
# Otherwise, transition back to hidden
self._state = BookmarkState.HIDDEN
# Reset swipe state
self._is_swiping = False
self._is_swiping_left = False
def _render(self, _):
"""Render the bookmark icon."""
if self._offset_filter.x > 0:
icon_x = self.rect.x + self.rect.width - round(self._offset_filter.x)
icon_y = self.rect.y + (self.rect.height - self._icon.height) / 2 # Vertically centered
rl.draw_texture_ex(self._icon, rl.Vector2(icon_x, icon_y), 0.0, 1.0, rl.WHITE)
class AugmentedRoadView(CameraView):
def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
super().__init__("camerad", stream_type)
self._bookmark_callback = bookmark_callback
self._set_placeholder_color(rl.BLACK)
self.device_camera: DeviceCameraConfig | None = None
self.view_from_calib = view_frame_from_device_frame.copy()
self.view_from_wide_calib = view_frame_from_device_frame.copy()
self._matrix_cache_key: tuple | None = None
self._cached_matrix: np.ndarray | None = None
self._content_rect = rl.Rectangle()
self._last_click_time = 0.0
# Bookmark icon with swipe gesture
self._bookmark_icon = BookmarkIcon(bookmark_callback)
self._model_renderer = ModelRenderer()
self._hud_renderer = HudRenderer()
self._alert_renderer = AlertRenderer()
self._driver_state_renderer = DriverStateRenderer()
self._confidence_ball = ConfidenceBall()
self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE)
self._fade_texture = gui_app.texture("icons_mici/onroad/onroad_fade.png")
def is_swiping_left(self) -> bool:
"""Check if currently swiping left (for scroller to disable)."""
return self._bookmark_icon.is_swiping_left()
def _update_state(self):
super()._update_state()
# update offroad label
if ui_state.panda_type == log.PandaState.PandaType.unknown:
self._offroad_label.set_text("system booting")
elif ui_state.ignition and not ui_state.started:
self._offroad_label.set_text("openpilot can't start\ncheck alerts")
else:
self._offroad_label.set_text("start the car to\nuse sunnypilot")
def _handle_mouse_release(self, mouse_pos: MousePos):
# Don't trigger click callback if bookmark was triggered
if not self._bookmark_icon.interacting():
super()._handle_mouse_release(mouse_pos)
def _render(self, _):
# Draw text if not onroad
if not ui_state.started:
rl.draw_rectangle_rec(self.rect, rl.BLACK)
self._offroad_label.render(self._rect)
return
self._switch_stream_if_needed(ui_state.sm)
# Update calibration before rendering
self._update_calibration()
# Create inner content area with border padding
self._content_rect = rl.Rectangle(
self.rect.x,
self.rect.y,
self.rect.width - SIDE_PANEL_WIDTH,
self.rect.height,
)
# Enable scissor mode to clip all rendering within content rectangle boundaries
# This creates a rendering viewport that prevents graphics from drawing outside the border
rl.begin_scissor_mode(
int(self._content_rect.x),
int(self._content_rect.y),
int(self._content_rect.width),
int(self._content_rect.height)
)
# Render the base camera view
super()._render(self._content_rect)
# Draw all UI overlays
self._model_renderer.render(self._content_rect)
# Fade out bottom of overlays for looks
rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, rl.WHITE)
alert_to_render, not_animating_out = self._alert_renderer.will_render()
# Hide DMoji when disengaged unless AlwaysOnDM is enabled
should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
(ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
self._driver_state_renderer.set_should_draw(should_draw_dmoji)
self._driver_state_renderer.set_position(self._rect.x + 16, self._rect.y + 10)
self._driver_state_renderer.render()
self._hud_renderer.set_can_draw_top_icons(alert_to_render is None)
self._hud_renderer.set_wheel_critical_icon(alert_to_render is not None and not not_animating_out and
alert_to_render.visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired)
self._alert_renderer.render(self._content_rect)
self._hud_renderer.render(self._content_rect)
# Draw fake rounded border
rl.draw_rectangle_rounded_lines_ex(self._content_rect, 0.2 * 1.02, 10, 50, rl.BLACK)
# End clipping region
rl.end_scissor_mode()
# Custom UI extension point - add custom overlays here
# Use self._content_rect for positioning within camera bounds
self._confidence_ball.render(self.rect)
self._bookmark_icon.render(self.rect)
def _switch_stream_if_needed(self, sm):
if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
v_ego = sm['carState'].vEgo
if v_ego < WIDE_CAM_MAX_SPEED:
target = WIDE_CAM
elif v_ego > ROAD_CAM_MIN_SPEED:
target = ROAD_CAM
else:
# Hysteresis zone - keep current stream
target = self.stream_type
else:
target = ROAD_CAM
if self.stream_type != target:
self.switch_stream(target)
def _update_calibration(self):
# Update device camera if not already set
sm = ui_state.sm
if not self.device_camera and sm.seen['roadCameraState'] and sm.seen['deviceState']:
self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
# Check if live calibration data is available and valid
if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']):
return
calib = sm['liveCalibration']
if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED:
return
# Update view_from_calib matrix
device_from_calib = rot_from_euler(calib.rpyCalib)
self.view_from_calib = view_frame_from_device_frame @ device_from_calib
# Update wide calibration if available
if hasattr(calib, 'wideFromDeviceEuler') and len(calib.wideFromDeviceEuler) == 3:
wide_from_device = rot_from_euler(calib.wideFromDeviceEuler)
self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
cache_key = (
ui_state.sm.recv_frame['liveCalibration'],
int(self._content_rect.width),
int(self._content_rect.height),
self.stream_type,
round(ui_state.sm['carState'].vEgo, 1),
)
if cache_key == self._matrix_cache_key and self._cached_matrix is not None:
return self._cached_matrix
# Get camera configuration
device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA
is_wide_camera = self.stream_type == WIDE_CAM
intrinsic = device_camera.ecam.intrinsics if is_wide_camera else device_camera.fcam.intrinsics
calibration = self.view_from_wide_calib if is_wide_camera else self.view_from_calib
if is_wide_camera:
zoom = 0.7 * 1.5
else:
zoom = np.interp(ui_state.sm['carState'].vEgo, [10, 30], [0.8, 1.0])
# Calculate transforms for vanishing point
inf_point = np.array([1000.0, 0.0, 0.0])
calib_transform = intrinsic @ calibration
kep = calib_transform @ inf_point
# Calculate center points and dimensions
w, h = self._content_rect.width, self._content_rect.height
cx, cy = intrinsic[0, 2], intrinsic[1, 2]
# Ensure zoom views the whole area
zoom = max(zoom, w / (2 * cx), h / (2 * cy))
# Calculate max allowed offsets with margins
margin = 5
max_x_offset = max(0.0, cx * zoom - w / 2 - margin)
max_y_offset = max(0.0, cy * zoom - h / 2 - margin)
# Calculate and clamp offsets to prevent out-of-bounds issues
try:
if abs(kep[2]) > 1e-6:
x_offset = np.clip((kep[0] / kep[2] - cx) * zoom, -max_x_offset, max_x_offset)
y_offset = np.clip((kep[1] / kep[2] - cy) * zoom + CAM_Y_OFFSET, -max_y_offset, max_y_offset)
else:
x_offset, y_offset = 0, 0
except (ZeroDivisionError, OverflowError):
x_offset, y_offset = 0, 0
# Cache the computed transformation matrix to avoid recalculations
self._matrix_cache_key = cache_key
self._cached_matrix = np.array([
[zoom * 2 * cx / w, 0, -x_offset / w * 2],
[0, zoom * 2 * cy / h, -y_offset / h * 2],
[0, 0, 1.0]
])
# built without rect.x/y so cache stays hot during scroll. ModelRenderer adds offset at draw time
video_transform = np.array([
[zoom, 0.0, (w / 2 - x_offset) - (cx * zoom)],
[0.0, zoom, (h / 2 - y_offset) - (cy * zoom)],
[0.0, 0.0, 1.0]
])
self._model_renderer.set_transform(video_transform @ calib_transform)
return self._cached_matrix
def show_event(self):
if gui_app.sunnypilot_ui():
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)
def hide_event(self):
if gui_app.sunnypilot_ui():
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)
if __name__ == "__main__":
gui_app.init_window("OnRoad Camera View")
road_camera_view = AugmentedRoadView(lambda: None, stream_type=ROAD_CAM)
print("***press space to switch camera view***")
try:
for _ in gui_app.render():
ui_state.update()
if rl.is_key_released(rl.KeyboardKey.KEY_SPACE):
if WIDE_CAM in road_camera_view.available_streams:
stream = ROAD_CAM if road_camera_view.stream_type == WIDE_CAM else WIDE_CAM
road_camera_view.switch_stream(stream)
road_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
finally:
road_camera_view.close()
@@ -0,0 +1,413 @@
import platform
import numpy as np
import pyray as rl
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
from openpilot.common.swaglog import cloudlog
from openpilot.common.hardware import TICI
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.egl import init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, EGLImage
from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts
VERSION = """
#version 300 es
precision mediump float;
"""
if platform.system() == "Darwin":
VERSION = """
#version 330 core
"""
VERTEX_SHADER = VERSION + """
in vec3 vertexPosition;
in vec2 vertexTexCoord;
in vec3 vertexNormal;
in vec4 vertexColor;
uniform mat4 mvp;
out vec2 fragTexCoord;
out vec4 fragColor;
void main() {
fragTexCoord = vertexTexCoord;
fragColor = vertexColor;
gl_Position = mvp * vec4(vertexPosition, 1.0);
}
"""
# Choose fragment shader based on platform capabilities
if TICI:
FRAME_FRAGMENT_SHADER = """
#version 300 es
#extension GL_OES_EGL_image_external_essl3 : enable
precision mediump float;
in vec2 fragTexCoord;
uniform samplerExternalOES texture0;
out vec4 fragColor;
uniform int engaged;
uniform int enhance_driver;
void main() {
vec4 color = texture(texture0, fragTexCoord);
if (engaged == 1) {
float gray = dot(color.rgb, vec3(0.299, 0.587, 0.114)); // Luma
color.rgb = mix(vec3(gray), color.rgb, 0.2); // 20% saturation
color.rgb = clamp((color.rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0); // +20% contrast
color.rgb = pow(color.rgb, vec3(1.0/1.28));
fragColor = vec4(color.rgb, color.a);
} else {
color.rgb *= 0.85; // 85% opacity
}
if (enhance_driver == 1) {
float brightness = 1.1;
color.rgb = color.rgb + 0.15;
color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb);
color.rgb = pow(color.rgb, vec3(0.8));
}
fragColor = vec4(color.rgb, color.a);
}
"""
else:
FRAME_FRAGMENT_SHADER = VERSION + """
in vec2 fragTexCoord;
uniform sampler2D texture0;
uniform sampler2D texture1;
out vec4 fragColor;
uniform int engaged;
uniform int enhance_driver;
void main() {
float y = texture(texture0, fragTexCoord).r;
vec2 uv = texture(texture1, fragTexCoord).ra - 0.5;
vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x);
if (engaged == 1) {
float gray = dot(rgb, vec3(0.299, 0.587, 0.114));
rgb = mix(vec3(gray), rgb, 0.2); // 20% saturation
rgb = clamp((rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0); // +20% contrast
} else {
rgb *= 0.85; // 85% opacity
}
// TODO: the images out of camerad need some more correction and
// the ui should apply a gamma curve for the device display
if (enhance_driver == 1) {
float brightness = 1.1;
rgb = rgb + 0.15;
rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
rgb = rgb * rgb * (3.0 - 2.0 * rgb);
rgb = pow(rgb, vec3(0.8));
}
fragColor = vec4(rgb, 1.0);
}
"""
class CameraView(Widget):
def __init__(self, name: str, stream_type: VisionStreamType):
super().__init__()
self._name = name
# Primary stream
self.client = VisionIpcClient(name, stream_type, conflate=True)
self._stream_type = stream_type
self.available_streams: list[VisionStreamType] = []
# Target stream for switching
self._target_client: VisionIpcClient | None = None
self._target_stream_type: VisionStreamType | None = None
self._switching: bool = False
self._texture_needs_update = True
self.last_connection_attempt: float = 0.0
self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER)
self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not TICI else -1
self._engaged_loc = rl.get_shader_location(self.shader, "engaged")
self._engaged_val = rl.ffi.new("int[1]", [1])
self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver")
self._enhance_driver_val = rl.ffi.new("int[1]", [1 if stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0])
self.frame: VisionBuf | None = None
self.texture_y: rl.Texture | None = None
self.texture_uv: rl.Texture | None = None
# EGL resources
self.egl_images: dict[int, EGLImage] = {}
self.egl_texture: rl.Texture | None = None
self._placeholder_color: rl.Color | None = None
# Initialize EGL for zero-copy rendering on TICI
if TICI:
if not init_egl():
raise RuntimeError("Failed to initialize EGL")
# Create a 1x1 pixel placeholder texture for EGL image binding
temp_image = rl.gen_image_color(1, 1, rl.BLACK)
self.egl_texture = rl.load_texture_from_image(temp_image)
rl.unload_image(temp_image)
ui_state.add_offroad_transition_callback(self._offroad_transition)
def _offroad_transition(self):
# Drain queued SubSocket messages to prevent old frames from showing when going
# onroad. Qt had a separate thread which drains the VisionIpcClient SubSocket for us.
if self.client and self.client.is_connected():
while self.client.recv(timeout_ms=0) is not None:
pass
self.frame = None
def _set_placeholder_color(self, color: rl.Color):
"""Set a placeholder color to be drawn when no frame is available."""
self._placeholder_color = color
def switch_stream(self, stream_type: VisionStreamType) -> None:
if self._stream_type == stream_type:
return
if self._switching and self._target_stream_type == stream_type:
return
cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}')
if self._target_client:
del self._target_client
self._target_stream_type = stream_type
self._target_client = VisionIpcClient(self._name, stream_type, conflate=True)
self._switching = True
@property
def stream_type(self) -> VisionStreamType:
return self._stream_type
def close(self) -> None:
self._clear_textures()
# Clean up EGL texture
if TICI and self.egl_texture:
rl.unload_texture(self.egl_texture)
self.egl_texture = None
# Clean up shader
if self.shader and self.shader.id:
rl.unload_shader(self.shader)
self.shader.id = 0
self.frame = None
self.available_streams.clear()
self.client = None
def __del__(self):
self.close()
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
if not self.frame:
return np.eye(3)
# Calculate aspect ratios
widget_aspect_ratio = rect.width / rect.height
frame_aspect_ratio = self.frame.width / self.frame.height
# Calculate scaling factors to maintain aspect ratio
zx = min(frame_aspect_ratio / widget_aspect_ratio, 1.0)
zy = min(widget_aspect_ratio / frame_aspect_ratio, 1.0)
return np.array([
[zx, 0.0, 0.0],
[0.0, zy, 0.0],
[0.0, 0.0, 1.0]
])
def _render(self, rect: rl.Rectangle):
if self._switching:
self._handle_switch()
if not self._ensure_connection():
self._draw_placeholder(rect)
return
# Try to get a new buffer without blocking
buffer = self.client.recv(timeout_ms=0)
if buffer:
self._texture_needs_update = True
self.frame = buffer
elif not self.client.is_connected():
# ensure we clear the displayed frame when the connection is lost
self.frame = None
if not self.frame:
self._draw_placeholder(rect)
return
transform = self._calc_frame_matrix(rect)
src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height))
# Flip driver camera horizontally
if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER:
src_rect.width = -src_rect.width
# Calculate scale
scale_x = rect.width * transform[0, 0] # zx
scale_y = rect.height * transform[1, 1] # zy
# Calculate base position (centered)
x_offset = rect.x + (rect.width - scale_x) / 2
y_offset = rect.y + (rect.height - scale_y) / 2
x_offset += transform[0, 2] * rect.width / 2
y_offset += transform[1, 2] * rect.height / 2
dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y)
# Render with appropriate method
if TICI:
self._render_egl(src_rect, dst_rect)
else:
self._render_textures(src_rect, dst_rect)
def _draw_placeholder(self, rect: rl.Rectangle):
if self._placeholder_color:
rl.draw_rectangle_rec(rect, self._placeholder_color)
def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None:
"""Render using EGL for direct buffer access"""
if self.frame is None or self.egl_texture is None:
return
idx = self.frame.idx
egl_image = self.egl_images.get(idx)
# Create EGL image if needed
if egl_image is None:
egl_image = create_egl_image(self.frame.width, self.frame.height, self.frame.stride, self.frame.fd, self.frame.uv_offset)
if egl_image:
self.egl_images[idx] = egl_image
else:
return
# Update texture dimensions to match current frame
self.egl_texture.width = self.frame.width
self.egl_texture.height = self.frame.height
# Bind the EGL image to our texture
bind_egl_image_to_texture(self.egl_texture.id, egl_image)
# Render with shader
rl.begin_shader_mode(self.shader)
self._update_texture_color_filtering()
rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
rl.end_shader_mode()
def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None:
"""Render using texture copies"""
if not self.texture_y or not self.texture_uv or self.frame is None:
return
# Update textures with new frame data
if self._texture_needs_update:
y_data = self.frame.data[: self.frame.uv_offset]
uv_data = self.frame.data[self.frame.uv_offset:]
rl.update_texture(self.texture_y, rl.ffi.cast("void *", rl.ffi.from_buffer(y_data)))
rl.update_texture(self.texture_uv, rl.ffi.cast("void *", rl.ffi.from_buffer(uv_data)))
self._texture_needs_update = False
# Render with shader
rl.begin_shader_mode(self.shader)
self._update_texture_color_filtering()
rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv)
rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
rl.end_shader_mode()
def _update_texture_color_filtering(self):
self._engaged_val[0] = 1 if ui_state.status != UIStatus.DISENGAGED else 0
rl.set_shader_value(self.shader, self._engaged_loc, self._engaged_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT)
rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT)
def _ensure_connection(self) -> bool:
if not self.client.is_connected():
self.frame = None
self.available_streams.clear()
# Throttle connection attempts
current_time = rl.get_time()
if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL:
return False
self.last_connection_attempt = current_time
if not self.client.connect(False) or not self.client.num_buffers:
return False
cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}")
self._initialize_textures()
self.available_streams = self.client.available_streams(self._name, block=False)
return True
def _handle_switch(self) -> None:
"""Check if target stream is ready and switch immediately."""
if not self._target_client or not self._switching:
return
# Try to connect target if needed
if not self._target_client.is_connected():
if not self._target_client.connect(False) or not self._target_client.num_buffers:
return
cloudlog.debug(f"Target stream connected: {self._target_stream_type}")
# Check if target has frames ready
target_frame = self._target_client.recv(timeout_ms=0)
if target_frame:
self.frame = target_frame # Update current frame to target frame
self._complete_switch()
def _complete_switch(self) -> None:
"""Instantly switch to target stream."""
cloudlog.debug(f"Switching to {self._target_stream_type}")
# Clean up current resources
if self.client:
del self.client
# Switch to target
self.client = self._target_client
self._stream_type = self._target_stream_type
self._texture_needs_update = True
# Reset state
self._target_client = None
self._target_stream_type = None
self._switching = False
# Initialize textures for new stream
self._initialize_textures()
def _initialize_textures(self):
self._clear_textures()
if not TICI:
self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride),
int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE))
self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2),
int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA))
def _clear_textures(self):
if self.texture_y and self.texture_y.id:
rl.unload_texture(self.texture_y)
self.texture_y = None
if self.texture_uv and self.texture_uv.id:
rl.unload_texture(self.texture_uv)
self.texture_uv = None
# Clean up EGL resources
if TICI:
for data in self.egl_images.values():
destroy_egl_image(data)
self.egl_images = {}
if __name__ == "__main__":
gui_app.init_window("camera view")
road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD)
for _ in gui_app.render():
road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
@@ -0,0 +1,86 @@
import math
import pyray as rl
from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.system.ui.widgets import Widget
from openpilot.system.ui.lib.application import gui_app
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.confidence_ball import ConfidenceBallSP
def draw_circle_gradient(center_x: float, center_y: float, radius: int,
top: rl.Color, bottom: rl.Color) -> None:
# Draw a square with the gradient
rl.draw_rectangle_gradient_v(int(center_x - radius), int(center_y - radius),
radius * 2, radius * 2,
top, bottom)
# Paint over square with a ring
outer_radius = math.ceil(radius * math.sqrt(2)) + 1
rl.draw_ring(rl.Vector2(int(center_x), int(center_y)), radius, outer_radius,
0.0, 360.0,
20, rl.BLACK)
class ConfidenceBall(Widget, ConfidenceBallSP):
def __init__(self, demo: bool = False):
Widget.__init__(self)
ConfidenceBallSP.__init__(self)
self._demo = demo
self._confidence_filter = FirstOrderFilter(-0.5, 0.5, 1 / gui_app.target_fps)
def update_filter(self, value: float):
self._confidence_filter.update(value)
def _update_state(self):
if self._demo:
return
# animate status dot in from bottom
if ui_state.status == UIStatus.DISENGAGED:
self._confidence_filter.update(-0.5)
elif ui_state.status in (UIStatus.LAT_ONLY, UIStatus.LONG_ONLY):
self._confidence_filter.update(1 - max(self.get_animate_status_probs() or [1]))
else:
self._confidence_filter.update((1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs or [1])) *
(1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.steerOverrideProbs or [1])))
def _render(self, _):
content_rect = rl.Rectangle(
self.rect.x + self.rect.width - SIDE_PANEL_WIDTH,
self.rect.y,
SIDE_PANEL_WIDTH,
self.rect.height,
)
status_dot_radius = 24
dot_height = (1 - self._confidence_filter.x) * (content_rect.height - 2 * status_dot_radius) + status_dot_radius
dot_height = self._rect.y + dot_height
# confidence zones
if ui_state.status == UIStatus.ENGAGED or self._demo:
if self._confidence_filter.x > 0.5:
top_dot_color = rl.Color(0, 255, 204, 255)
bottom_dot_color = rl.Color(0, 255, 38, 255)
elif self._confidence_filter.x > 0.2:
top_dot_color = rl.Color(255, 200, 0, 255)
bottom_dot_color = rl.Color(255, 115, 0, 255)
else:
top_dot_color = rl.Color(255, 0, 21, 255)
bottom_dot_color = rl.Color(255, 0, 89, 255)
elif ui_state.status in (UIStatus.LAT_ONLY, UIStatus.LONG_ONLY):
top_dot_color = bottom_dot_color = self.get_lat_long_dot_color()
elif ui_state.status == UIStatus.OVERRIDE:
top_dot_color = rl.Color(255, 255, 255, 255)
bottom_dot_color = rl.Color(82, 82, 82, 255)
else:
top_dot_color = rl.Color(50, 50, 50, 255)
bottom_dot_color = rl.Color(13, 13, 13, 255)
draw_circle_gradient(content_rect.x + content_rect.width - status_dot_radius,
dot_height, status_dot_radius,
top_dot_color, bottom_dot_color)
@@ -0,0 +1,248 @@
import pyray as rl
from openpilot.cereal import log, messaging
from msgq.visionipc import VisionStreamType
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
from openpilot.selfdrive.ui.ui_state import ui_state, device
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.widgets import Widget
from openpilot.system.ui.widgets.nav_widget import NavWidget
from openpilot.system.ui.widgets.label import gui_label
class DriverCameraView(CameraView):
def _calc_frame_matrix(self, rect: rl.Rectangle):
base = super()._calc_frame_matrix(rect)
driver_view_ratio = 1.5
base[0, 0] *= driver_view_ratio
base[1, 1] *= driver_view_ratio
return base
class BaseDriverCameraDialog(Widget):
# Not a NavWidget so training guide can use this without back navigation
def __init__(self):
super().__init__()
self._camera_view = DriverCameraView("camerad", VisionStreamType.VISION_STREAM_DRIVER)
self.driver_state_renderer = DriverStateRenderer(lines=True)
self.driver_state_renderer.set_rect(rl.Rectangle(0, 0, 200, 200))
self.driver_state_renderer.load_icons()
self._pm: messaging.PubMaster | None = None
# Load eye icons
self._eye_fill_texture = None
self._eye_orange_texture = None
self._eye_size = 74
self._glasses_texture = None
self._glasses_size = 171
self._load_eye_textures()
def show_event(self):
super().show_event()
ui_state.params.put_bool("IsDriverViewEnabled", True)
self._publish_alert_sound(None)
device.set_override_interactive_timeout(300)
ui_state.params.remove("DriverTooDistracted")
self._pm = messaging.PubMaster(['selfdriveState'])
def hide_event(self):
super().hide_event()
ui_state.params.put_bool("IsDriverViewEnabled", False)
device.set_override_interactive_timeout(None)
def _handle_mouse_release(self, _):
ui_state.params.remove("DriverTooDistracted")
def __del__(self):
self.close()
def close(self):
if self._camera_view:
self._camera_view.close()
def _update_state(self):
if self._camera_view:
self._camera_view._update_state()
# Enable driver state renderer to show Dmoji in preview
self.driver_state_renderer.set_should_draw(True)
self.driver_state_renderer.set_force_active(True)
super()._update_state()
def _render(self, rect):
rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height))
self._camera_view._render(rect)
if not self._camera_view.frame:
gui_label(rect, tr("camera starting"), font_size=54, font_weight=FontWeight.BOLD,
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
rl.end_scissor_mode()
self._publish_alert_sound(None)
return
driver_data = self._draw_face_detection(rect)
if driver_data is not None:
self._draw_eyes(rect, driver_data)
# Position dmoji on opposite side from driver
driver_state_rect = (
rect.x if self.driver_state_renderer.is_rhd else rect.x + rect.width - self.driver_state_renderer.rect.width,
rect.y + (rect.height - self.driver_state_renderer.rect.height) / 2,
)
self.driver_state_renderer.set_position(*driver_state_rect)
self.driver_state_renderer.render()
# Render driver monitoring alerts
self._render_dm_alerts(rect)
rl.end_scissor_mode()
return
def _publish_alert_sound(self, dm_state):
"""Publish selfdriveState with only alertSound field set"""
if self._pm is None:
return
AudibleAlert = log.SelfdriveState.AudibleAlert
ALERT_SOUNDS = {
'one': AudibleAlert.preAlert,
'two': AudibleAlert.promptDistracted,
'three': AudibleAlert.warningImmediate,
}
msg = messaging.new_message('selfdriveState')
if dm_state is not None:
msg.selfdriveState.alertSound = ALERT_SOUNDS.get(str(dm_state.alertLevel), AudibleAlert.none)
self._pm.send('selfdriveState', msg)
def _render_dm_alerts(self, rect: rl.Rectangle):
"""Render driver monitoring event names"""
dm_state = ui_state.sm["driverMonitoringState"]
self._publish_alert_sound(dm_state)
is_vision = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
awareness_pct = dm_state.visionPolicyState.awarenessPercent if is_vision else dm_state.wheeltouchPolicyState.awarenessPercent
gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height),
f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
color=rl.Color(0, 0, 0, 180))
gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
color=rl.Color(255, 255, 255, int(255 * 0.9)))
if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none:
return
# Show alert level
alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}"
alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT
shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height)
gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
alignment=alignment,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
color=rl.Color(0, 0, 0, 180))
gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
alignment=alignment,
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
color=rl.Color(255, 255, 255, int(255 * 0.9)))
def _load_eye_textures(self):
"""Lazy load eye textures"""
if self._eye_fill_texture is None:
self._eye_fill_texture = gui_app.texture("icons_mici/onroad/eye_fill.png", self._eye_size, self._eye_size)
if self._eye_orange_texture is None:
self._eye_orange_texture = gui_app.texture("icons_mici/onroad/eye_orange.png", self._eye_size, self._eye_size)
if self._glasses_texture is None:
self._glasses_texture = gui_app.texture("icons_mici/onroad/glasses.png", self._glasses_size, self._glasses_size)
def _draw_face_detection(self, rect: rl.Rectangle):
dm_state = ui_state.sm["driverMonitoringState"]
driver_data = self.driver_state_renderer.get_driver_data()
if not dm_state.visionPolicyState.faceDetected:
return
# Get face position and orientation
face_x, face_y = driver_data.facePosition
face_std = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1])
alpha = 0.7
if face_std > 0.15:
alpha = max(0.7 - (face_std - 0.15) * 3.5, 0.0)
# use approx instead of distort_points
# TODO: replace with distort_points
tici_x = 1080.0 - 1714.0 * face_x
tici_y = -135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y
# Tici coords are relative to center, scale offset
offset_x = (tici_x - 1080.0) * 1.25
offset_y = (tici_y - 540.0) * 1.25
# Map to mici screen (scale from 2160x1080 to rect dimensions)
scale_x = rect.width / 2160.0
scale_y = rect.height / 1080.0
fbox_x = rect.x + rect.width / 2 + offset_x * scale_x
fbox_y = rect.y + rect.height / 2 + offset_y * scale_y
box_size = 75
line_thickness = 3
line_color = rl.Color(255, 255, 255, int(alpha * 255))
rl.draw_rectangle_rounded_lines_ex(
rl.Rectangle(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size),
35.0 / box_size / 2,
line_thickness,
line_thickness,
line_color,
)
return driver_data
def _draw_eyes(self, rect: rl.Rectangle, driver_data):
# Draw eye indicators based on eye probabilities
eye_offset_x = 10
eye_offset_y = 10
eye_spacing = self._eye_size + 15
left_eye_x = rect.x + eye_offset_x
left_eye_y = rect.y + eye_offset_y
left_eye_prob = driver_data.leftEyeProb
right_eye_x = rect.x + eye_offset_x + eye_spacing
right_eye_y = rect.y + eye_offset_y
right_eye_prob = driver_data.rightEyeProb
# Draw eyes with opacity based on probability
for eye_x, eye_y, eye_prob in [(left_eye_x, left_eye_y, left_eye_prob), (right_eye_x, right_eye_y, right_eye_prob)]:
fill_opacity = eye_prob
orange_opacity = 1.0 - eye_prob
rl.draw_texture_v(self._eye_orange_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * orange_opacity)))
rl.draw_texture_v(self._eye_fill_texture, (eye_x, eye_y), rl.Color(255, 255, 255, int(255 * fill_opacity)))
# Draw sunglasses indicator based on sunglasses probability
# Position glasses centered between the two eyes at top left
glasses_x = rect.x + eye_offset_x - 4
glasses_y = rect.y
glasses_pos = rl.Vector2(glasses_x, glasses_y)
glasses_prob = driver_data.sunglassesProb
rl.draw_texture_v(self._glasses_texture, glasses_pos, rl.Color(70, 80, 161, int(255 * glasses_prob)))
class DriverCameraDialog(NavWidget, BaseDriverCameraDialog):
def __init__(self):
super().__init__()
# TODO: this can grow unbounded, should be given some thought
device.add_interactive_timeout_callback(gui_app.pop_widget)
if __name__ == "__main__":
gui_app.init_window("Driver Camera View (mici)")
driver_camera_view = DriverCameraDialog()
gui_app.push_widget(driver_camera_view)
try:
for _ in gui_app.render():
ui_state.update()
finally:
driver_camera_view.close()
@@ -0,0 +1,216 @@
import pyray as rl
import numpy as np
import math
from openpilot.cereal import log
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.ui_state import ui_state
AlertSize = log.SelfdriveState.AlertSize
DEBUG = False
# TODO: Only left for DM preview, remove
LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6)
LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3)
CONE_COLOR_GREEN = (0, 255, 64)
CONE_COLOR_ORANGE = (255, 115, 0)
class DriverStateRenderer(Widget):
BASE_SIZE = 60
LINES_ANGLE_INCREMENT = 5
LINES_STALE_ANGLES = 3.0 # seconds
AWARENESS_UNFULL_PERCENT = 95 # ~0.5s
def __init__(self, lines: bool = False, inset: bool = False):
super().__init__()
self.set_rect(rl.Rectangle(0, 0, self.BASE_SIZE, self.BASE_SIZE))
self._lines = lines
self._inset = inset
# In line mode, track smoothed angles
assert 360 % self.LINES_ANGLE_INCREMENT == 0
self._head_angles = {i * self.LINES_ANGLE_INCREMENT: FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) for i in range(360 // self.LINES_ANGLE_INCREMENT)}
self._is_active = False
self._is_rhd = False
self._face_detected = False
self._face_pitch = 0.
self._face_yaw = 0.
self._should_draw = False
self._force_active = False
self._looking_center = False
self._awareness_unfull = False
self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps)
self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps) # 1.0 = full green, 0.0 = full orange
self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False)
self._looking_center_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
# Load the driver face icons
self.load_icons()
def load_icons(self):
"""Load or reload the driver face icon texture"""
cone_and_person_size = round(52 / self.BASE_SIZE * self._rect.width)
# If inset is enabled, push cone and person smaller by 2x the current inset space
if self._inset:
# Current inset space = (rect.width - cone_and_person_size) / 2
current_inset = (self._rect.width - cone_and_person_size) / 2
# Reduce size by 2x the current inset (1x on each side)
cone_and_person_size = round(cone_and_person_size - current_inset * 2)
self._dm_person = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_person.png", cone_and_person_size, cone_and_person_size)
self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", cone_and_person_size, cone_and_person_size)
self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", int(self._rect.width), int(self._rect.height))
def set_should_draw(self, should_draw: bool):
self._should_draw = should_draw
@property
def should_draw(self):
return (self._should_draw and ui_state.sm["selfdriveState"].alertSize == AlertSize.none and
ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame)
def set_force_active(self, force_active: bool):
"""Force the dmoji to always appear active (green) regardless of actual state"""
self._force_active = force_active
@property
def effective_active(self) -> bool:
"""Returns True if dmoji should appear active (either actually active or forced)"""
return bool(self._force_active or self._is_active)
@property
def is_rhd(self) -> bool:
return self._is_rhd
def _render(self, _):
if DEBUG:
rl.draw_rectangle_lines_ex(self._rect, 1, rl.RED)
rl.draw_texture_ex(self._dm_background,
rl.Vector2(self._rect.x, self._rect.y), 0.0, 1.0,
rl.Color(255, 255, 255, int(255 * self._fade_filter.x)))
rl.draw_texture_ex(self._dm_person,
rl.Vector2(self._rect.x + (self._rect.width - self._dm_person.width) / 2,
self._rect.y + (self._rect.height - self._dm_person.height) / 2), 0.0, 1.0,
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
green_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0)
if self.effective_active:
source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height)
dest_rect = rl.Rectangle(
self._rect.x + self._rect.width / 2,
self._rect.y + self._rect.height / 2,
self._dm_cone.width,
self._dm_cone.height,
)
if not self._lines:
r = int(round(CONE_COLOR_GREEN[0] * green_amount + CONE_COLOR_ORANGE[0] * (1 - green_amount)))
g = int(round(CONE_COLOR_GREEN[1] * green_amount + CONE_COLOR_ORANGE[1] * (1 - green_amount)))
b = int(round(CONE_COLOR_GREEN[2] * green_amount + CONE_COLOR_ORANGE[2] * (1 - green_amount)))
rl.draw_texture_pro(
self._dm_cone,
source_rect,
dest_rect,
rl.Vector2(dest_rect.width / 2, dest_rect.height / 2),
self._rotation_filter.x - 90,
rl.Color(r, g, b, int(255 * self._fade_filter.x)),
)
else:
# remove old angles
for angle, f in self._head_angles.items():
dst_from_current = ((angle - self._rotation_filter.x) % 360) - 180
target = 1.0 if abs(dst_from_current) <= self.LINES_ANGLE_INCREMENT * 5 else 0.0
if not self._face_detected:
target = 0.0
# Reduce all line lengths when looking center
if self._looking_center:
target = np.interp(self._looking_center_filter.x, [0.0, 1.0], [target, 0.45])
f.update(target)
self._draw_line(angle, f, self._looking_center)
def _draw_line(self, angle: int, f: FirstOrderFilter, grey: bool):
line_length = self._rect.width / 6
line_length = round(np.interp(f.x, [0.0, 1.0], [0, line_length]))
line_offset = self._rect.width / 2 - line_length * 2 # ensure line ends within rect
center_x = self._rect.x + self._rect.width / 2
center_y = self._rect.y + self._rect.height / 2
start_x = center_x + (line_offset + line_length) * math.cos(math.radians(angle))
start_y = center_y + (line_offset + line_length) * math.sin(math.radians(angle))
end_x = start_x + line_length * math.cos(math.radians(angle))
end_y = start_y + line_length * math.sin(math.radians(angle))
color = rl.Color(0, 255, 64, 255)
if grey:
color = rl.Color(166, 166, 166, 255)
if f.x > 0.01:
rl.draw_line_ex((start_x, start_y), (end_x, end_y), 12, color)
def get_driver_data(self):
sm = ui_state.sm
dm_state = sm["driverMonitoringState"]
self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
self._is_rhd = dm_state.isRHD
self._face_detected = dm_state.visionPolicyState.faceDetected
self._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT
self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6) # calib or DM pose is not accurate, add a fake upward pitch to bias forward
self._face_yaw = -dm_state.visionPolicyState.pose.yaw # undo sign flip in face_orientation_from_model to match UI convention
driverstate = sm["driverStateV2"]
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
return driver_data
def _update_state(self):
# Get monitoring state
_ = self.get_driver_data()
pitch = self._pitch_filter.update(self._face_pitch)
yaw = self._yaw_filter.update(self._face_yaw)
# hysteresis on looking center
if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:
self._looking_center = True
elif abs(pitch) > LOOKING_CENTER_THRESHOLD_UPPER or abs(yaw) > LOOKING_CENTER_THRESHOLD_UPPER:
self._looking_center = False
self._looking_center_filter.update(1 if self._looking_center else 0)
if DEBUG:
pitchd = math.degrees(pitch)
yawd = math.degrees(yaw)
rl.draw_line_ex((0, 100), (200, 100), 3, rl.RED)
rl.draw_line_ex((0, 120), (200, 120), 3, rl.RED)
rl.draw_line_ex((0, 140), (200, 140), 3, rl.RED)
pitch_x = 100 + pitchd
yaw_x = 100 + yawd
rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN)
rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN)
# filter head rotation, handling wrap-around
rotation = math.degrees(math.atan2(pitch * 2, yaw)) # reduce yaw sensitivity
angle_diff = rotation - self._rotation_filter.x
angle_diff = ((angle_diff + 180) % 360) - 180
self._rotation_filter.update(self._rotation_filter.x + angle_diff)
if not self.should_draw:
self._fade_filter.update(0.0)
elif not self.effective_active:
self._fade_filter.update(0.35)
else:
self._fade_filter.update(1.0)
@@ -0,0 +1,278 @@
import pyray as rl
from dataclasses import dataclass
from openpilot.common.constants import CV
from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.multilang import tr
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.widgets import Widget
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.cereal import log
EventName = log.OnroadEvent.EventName
# Constants
SET_SPEED_NA = 255
KM_TO_MILE = 0.621371
CRUISE_DISABLED_CHAR = '–'
SET_SPEED_PERSISTENCE = 2.5 # seconds
@dataclass(frozen=True)
class FontSizes:
current_speed: int = 176
speed_unit: int = 66
max_speed: int = 36
set_speed: int = 112
@dataclass(frozen=True)
class Colors:
WHITE = rl.WHITE
WHITE_TRANSLUCENT = rl.Color(255, 255, 255, 200)
FONT_SIZES = FontSizes()
COLORS = Colors()
class TurnIntent(Widget):
FADE_IN_ANGLE = 30 # degrees
def __init__(self):
super().__init__()
self._pre = False
self._turn_intent_direction: int = 0
self._turn_intent_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
self._turn_intent_rotation_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
self._txt_turn_intent_left: rl.Texture = gui_app.texture('icons_mici/turn_intent_left.png', 50, 20)
self._txt_turn_intent_right: rl.Texture = gui_app.texture('icons_mici/turn_intent_left.png', 50, 20, flip_x=True)
def _render(self, _):
if self._turn_intent_alpha_filter.x > 1e-2:
turn_intent_texture = self._txt_turn_intent_right if self._turn_intent_direction == 1 else self._txt_turn_intent_left
src_rect = rl.Rectangle(0, 0, turn_intent_texture.width, turn_intent_texture.height)
dest_rect = rl.Rectangle(self._rect.x + self._rect.width / 2, self._rect.y + self._rect.height / 2,
turn_intent_texture.width, turn_intent_texture.height)
origin = (turn_intent_texture.width / 2, self._rect.height / 2)
color = rl.Color(255, 255, 255, int(255 * self._turn_intent_alpha_filter.x))
rl.draw_texture_pro(turn_intent_texture, src_rect, dest_rect, origin, self._turn_intent_rotation_filter.x, color)
def _update_state(self) -> None:
sm = ui_state.sm
left = any(e.name == EventName.preLaneChangeLeft for e in sm['onroadEvents'])
right = any(e.name == EventName.preLaneChangeRight for e in sm['onroadEvents'])
if left or right:
# pre lane change
if not self._pre:
self._turn_intent_rotation_filter.x = self.FADE_IN_ANGLE if left else -self.FADE_IN_ANGLE
self._pre = True
self._turn_intent_direction = -1 if left else 1
self._turn_intent_alpha_filter.update(1)
self._turn_intent_rotation_filter.update(0)
elif any(e.name == EventName.laneChange for e in sm['onroadEvents']):
# fade out and rotate away
self._pre = False
self._turn_intent_alpha_filter.update(0)
if self._turn_intent_direction == 0:
# unknown. missed pre frame?
self._turn_intent_rotation_filter.update(0)
else:
self._turn_intent_rotation_filter.update(self._turn_intent_direction * self.FADE_IN_ANGLE)
else:
# didn't complete lane change, just hide
self._pre = False
self._turn_intent_direction = 0
self._turn_intent_alpha_filter.update(0)
self._turn_intent_rotation_filter.update(0)
class HudRenderer(Widget):
def __init__(self):
super().__init__()
"""Initialize the HUD renderer."""
self.is_cruise_set: bool = False
self.is_cruise_available: bool = True
self.set_speed: float = SET_SPEED_NA
self._set_speed_changed_time: float = 0
self.speed: float = 0.0
self.v_ego_cluster_seen: bool = False
self._engaged: bool = False
self._can_draw_top_icons = True
self._show_wheel_critical = False
self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
self._font_display: rl.Font = gui_app.font(FontWeight.DISPLAY)
self._turn_intent = TurnIntent()
self._torque_bar = TorqueBar()
self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50)
self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50)
self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 9, 44)
self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
self._set_speed_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
def set_wheel_critical_icon(self, critical: bool):
"""Set the wheel icon to critical or normal state."""
self._show_wheel_critical = critical
def set_can_draw_top_icons(self, can_draw_top_icons: bool):
"""Set whether to draw the top part of the HUD."""
self._can_draw_top_icons = can_draw_top_icons
def drawing_top_icons(self) -> bool:
# whether we're drawing any top icons currently
return bool(self._set_speed_alpha_filter.x > 1e-2)
def _update_state(self) -> None:
"""Update HUD state based on car state and controls state."""
sm = ui_state.sm
if sm.recv_frame["carState"] < ui_state.started_frame:
self.is_cruise_set = False
self.set_speed = SET_SPEED_NA
self.speed = 0.0
return
controls_state = sm['controlsState']
car_state = sm['carState']
v_cruise_cluster = car_state.vCruiseCluster
set_speed = (
controls_state.deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster
)
engaged = sm['selfdriveState'].enabled
if (set_speed != self.set_speed and engaged) or (engaged and not self._engaged):
self._set_speed_changed_time = rl.get_time()
self._engaged = engaged
self.set_speed = set_speed
self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA
self.is_cruise_available = self.set_speed != -1
v_ego_cluster = car_state.vEgoCluster
self.v_ego_cluster_seen = self.v_ego_cluster_seen or v_ego_cluster != 0.0
v_ego = v_ego_cluster if self.v_ego_cluster_seen else car_state.vEgo
speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
self.speed = max(0.0, v_ego * speed_conversion)
def _render(self, rect: rl.Rectangle) -> None:
"""Render HUD elements to the screen."""
self._torque_bar.render(rect)
if self.is_cruise_set:
self._draw_set_speed(rect)
self._draw_steering_wheel(rect)
def _draw_steering_wheel(self, rect: rl.Rectangle) -> None:
wheel_txt = self._txt_wheel_critical if self._show_wheel_critical else self._txt_wheel
bsm_detected = self._has_blind_spot_detected() if gui_app.sunnypilot_ui() else False
if self._show_wheel_critical:
self._wheel_alpha_filter.update(255)
self._wheel_y_filter.update(0)
else:
if ui_state.status == UIStatus.DISENGAGED or bsm_detected:
self._wheel_alpha_filter.update(0)
self._wheel_y_filter.update(wheel_txt.height / 2)
else:
self._wheel_alpha_filter.update(255 * 0.9)
self._wheel_y_filter.update(0)
# pos
pos_x = int(rect.x + 21 + wheel_txt.width / 2)
pos_y = int(rect.y + rect.height - 14 - wheel_txt.height / 2 + self._wheel_y_filter.x)
rotation = -ui_state.sm['carState'].steeringAngleDeg
turn_intent_margin = 25
self._turn_intent.render(rl.Rectangle(
pos_x - wheel_txt.width / 2 - turn_intent_margin,
pos_y - wheel_txt.height / 2 - turn_intent_margin,
wheel_txt.width + turn_intent_margin * 2,
wheel_txt.height + turn_intent_margin * 2,
))
src_rect = rl.Rectangle(0, 0, wheel_txt.width, wheel_txt.height)
dest_rect = rl.Rectangle(pos_x, pos_y, wheel_txt.width, wheel_txt.height)
origin = (wheel_txt.width / 2, wheel_txt.height / 2)
# color and draw
color = rl.Color(255, 255, 255, int(self._wheel_alpha_filter.x))
rl.draw_texture_pro(wheel_txt, src_rect, dest_rect, origin, rotation, color)
if self._show_wheel_critical:
# Draw exclamation point icon
EXCLAMATION_POINT_SPACING = 10
exclamation_pos_x = pos_x - self._txt_exclamation_point.width / 2 + wheel_txt.width / 2 + EXCLAMATION_POINT_SPACING
exclamation_pos_y = pos_y - self._txt_exclamation_point.height / 2
rl.draw_texture_ex(self._txt_exclamation_point, rl.Vector2(exclamation_pos_x, exclamation_pos_y), 0.0, 1.0, rl.WHITE)
def _draw_set_speed(self, rect: rl.Rectangle) -> None:
"""Draw the MAX speed indicator box."""
alpha = self._set_speed_alpha_filter.update(0 < rl.get_time() - self._set_speed_changed_time < SET_SPEED_PERSISTENCE and
self._can_draw_top_icons and self._engaged)
if alpha < 1e-2:
return
x = rect.x
y = rect.y
# draw drop shadow
circle_radius = 162 // 2
rl.draw_circle_gradient(rl.Vector2(x + circle_radius, y + circle_radius), circle_radius,
rl.Color(0, 0, 0, int(255 / 2 * alpha)), rl.BLANK)
set_speed_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
max_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
set_speed = self.set_speed
if self.is_cruise_set and not ui_state.is_metric:
set_speed *= KM_TO_MILE
set_speed_text = CRUISE_DISABLED_CHAR if not self.is_cruise_set else str(round(set_speed))
rl.draw_text_ex(
self._font_display,
set_speed_text,
rl.Vector2(x + 13 + 4, y + 3 - 8 - 3 + 4),
FONT_SIZES.set_speed,
0,
set_speed_color,
)
max_text = tr("MAX")
rl.draw_text_ex(
self._font_semi_bold,
max_text,
rl.Vector2(x + 25, y + FONT_SIZES.set_speed - 7 + 4),
FONT_SIZES.max_speed,
0,
max_color,
)
def _draw_current_speed(self, rect: rl.Rectangle) -> None:
"""Draw the current vehicle speed and unit."""
speed_text = str(round(self.speed))
speed_text_size = measure_text_cached(self._font_bold, speed_text, FONT_SIZES.current_speed)
speed_pos = rl.Vector2(rect.x + rect.width / 2 - speed_text_size.x / 2, 180 - speed_text_size.y / 2)
rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.WHITE)
unit_text = tr("km/h") if ui_state.is_metric else tr("mph")
unit_text_size = measure_text_cached(self._font_medium, unit_text, FONT_SIZES.speed_unit)
unit_pos = rl.Vector2(rect.x + rect.width / 2 - unit_text_size.x / 2, 290 - unit_text_size.y / 2)
rl.draw_text_ex(self._font_medium, unit_text, unit_pos, FONT_SIZES.speed_unit, 0, COLORS.WHITE_TRANSLUCENT)
@@ -0,0 +1,499 @@
import colorsys
import numpy as np
import pyray as rl
from openpilot.cereal import messaging
from opendbc.car.structs import car
from dataclasses import dataclass, field
from openpilot.common.params import Params
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.selfdrive.ui.mici.onroad import blend_colors
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LINE_COLORS_SP, ModelRendererSP
CLIP_MARGIN = 500
MIN_DRAW_DISTANCE = 10.0
MAX_DRAW_DISTANCE = 100.0
THROTTLE_COLORS = [
rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4)
rl.Color(114, 255, 92, 89), # HSLF(112/360, 1.0, 0.68, 0.35)
rl.Color(114, 255, 92, 0), # HSLF(112/360, 1.0, 0.68, 0.0)
]
NO_THROTTLE_COLORS = [
rl.Color(242, 242, 242, 102), # HSLF(148/360, 0.0, 0.95, 0.4)
rl.Color(242, 242, 242, 89), # HSLF(112/360, 0.0, 0.95, 0.35)
rl.Color(242, 242, 242, 0), # HSLF(112/360, 0.0, 0.95, 0.0)
]
LANE_LINE_COLORS = {
UIStatus.DISENGAGED: rl.Color(200, 200, 200, 255),
UIStatus.OVERRIDE: rl.Color(255, 255, 255, 255),
UIStatus.ENGAGED: rl.Color(0, 255, 64, 255),
**LANE_LINE_COLORS_SP,
}
@dataclass
class ModelPoints:
raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32))
projected_points: np.ndarray = field(default_factory=lambda: np.empty((0, 2), dtype=np.float32))
@dataclass
class LeadVehicle:
glow: list[float] = field(default_factory=list)
chevron: list[float] = field(default_factory=list)
fill_alpha: int = 0
class ModelRenderer(Widget, ModelRendererSP):
def __init__(self):
Widget.__init__(self)
ModelRendererSP.__init__(self)
self._longitudinal_control = False
self._experimental_mode = False
self._blend_filter = FirstOrderFilter(1.0, 0.25, 1 / gui_app.target_fps)
self._prev_allow_throttle = True
self._lane_line_probs = np.zeros(4, dtype=np.float32)
self._road_edge_stds = np.zeros(2, dtype=np.float32)
self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
self._path_offset_z = HEIGHT_INIT[0]
# Initialize ModelPoints objects
self._path = ModelPoints()
self._lane_lines = [ModelPoints() for _ in range(4)]
self._road_edges = [ModelPoints() for _ in range(2)]
self._acceleration_x = np.empty((0,), dtype=np.float32)
self._acceleration_x_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
self._acceleration_x_filter2 = FirstOrderFilter(0.0, 1, 1 / gui_app.target_fps)
self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
self._ll_color_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
# 3x3 car space -> rect-origin space (draw methods add rect.x/y)
self._car_space_transform = np.zeros((3, 3), dtype=np.float32)
self._transform_dirty = True
self._clip_region = None
self._counter = -1
self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
self._exp_gradient = Gradient(
start=(0.0, 1.0), # Bottom of path
end=(0.0, 0.0), # Top of path
colors=[],
stops=[],
)
# Get longitudinal control setting from car parameters
if car_params := Params().get("CarParams"):
cp = messaging.log_from_bytes(car_params, car.CarParams)
self._longitudinal_control = cp.openpilotLongitudinalControl
def set_transform(self, transform: np.ndarray):
self._car_space_transform = transform.astype(np.float32)
self._transform_dirty = True
def _render(self, rect: rl.Rectangle):
sm = ui_state.sm
if self._counter % 180 == 0: # This runs at 60fps, so we query every 3 seconds
self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
self._counter += 1
self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque)
# Check if data is up-to-date
if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or
sm.recv_frame["modelV2"] < ui_state.started_frame):
return
# Set up clipping region
self._clip_region = rl.Rectangle(
rect.x - CLIP_MARGIN, rect.y - CLIP_MARGIN, rect.width + 2 * CLIP_MARGIN, rect.height + 2 * CLIP_MARGIN
)
# Update state
self._experimental_mode = sm['selfdriveState'].experimentalMode
live_calib = sm['liveCalibration']
self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0]
if sm.updated['carParams']:
self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl
model = sm['modelV2']
radar_state = sm['radarState'] if sm.valid['radarState'] else None
lead_one = radar_state.leadOne if radar_state else None
render_lead_indicator = self._longitudinal_control and radar_state is not None
# Update model data when needed
model_updated = sm.updated['modelV2']
if model_updated or sm.updated['radarState'] or self._transform_dirty:
if model_updated:
self._update_raw_points(model)
path_x_array = self._path.raw_points[:, 0]
if path_x_array.size == 0:
return
self._update_model(lead_one, path_x_array)
if render_lead_indicator:
self._update_leads(radar_state, path_x_array)
self._transform_dirty = False
# Draw elements (hide when disengaged)
if ui_state.status != UIStatus.DISENGAGED:
self._draw_lane_lines()
self._draw_path(sm)
# if render_lead_indicator and radar_state:
# self._draw_lead_indicator()
def _update_raw_points(self, model):
"""Update raw 3D points from model data"""
self._path.raw_points = np.array([model.position.x, np.array(model.position.y) + self._camera_offset, model.position.z], dtype=np.float32).T
for i, lane_line in enumerate(model.laneLines):
self._lane_lines[i].raw_points = np.array([lane_line.x, np.array(lane_line.y) + self._camera_offset, lane_line.z], dtype=np.float32).T
for i, road_edge in enumerate(model.roadEdges):
self._road_edges[i].raw_points = np.array([road_edge.x, np.array(road_edge.y) + self._camera_offset, road_edge.z], dtype=np.float32).T
self._lane_line_probs = np.array(model.laneLineProbs, dtype=np.float32)
self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32)
self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32)
def _update_leads(self, radar_state, path_x_array):
"""Update positions of lead vehicles"""
self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
leads = [radar_state.leadOne, radar_state.leadTwo]
for i, lead_data in enumerate(leads):
if lead_data and lead_data.present:
d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel
idx = self._get_path_length_idx(path_x_array, d_rel)
# Get z-coordinate from path at the lead vehicle position
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
if point:
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
def _update_model(self, lead, path_x_array):
"""Update model visualization data based on model message"""
max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE)
max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance)
# Update lane lines using raw points
line_width_factor = 0.12
for i, lane_line in enumerate(self._lane_lines):
if i in (1, 2):
line_width_factor = 0.16
lane_line.projected_points = self._map_line_to_polygon(
lane_line.raw_points, line_width_factor * self._lane_line_probs[i], 0.0, max_idx
)
# Update road edges using raw points
for road_edge in self._road_edges:
road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, line_width_factor, 0.0, max_idx)
# Update path using raw points
if lead and lead.present:
lead_d = lead.dRel * 2.0
max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance)
soon_acceleration = self._acceleration_x[len(self._acceleration_x) // 4] if len(self._acceleration_x) > 0 else 0
self._acceleration_x_filter.update(soon_acceleration)
self._acceleration_x_filter2.update(soon_acceleration)
# make path width wider/thinner when initially braking/accelerating
if self._experimental_mode and False:
high_pass_acceleration = self._acceleration_x_filter.x - self._acceleration_x_filter2.x
y_off = np.interp(high_pass_acceleration, [-1, 0, 1], [0.9 * 2, 0.9, 0.9 / 2])
else:
y_off = 0.9
max_idx = self._get_path_length_idx(path_x_array, max_distance)
self._path.projected_points = self._map_line_to_polygon(
self._path.raw_points, y_off, self._path_offset_z, max_idx, allow_invert=False
)
self._update_experimental_gradient()
def _update_experimental_gradient(self):
"""Pre-calculate experimental mode gradient colors"""
if not self._experimental_mode:
return
path_pts = self._path.projected_points + np.array([self._rect.x, self._rect.y], dtype=np.float32)
max_len = min(len(path_pts) // 2, len(self._acceleration_x))
segment_colors = []
gradient_stops = []
i = 0
while i < max_len:
# Some points (screen space) are out of frame (rect space)
track_y = path_pts[i][1]
if track_y < self._rect.y or track_y > (self._rect.y + self._rect.height):
i += 1
continue
# Calculate color based on acceleration (0 is bottom, 1 is top)
lin_grad_point = 1 - (track_y - self._rect.y) / self._rect.height
# speed up: 120, slow down: 0
path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120)
saturation = min(abs(self._acceleration_x[i] * 1.5), 1)
lightness = np.interp(saturation, [0.0, 1.0], [0.95, 0.62])
alpha = np.interp(lin_grad_point, [0.75 / 2.0, 0.75], [0.4, 0.0])
# Use HSL to RGB conversion
color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha)
gradient_stops.append(lin_grad_point)
segment_colors.append(color)
# Skip a point, unless next is last
i += 1 + (1 if (i + 2) < max_len else 0)
# Store the gradient in the path object
self._exp_gradient.colors = segment_colors
self._exp_gradient.stops = gradient_stops
def _update_lead_vehicle(self, d_rel, v_rel, point, rect):
speed_buff, lead_buff = 10.0, 40.0
# Calculate fill alpha
fill_alpha = 0
if d_rel < lead_buff:
fill_alpha = 255 * (1.0 - (d_rel / lead_buff))
if v_rel < 0:
fill_alpha += 255 * (-1 * (v_rel / speed_buff))
fill_alpha = min(fill_alpha, 255)
# Calculate size and position
sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 1
x = np.clip(point[0], 0.0, rect.width - sz / 2)
y = min(point[1], rect.height - sz * 0.6)
g_xo = sz / 5
g_yo = sz / 10
glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)]
chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)]
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
def _get_ll_color(self, prob: float, adjacent: bool, left: bool):
alpha = np.clip(prob, 0.0, 0.7)
if adjacent:
_base_color = LANE_LINE_COLORS.get(ui_state.status, LANE_LINE_COLORS[UIStatus.DISENGAGED])
color = rl.Color(_base_color.r, _base_color.g, _base_color.b, int(alpha * 255))
# turn adjacent lls orange if torque is high
torque = self._torque_filter.x
high_torque = abs(torque) > 0.6
if high_torque and (left == (torque > 0)):
color = blend_colors(
color,
rl.Color(255, 115, 0, int(alpha * 255)), # orange
np.interp(abs(torque), [0.6, 0.8], [0.0, 1.0])
)
else:
color = rl.Color(255, 255, 255, int(alpha * 255))
if ui_state.status == UIStatus.DISENGAGED:
color = rl.Color(0, 0, 0, int(alpha * 255))
return color
def _draw_lane_lines(self):
"""Draw lane lines and road edges. Two closest lines should be green (lane line or road edges)."""
offset = np.array([self._rect.x, self._rect.y], dtype=np.float32)
for i, lane_line in enumerate(self._lane_lines):
if lane_line.projected_points.size == 0:
continue
color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1))
draw_polygon(self._rect, lane_line.projected_points + offset, color)
for i, road_edge in enumerate(self._road_edges):
if road_edge.projected_points.size == 0:
continue
# if closest lane lines are not confident, make road edges green
color = self._get_ll_color(float(1.0 - self._road_edge_stds[i]), float(self._lane_line_probs[i + 1]) < 0.25, i == 0)
draw_polygon(self._rect, road_edge.projected_points + offset, color)
def _draw_path(self, sm):
"""Draw path with dynamic coloring based on mode and throttle state."""
if not self._path.projected_points.size:
return
allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
self._blend_filter.update(int(allow_throttle))
if ui_state.rainbow_path:
self.rainbow_path.draw_rainbow_path(self._rect, self._path)
return
path_pts = self._path.projected_points + np.array([self._rect.x, self._rect.y], dtype=np.float32)
if self._experimental_mode:
# Draw with acceleration coloring
if ui_state.status == UIStatus.DISENGAGED:
draw_polygon(self._rect, path_pts, rl.Color(0, 0, 0, 90))
elif len(self._exp_gradient.colors) > 1:
draw_polygon(self._rect, path_pts, gradient=self._exp_gradient)
else:
draw_polygon(self._rect, path_pts, rl.Color(255, 255, 255, 30))
else:
# Blend throttle/no throttle colors based on transition
blend_factor = round(self._blend_filter.x * 100) / 100
blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor)
gradient = Gradient(
start=(0.0, 1.0), # Bottom of path
end=(0.0, 0.0), # Top of path
colors=blended_colors,
stops=[0.0, 0.5, 1.0],
)
if ui_state.status == UIStatus.DISENGAGED:
draw_polygon(self._rect, path_pts, rl.Color(0, 0, 0, 90))
else:
draw_polygon(self._rect, path_pts, gradient=gradient)
def _draw_lead_indicator(self):
# Draw lead vehicles if available
for lead in self._lead_vehicles:
if not lead.glow or not lead.chevron:
continue
rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255))
rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha))
@staticmethod
def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
"""Get the index corresponding to the given path height"""
if len(pos_x_array) == 0:
return 0
indices = np.where(pos_x_array <= path_height)[0]
return indices[-1] if indices.size > 0 else 0
def _map_to_screen(self, in_x, in_y, in_z):
"""Project a point in car space to screen space"""
input_pt = np.array([in_x, in_y, in_z])
pt = self._car_space_transform @ input_pt
if abs(pt[2]) < 1e-6:
return None
x, y = pt[0] / pt[2], pt[1] / pt[2]
clip = self._clip_region
if not (clip.x <= x <= clip.x + clip.width and clip.y <= y <= clip.y + clip.height):
return None
return (x, y)
def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, allow_invert: bool = True) -> np.ndarray:
"""Convert 3D line to 2D polygon for rendering."""
if line.shape[0] == 0:
return np.empty((0, 2), dtype=np.float32)
# Slice points and filter non-negative x-coordinates
points = line[:max_idx + 1]
points = points[points[:, 0] >= 0]
if points.shape[0] == 0:
return np.empty((0, 2), dtype=np.float32)
N = points.shape[0]
# Generate left and right 3D points in one array using broadcasting
offsets = np.array([[0, -y_off, z_off], [0, y_off, z_off]], dtype=np.float32)
points_3d = points[None, :, :] + offsets[:, None, :] # Shape: 2xNx3
points_3d = points_3d.reshape(2 * N, 3) # Shape: (2*N)x3
# Transform all points to projected space in one operation
proj = self._car_space_transform @ points_3d.T # Shape: 3x(2*N)
proj = proj.reshape(3, 2, N)
left_proj = proj[:, 0, :]
right_proj = proj[:, 1, :]
# Filter points where z is sufficiently large
valid_proj = (np.abs(left_proj[2]) >= 1e-6) & (np.abs(right_proj[2]) >= 1e-6)
if not np.any(valid_proj):
return np.empty((0, 2), dtype=np.float32)
# Compute screen coordinates
left_screen = left_proj[:2, valid_proj] / left_proj[2, valid_proj][None, :]
right_screen = right_proj[:2, valid_proj] / right_proj[2, valid_proj][None, :]
# Define clip region bounds
clip = self._clip_region
x_min, x_max = clip.x, clip.x + clip.width
y_min, y_max = clip.y, clip.y + clip.height
# Filter points within clip region
left_in_clip = (
(left_screen[0] >= x_min) & (left_screen[0] <= x_max) &
(left_screen[1] >= y_min) & (left_screen[1] <= y_max)
)
right_in_clip = (
(right_screen[0] >= x_min) & (right_screen[0] <= x_max) &
(right_screen[1] >= y_min) & (right_screen[1] <= y_max)
)
both_in_clip = left_in_clip & right_in_clip
if not np.any(both_in_clip):
return np.empty((0, 2), dtype=np.float32)
# Select valid and clipped points
left_screen = left_screen[:, both_in_clip]
right_screen = right_screen[:, both_in_clip]
# Handle Y-coordinate inversion on hills
if not allow_invert and left_screen.shape[1] > 1:
y = left_screen[1, :] # y-coordinates
keep = y == np.minimum.accumulate(y)
if not np.any(keep):
return np.empty((0, 2), dtype=np.float32)
left_screen = left_screen[:, keep]
right_screen = right_screen[:, keep]
return np.vstack((left_screen.T, right_screen[:, ::-1].T)).astype(np.float32)
@staticmethod
def _hsla_to_color(h, s, l, a):
rgb = colorsys.hls_to_rgb(h, l, s)
return rl.Color(
int(rgb[0] * 255),
int(rgb[1] * 255),
int(rgb[2] * 255),
int(a * 255)
)
@staticmethod
def _blend_colors(begin_colors, end_colors, t):
if t >= 1.0:
return end_colors
if t <= 0.0:
return begin_colors
inv_t = 1.0 - t
return [rl.Color(
int(inv_t * start.r + t * end.r),
int(inv_t * start.g + t * end.g),
int(inv_t * start.b + t * end.b),
int(inv_t * start.a + t * end.a)
) for start, end in zip(begin_colors, end_colors, strict=True)]
@@ -0,0 +1,268 @@
import math
import time
from functools import wraps
from collections import OrderedDict
import numpy as np
import pyray as rl
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
from openpilot.selfdrive.ui.mici.onroad import blend_colors
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
from openpilot.system.ui.widgets import Widget
from openpilot.common.filter_simple import FirstOrderFilter
# TODO: arc_bar_pts doesn't consider rounded end caps part of the angle span
TORQUE_ANGLE_SPAN = 12.7
DEBUG = False
def quantized_lru_cache(maxsize=128):
def decorator(func):
cache = OrderedDict()
@wraps(func)
def wrapper(r_mid, thickness, a0_deg, a1_deg, **kwargs):
# Quantize inputs: balanced for smoothness vs cache effectiveness
key = (round(r_mid),
round(thickness), # 1px precision for smoother height transitions
round(a0_deg * 10) / 10, # 0.1° precision for smoother angle transitions
round(a1_deg * 10) / 10,
tuple(sorted(kwargs.items())))
if key in cache:
cache.move_to_end(key)
else:
if len(cache) >= maxsize:
cache.popitem(last=False)
result = func(r_mid, thickness, a0_deg, a1_deg, **kwargs)
cache[key] = result
return cache[key]
return wrapper
return decorator
@quantized_lru_cache(maxsize=256)
def arc_bar_pts(r_mid: float, thickness: float,
a0_deg: float, a1_deg: float,
*, max_points: int = 100, cap_segs: int = 10,
cap_radius: float = 7, px_per_seg: float = 2.0) -> np.ndarray:
"""Return Nx2 np.float32 points for a single closed polygon (rounded thick arc), centered at origin."""
def get_cap(left: bool, a_deg: float):
# end cap at a1: center (a1), sweep a1→a1+180 (skip endpoints to avoid dupes)
# quarter arc (outer corner) at a1 with fixed pixel radius cap_radius
nx, ny = math.cos(math.radians(a_deg)), math.sin(math.radians(a_deg)) # outward normal
tx, ty = -ny, nx # tangent (CCW)
mx, my = nx * r_mid, ny * r_mid # mid-point at a1
if DEBUG:
rl.draw_circle(int(mx), int(my), 4, rl.PURPLE)
ex = mx + nx * (half - cap_radius)
ey = my + ny * (half - cap_radius)
if DEBUG:
rl.draw_circle(int(ex), int(ey), 2, rl.WHITE)
# sweep 90° in the local (t,n) frame: from outer edge toward inside
if not left:
alpha = np.deg2rad(np.linspace(90, 0, cap_segs + 2))[1:-1]
else:
alpha = np.deg2rad(np.linspace(180, 90, cap_segs + 2))[1:-1]
cap_end = np.c_[ex + np.cos(alpha) * cap_radius * tx + np.sin(alpha) * cap_radius * nx,
ey + np.cos(alpha) * cap_radius * ty + np.sin(alpha) * cap_radius * ny]
# bottom quarter (inner corner) at a1
ex2 = mx + nx * (-half + cap_radius)
ey2 = my + ny * (-half + cap_radius)
if DEBUG:
rl.draw_circle(int(ex2), int(ey2), 2, rl.WHITE)
if not left:
alpha2 = np.deg2rad(np.linspace(0, -90, cap_segs + 1))[:-1] # include 0 once, exclude -90
else:
alpha2 = np.deg2rad(np.linspace(90 - 90 - 90, 0 - 90 - 90, cap_segs + 1))[:-1]
cap_end_bot = np.c_[ex2 + np.cos(alpha2) * cap_radius * tx + np.sin(alpha2) * cap_radius * nx,
ey2 + np.cos(alpha2) * cap_radius * ty + np.sin(alpha2) * cap_radius * ny]
# append to the top quarter
if not left:
cap_end = np.vstack((cap_end, cap_end_bot))
else:
cap_end = np.vstack((cap_end_bot, cap_end))
return cap_end
if a1_deg < a0_deg:
a0_deg, a1_deg = a1_deg, a0_deg
half = thickness * 0.5
cap_radius = min(cap_radius, half)
span = max(1e-3, a1_deg - a0_deg)
# pick arc segment count from arc length, clamp to shader points[] budget
arc_len = r_mid * math.radians(span)
arc_segs = max(6, int(arc_len / px_per_seg))
max_arc = (max_points - (4 * cap_segs + 3)) // 2
arc_segs = max(6, min(arc_segs, max_arc))
# outer arc a0→a1
ang_o = np.deg2rad(np.linspace(a0_deg, a1_deg, arc_segs + 1))
outer = np.c_[np.cos(ang_o) * (r_mid + half),
np.sin(ang_o) * (r_mid + half)]
# end cap at a1
cap_end = get_cap(False, a1_deg)
# inner arc a1→a0
ang_i = np.deg2rad(np.linspace(a1_deg, a0_deg, arc_segs + 1))
inner = np.c_[np.cos(ang_i) * (r_mid - half),
np.sin(ang_i) * (r_mid - half)]
# start cap at a0
cap_start = get_cap(True, a0_deg)
pts = np.vstack((outer, cap_end, inner, cap_start, outer[:1])).astype(np.float32)
# Rotate to start from middle of cap for proper triangulation
pts = np.roll(pts, cap_segs, axis=0)
if DEBUG:
n = len(pts)
idx = int(time.monotonic() * 12) % max(1, n) # speed: 12 pts/sec
for i, (x, y) in enumerate(pts):
j = (i - idx) % n # rotate the gradient
t = j / n
color = rl.Color(255, int(255 * (1 - t)), int(255 * t), 255)
rl.draw_circle(int(x), int(y), 2, color)
return pts
DEFAULT_MAX_LAT_ACCEL = 3.0 # m/s^2
class TorqueBar(Widget):
def __init__(self, demo: bool = False, scale: float = 1.0, always: bool = False):
super().__init__()
self._demo = demo
self._scale = scale
self._always = always
self._torque_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
self._torque_line_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
def update_filter(self, value: float):
"""Update the torque filter value (for demo mode)."""
self._torque_filter.update(value)
def _update_state(self):
if self._demo:
return
# torque line
if ui_state.sm['controlsState'].lateralControlState.which() in ('angleState', 'curvatureState'):
controls_state = ui_state.sm['controlsState']
car_state = ui_state.sm['carState']
live_parameters = ui_state.sm['liveParameters']
car_control = ui_state.sm['carControl']
# Include lateral accel error in estimated torque utilization
actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2
desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2
accel_diff = (desired_lateral_accel - actual_lateral_accel)
# Include road roll in estimated torque utilization
# Roll is less accurate near standstill, so reduce its effect at low speed
roll_compensation = live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY * np.interp(car_state.vEgo, [5, 15], [0.0, 1.0])
lateral_acceleration = actual_lateral_accel - roll_compensation
max_lateral_acceleration = ui_state.CP.maxLateralAccel if ui_state.CP else DEFAULT_MAX_LAT_ACCEL
if not car_control.latActive:
self._torque_filter.update(0.0)
else:
self._torque_filter.update(np.clip((lateral_acceleration + accel_diff) / max_lateral_acceleration, -1, 1))
else:
self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque)
def _render(self, rect: rl.Rectangle) -> None:
# adjust y pos with torque
torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22 * self._scale, 26 * self._scale])
torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14 * self._scale, 56 * self._scale])
# animate alpha and angle span
if not self._demo:
self._torque_line_alpha_filter.update(ui_state.status not in (UIStatus.DISENGAGED, UIStatus.LONG_ONLY))
else:
self._torque_line_alpha_filter.update(1.0)
torque_line_bg_alpha = np.interp(abs(self._torque_filter.x), [0.5, 1.0], [0.25, 0.5])
torque_line_bg_color = rl.Color(255, 255, 255, int(255 * torque_line_bg_alpha * self._torque_line_alpha_filter.x))
if ui_state.status not in (UIStatus.ENGAGED, UIStatus.LAT_ONLY) and not self._demo:
torque_line_bg_color = rl.Color(255, 255, 255, int(255 * 0.15 * self._torque_line_alpha_filter.x))
# draw curved line polygon torque bar
torque_line_radius = 1200 * self._scale
top_angle = -90
torque_bg_angle_span = self._torque_line_alpha_filter.x * TORQUE_ANGLE_SPAN
torque_start_angle = top_angle - torque_bg_angle_span / 2
torque_end_angle = top_angle + torque_bg_angle_span / 2
# centerline radius & center (you already have these values)
mid_r = torque_line_radius + torque_line_height / 2
cx = rect.x + rect.width / 2 + 8 # offset 8px to right of camera feed
cy = rect.y + rect.height + torque_line_radius - torque_line_offset
offset = np.array([cx, cy], dtype=np.float32)
# draw bg torque indicator line
bg_pts = arc_bar_pts(mid_r, torque_line_height, torque_start_angle, torque_end_angle, cap_radius=7 * self._scale) + offset
draw_polygon(rect, bg_pts, color=torque_line_bg_color)
# draw torque indicator line
a0s = top_angle
a1s = a0s + torque_bg_angle_span / 2 * self._torque_filter.x
sl_pts = arc_bar_pts(mid_r, torque_line_height, a0s, a1s, cap_radius=7 * self._scale) + offset
# draw beautiful gradient from center to 65% of the bg torque bar width
start_grad_pt = cx / rect.width
if self._torque_filter.x < 0:
end_grad_pt = (cx * (1 - 0.65) + (min(bg_pts[:, 0]) * 0.65)) / rect.width
else:
end_grad_pt = (cx * (1 - 0.65) + (max(bg_pts[:, 0]) * 0.65)) / rect.width
# fade to orange as we approach max torque
start_color = blend_colors(
rl.Color(255, 255, 255, int(255 * 0.9 * self._torque_line_alpha_filter.x)),
rl.Color(255, 200, 0, int(255 * self._torque_line_alpha_filter.x)), # yellow
max(0, abs(self._torque_filter.x) - 0.75) * 4,
)
end_color = blend_colors(
rl.Color(255, 255, 255, int(255 * 0.9 * self._torque_line_alpha_filter.x)),
rl.Color(255, 115, 0, int(255 * self._torque_line_alpha_filter.x)), # orange
max(0, abs(self._torque_filter.x) - 0.75) * 4,
)
if ui_state.status not in (UIStatus.ENGAGED, UIStatus.LAT_ONLY) and not self._demo:
start_color = end_color = rl.Color(255, 255, 255, int(255 * 0.35 * self._torque_line_alpha_filter.x))
gradient = Gradient(
start=(start_grad_pt, 0),
end=(end_grad_pt, 0),
colors=[
start_color,
end_color,
],
stops=[0.0, 1.0],
)
draw_polygon(rect, sl_pts, gradient=gradient)
# draw center torque bar dot
if abs(self._torque_filter.x) < 0.5:
dot_y = self._rect.y + self._rect.height - torque_line_offset - torque_line_height / 2
rl.draw_circle(int(cx), int(dot_y), (10 // 2 * self._scale),
rl.Color(182, 182, 182, int(255 * 0.9 * self._torque_line_alpha_filter.x)))