forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ 0798119
This commit is contained in:
IQ.Lvbs history cleanup
12
selfdrive/ui/mici/onroad/__init__.py
Normal file
12
selfdrive/ui/mici/onroad/__init__.py
Normal file
@@ -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))
|
||||
369
selfdrive/ui/mici/onroad/alert_renderer.py
Normal file
369
selfdrive/ui/mici/onroad/alert_renderer.py
Normal file
@@ -0,0 +1,369 @@
|
||||
import time
|
||||
from enum import StrEnum
|
||||
from typing import NamedTuple
|
||||
import pyray as rl
|
||||
import random
|
||||
import string
|
||||
from dataclasses import dataclass
|
||||
from cereal import messaging, log, car
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.common.filter_simple import BounceFilter, FirstOrderFilter
|
||||
from openpilot.system.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
|
||||
|
||||
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
|
||||
|
||||
|
||||
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="IQ.Pilot 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):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
|
||||
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_right.png', 104, 96)
|
||||
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_right.png', 134, 150)
|
||||
|
||||
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
|
||||
|
||||
event_name = ss.alertType.split('/')[0] if ss.alertType else ''
|
||||
if event_name in {'selfdrivedLagging', 'commIssue', 'commIssueAvgFreq'}:
|
||||
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_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
|
||||
|
||||
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) 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.iqpilot_ui():
|
||||
ui_state.onroad_brightness_handle_alerts(ui_state.started, 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)
|
||||
|
||||
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
|
||||
|
||||
# re-derive dt every frame: this filter is constructed at offroad startup (60fps) but onroad
|
||||
# runs at a lower target_fps, and a dt frozen from construction decays far too slowly against
|
||||
# the real frame cadence, so the icon never dims and reads as static-on instead of blinking.
|
||||
self._turn_signal_alpha_filter.dt = 1 / gui_app.target_fps
|
||||
self._turn_signal_alpha_filter.update_alpha(0.3)
|
||||
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 = 255
|
||||
else:
|
||||
icon_alpha = int(min(self._turn_signal_alpha_filter.x, 255))
|
||||
|
||||
rl.draw_texture(alert_layout.icon.texture, pos_x, int(self._rect.y + alert_layout.icon.margin_y),
|
||||
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)
|
||||
492
selfdrive/ui/mici/onroad/augmented_road_view.py
Normal file
492
selfdrive/ui/mici/onroad/augmented_road_view.py
Normal file
@@ -0,0 +1,492 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
"""
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from cereal import messaging, car, log
|
||||
from openpilot.common.params import Params
|
||||
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.issue_debug import log_issue_limited
|
||||
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 openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
|
||||
from enum import IntEnum
|
||||
from openpilot.iqpilot.ui.onroad.augmented_road_view import BORDER_COLORS_IQ
|
||||
|
||||
if gui_app.iqpilot_ui():
|
||||
from openpilot.iqpilot.ui.mici.onroad.hud_renderer import IQMiciHudRenderer as HudRenderer
|
||||
from openpilot.iqpilot.ui.mici.onroad.road_label import RoadNameRendererMici
|
||||
from openpilot.selfdrive.ui.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
|
||||
MICI_BORDER_COLOR = rl.Color(0x0C, 0x94, 0x96, 0xFF)
|
||||
MICI_BORDER_THICKNESS = 50
|
||||
MICI_BORDER_ROUNDNESS = 0.2 * 1.02
|
||||
MICI_BORDER_BOTTOM_ONLY_HEIGHT = 95
|
||||
MICI_EXPERIMENTAL_ICON_SIZE = 28
|
||||
MICI_EXPERIMENTAL_ICON_SPACING = 8
|
||||
|
||||
|
||||
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._icon_fill = gui_app.texture("icons_mici/onroad/bookmark_fill.png", 180, 180)
|
||||
self._active_icon = self._icon
|
||||
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
|
||||
self._active_icon = self._icon
|
||||
|
||||
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
|
||||
self._active_icon = self._icon
|
||||
|
||||
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._active_icon = self._icon_fill
|
||||
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._active_icon.height) / 2 # Vertically centered
|
||||
rl.draw_texture(self._active_icon, int(icon_x), int(icon_y), 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._params = Params()
|
||||
self._iq_dynamic_mode: bool = False
|
||||
self._iq_dynamic_refresh: int = 0
|
||||
|
||||
self._model_renderer = ModelRenderer()
|
||||
self._hud_renderer = HudRenderer()
|
||||
self._alert_renderer = AlertRenderer()
|
||||
self._driver_state_renderer = DriverStateRenderer()
|
||||
self._confidence_ball = ConfidenceBall()
|
||||
self._road_name = RoadNameRendererMici() if gui_app.iqpilot_ui() else None
|
||||
self._experimental_txt = gui_app.texture("icons_mici/experimental_mode_mici.png",
|
||||
MICI_EXPERIMENTAL_ICON_SIZE,
|
||||
MICI_EXPERIMENTAL_ICON_SIZE)
|
||||
self._iqdynamic_txt = gui_app.texture("icons_mici/iqdynamic_mode_mici.png",
|
||||
MICI_EXPERIMENTAL_ICON_SIZE,
|
||||
MICI_EXPERIMENTAL_ICON_SIZE)
|
||||
self._iqstandard_txt = gui_app.texture("icons_mici/iqstandard_mode_mici.png",
|
||||
MICI_EXPERIMENTAL_ICON_SIZE,
|
||||
MICI_EXPERIMENTAL_ICON_SIZE)
|
||||
self._offroad_label = UnifiedLabel("start the car to\nuse IQ.Pilot", 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)
|
||||
|
||||
# debug
|
||||
self._pm = messaging.PubMaster(['uiDebug'])
|
||||
|
||||
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()
|
||||
# IQDynamicMode only changes from the settings UI; don't pay a Params syscall every frame on
|
||||
# the onroad hot path. Refresh ~1s (60 frames), matching model_renderer's throttled reads.
|
||||
self._iq_dynamic_refresh -= 1
|
||||
if self._iq_dynamic_refresh <= 0:
|
||||
self._iq_dynamic_refresh = 60
|
||||
self._iq_dynamic_mode = self._params.get_bool("IQDynamicMode")
|
||||
|
||||
# update offroad label
|
||||
if ui_state.panda_type == log.PandaState.PandaType.unknown:
|
||||
self._offroad_label.set_text("system booting")
|
||||
else:
|
||||
self._offroad_label.set_text("start the car to\nuse IQ.Pilot")
|
||||
|
||||
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, _):
|
||||
start_draw = time.monotonic()
|
||||
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)
|
||||
|
||||
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.is_onroad() 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)
|
||||
# TODO: have alert renderer draw offroad mici label below
|
||||
if ui_state.started:
|
||||
self._alert_renderer.render(self._content_rect)
|
||||
self._hud_renderer.render(self._content_rect)
|
||||
if self._road_name is not None and alert_to_render is None:
|
||||
self._road_name.update()
|
||||
self._road_name.render(self._content_rect)
|
||||
# don't draw the experimental/IQ.Dynamic icon over alert text (it falls back to the
|
||||
# top-left alert anchor when the DMoji is hidden while disengaged)
|
||||
if alert_to_render is None:
|
||||
self._draw_experimental_icon(should_draw_dmoji)
|
||||
|
||||
# End clipping region
|
||||
rl.end_scissor_mode()
|
||||
|
||||
self._draw_border()
|
||||
|
||||
# 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)
|
||||
|
||||
draw_time_ms = (time.monotonic() - start_draw) * 1000
|
||||
if draw_time_ms > 40.0:
|
||||
log_issue_limited(
|
||||
"ui_draw_slow_mici",
|
||||
"ui",
|
||||
f"mici onroad draw slow drawTimeMillis={draw_time_ms:.2f} navActive={getattr(ui_state.sm['iqNavState'], 'active', False)}",
|
||||
interval_sec=1.0,
|
||||
)
|
||||
msg = messaging.new_message('uiDebug')
|
||||
msg.uiDebug.drawTimeMillis = draw_time_ms
|
||||
self._pm.send('uiDebug', msg)
|
||||
|
||||
# Draw darkened background and text if not onroad
|
||||
if not ui_state.started:
|
||||
rl.draw_rectangle(int(self.rect.x), int(self.rect.y), int(self.rect.width), int(self.rect.height), rl.Color(0, 0, 0, 175))
|
||||
self._offroad_label.render(self._content_rect)
|
||||
|
||||
def _draw_experimental_icon(self, draw_below_driver_state: bool) -> None:
|
||||
if not ui_state.started:
|
||||
return
|
||||
|
||||
if not ui_state.sm['carParams'].openpilotLongitudinalControl:
|
||||
return
|
||||
|
||||
if ui_state.sm['selfdriveState'].experimentalMode:
|
||||
icon = self._iqdynamic_txt if self._iq_dynamic_mode else self._experimental_txt
|
||||
else:
|
||||
icon = self._iqstandard_txt
|
||||
|
||||
if draw_below_driver_state:
|
||||
pos_x = self._rect.x + 16 + (self._driver_state_renderer.rect.width - icon.width) / 2
|
||||
pos_y = self._rect.y + 10 + self._driver_state_renderer.rect.height + MICI_EXPERIMENTAL_ICON_SPACING
|
||||
else:
|
||||
pos_x = self._rect.x + 18
|
||||
pos_y = self._rect.y + 18
|
||||
|
||||
rl.draw_texture(icon, int(pos_x), int(pos_y), rl.WHITE)
|
||||
|
||||
def _draw_border(self):
|
||||
rl.draw_rectangle_rounded_lines_ex(self._content_rect, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, rl.BLACK)
|
||||
|
||||
aol = ui_state.sm["iqState"].aol
|
||||
ss_enabled = ui_state.sm["selfdriveState"].enabled
|
||||
if aol.active and ss_enabled:
|
||||
rl.draw_rectangle_rounded_lines_ex(self._content_rect, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, MICI_BORDER_COLOR)
|
||||
self._reblacken_border_edges()
|
||||
elif aol.active and not ss_enabled:
|
||||
clip_y = int(self._content_rect.y + self._content_rect.height - MICI_BORDER_BOTTOM_ONLY_HEIGHT)
|
||||
rl.begin_scissor_mode(int(self._content_rect.x), clip_y,
|
||||
int(self._content_rect.width), MICI_BORDER_BOTTOM_ONLY_HEIGHT)
|
||||
border_color = BORDER_COLORS_IQ[UIStatus.LAT_ONLY] if ui_state.status != UIStatus.OVERRIDE else rl.Color(0x89, 0x92, 0x8D, 0xFF)
|
||||
rl.draw_rectangle_rounded_lines_ex(self._content_rect, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, border_color)
|
||||
rl.end_scissor_mode()
|
||||
self._reblacken_border_edges()
|
||||
|
||||
def _reblacken_border_edges(self):
|
||||
cr = self._content_rect
|
||||
r = int(MICI_BORDER_ROUNDNESS * min(cr.width, cr.height) / 2) + MICI_BORDER_THICKNESS + 6
|
||||
regions = (
|
||||
(cr.x, cr.y, r, r), # top-left corner
|
||||
(cr.x, cr.y + cr.height - r, r, r), # bottom-left corner
|
||||
(cr.x + cr.width - r, cr.y, r + SIDE_PANEL_WIDTH, cr.height), # right edge + both right corners
|
||||
)
|
||||
for rx, ry, rw, rh in regions:
|
||||
rl.begin_scissor_mode(int(rx), int(ry), int(rw), int(rh))
|
||||
rl.draw_rectangle_rounded_lines_ex(cr, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, rl.BLACK)
|
||||
rl.end_scissor_mode()
|
||||
|
||||
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))]
|
||||
|
||||
if not sm.seen["liveCalibration"]:
|
||||
return
|
||||
|
||||
calib = sm['liveCalibration']
|
||||
calib_rpy = get_calibrated_rpy(calib)
|
||||
if calib_rpy is None:
|
||||
return
|
||||
|
||||
# Update view_from_calib matrix
|
||||
prev_view_from_calib = self.view_from_calib.copy()
|
||||
prev_view_from_wide_calib = self.view_from_wide_calib.copy()
|
||||
device_from_calib = rot_from_euler(calib_rpy)
|
||||
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
|
||||
|
||||
if (not np.allclose(self.view_from_calib, prev_view_from_calib) or
|
||||
not np.allclose(self.view_from_wide_calib, prev_view_from_wide_calib)):
|
||||
self._matrix_cache_key = (0, 0, 0, self.stream_type, 0.0)
|
||||
self._cached_matrix = None
|
||||
|
||||
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
|
||||
# Early-return the cached matrix when nothing that affects it changed. The key deliberately
|
||||
# excludes rect.x/y — those are applied as a draw-time offset in ModelRenderer (below), so the
|
||||
# cache stays hot while the onroad view translates during a scroll/transition (stock PR #37948).
|
||||
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 (rect.x/y are NOT used here — applied at draw time)
|
||||
w, h = self._content_rect.width, self._content_rect.height
|
||||
cx, cy = intrinsic[0, 2], intrinsic[1, 2]
|
||||
|
||||
# Calculate max allowed offsets with margins
|
||||
margin = 5
|
||||
max_x_offset = cx * zoom - w / 2 - margin
|
||||
max_y_offset = 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 the matrix (and the model_renderer projection it drives) stays
|
||||
# cache-stable while the view slides; ModelRenderer adds (rect.x, rect.y) as a draw-time offset.
|
||||
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.iqpilot_ui():
|
||||
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)
|
||||
|
||||
def hide_event(self):
|
||||
if gui_app.iqpilot_ui():
|
||||
ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
gui_app.init_window("OnRoad Camera View")
|
||||
road_camera_view = AugmentedRoadView(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()
|
||||
417
selfdrive/ui/mici/onroad/cameraview.py
Normal file
417
selfdrive/ui/mici/onroad/cameraview.py
Normal file
@@ -0,0 +1,417 @@
|
||||
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.system.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):
|
||||
# Reconnect if not first time going onroad
|
||||
if ui_state.is_onroad() and self.frame is not None:
|
||||
# Prevent old frames from showing when going onroad. Qt has a separate thread
|
||||
# which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough
|
||||
# and only clears internal buffers, not the message queue.
|
||||
self.frame = None
|
||||
self.available_streams.clear()
|
||||
if self.client:
|
||||
del self.client
|
||||
self.client = VisionIpcClient(self._name, self._stream_type, conflate=True)
|
||||
|
||||
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 *", y_data.ctypes.data))
|
||||
rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.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))
|
||||
86
selfdrive/ui/mici/onroad/confidence_ball.py
Normal file
86
selfdrive/ui/mici/onroad/confidence_ball.py
Normal file
@@ -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.iqpilot.ui.mici.onroad.confidence_ball import IQConfidenceBall
|
||||
|
||||
|
||||
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, IQConfidenceBall):
|
||||
def __init__(self, demo: bool = False):
|
||||
Widget.__init__(self)
|
||||
IQConfidenceBall.__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_colors(self._confidence_filter.x)
|
||||
|
||||
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)
|
||||
246
selfdrive/ui/mici/onroad/driver_camera_dialog.py
Normal file
246
selfdrive/ui/mici/onroad/driver_camera_dialog.py
Normal file
@@ -0,0 +1,246 @@
|
||||
import pyray as rl
|
||||
from 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.selfdrive.selfdrived.events import EVENTS, ET
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.widgets.nav_widget import NavWidget
|
||||
from openpilot.system.ui.widgets.label import gui_label
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
EVENT_TO_INT = EventName.schema.enumerants
|
||||
|
||||
|
||||
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 DriverCameraDialog(NavWidget):
|
||||
def __init__(self, no_escape=False):
|
||||
super().__init__()
|
||||
self._no_escape = no_escape
|
||||
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
|
||||
if not no_escape:
|
||||
# TODO: this can grow unbounded, should be given some thought
|
||||
device.add_interactive_timeout_callback(lambda: gui_app.set_modal_overlay(None))
|
||||
self.set_back_callback(lambda: gui_app.set_modal_overlay(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 _back_enabled(self) -> bool:
|
||||
return not self._no_escape
|
||||
|
||||
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 -1
|
||||
|
||||
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 -1
|
||||
|
||||
def _publish_alert_sound(self, dm_state):
|
||||
"""Publish selfdriveState with only alertSound field set"""
|
||||
if self._pm is None:
|
||||
return
|
||||
|
||||
msg = messaging.new_message('selfdriveState')
|
||||
if dm_state is not None and len(dm_state.events):
|
||||
event_name = EVENT_TO_INT[dm_state.events[0].name]
|
||||
if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]:
|
||||
msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert
|
||||
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)
|
||||
|
||||
gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height),
|
||||
f"Awareness: {dm_state.awarenessStatus * 100:.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: {dm_state.awarenessStatus * 100:.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 not dm_state.events:
|
||||
return
|
||||
|
||||
# Show first event (only one should be active at a time)
|
||||
event_name_str = str(dm_state.events[0].name).split('.')[-1]
|
||||
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, event_name_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, event_name_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.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)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
gui_app.init_window("Driver Camera View (mici)")
|
||||
|
||||
driver_camera_view = DriverCameraDialog()
|
||||
try:
|
||||
for _ in gui_app.render():
|
||||
ui_state.update()
|
||||
driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
|
||||
finally:
|
||||
driver_camera_view.close()
|
||||
213
selfdrive/ui/mici/onroad/driver_state.py
Normal file
213
selfdrive/ui/mici/onroad/driver_state.py
Normal file
@@ -0,0 +1,213 @@
|
||||
import pyray as rl
|
||||
import numpy as np
|
||||
import math
|
||||
from 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
|
||||
from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
|
||||
DEBUG = False
|
||||
ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF)
|
||||
|
||||
LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6)
|
||||
LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3)
|
||||
|
||||
|
||||
class DriverStateRenderer(Widget):
|
||||
BASE_SIZE = 60
|
||||
LINES_ANGLE_INCREMENT = 5
|
||||
LINES_STALE_ANGLES = 3.0 # seconds
|
||||
|
||||
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._should_draw = False
|
||||
self._force_active = False
|
||||
self._looking_center = False
|
||||
|
||||
self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps)
|
||||
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):
|
||||
cone_and_person_size = round(56 / self.BASE_SIZE * self._rect.width)
|
||||
|
||||
if self._inset:
|
||||
current_inset = (self._rect.width - cone_and_person_size) / 2
|
||||
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", self._rect.width, 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(self._dm_background,
|
||||
int(self._rect.x),
|
||||
int(self._rect.y),
|
||||
rl.Color(255, 255, 255, int(255 * self._fade_filter.x)))
|
||||
|
||||
rl.draw_texture(self._dm_person,
|
||||
int(self._rect.x + (self._rect.width - self._dm_person.width) / 2),
|
||||
int(self._rect.y + (self._rect.height - self._dm_person.height) / 2),
|
||||
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
|
||||
|
||||
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:
|
||||
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(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.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 = ACTIVE_ACCENT
|
||||
|
||||
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.isActiveMode
|
||||
self._is_rhd = dm_state.isRHD
|
||||
self._face_detected = dm_state.faceDetected
|
||||
|
||||
driverstate = sm["driverStateV2"]
|
||||
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
|
||||
return driver_data
|
||||
|
||||
def _update_state(self):
|
||||
# Get monitoring state
|
||||
driver_data = self.get_driver_data()
|
||||
driver_orient = driver_data.faceOrientation
|
||||
|
||||
if len(driver_orient) != 3:
|
||||
return
|
||||
|
||||
# Calibrate orientation so looking straight ahead at the road (instead of at the device) reads
|
||||
# (0, 0), using live calibration. Makes the cone point in the correct direction. (stock PR #37149)
|
||||
sm = ui_state.sm
|
||||
if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3:
|
||||
cal_rpy = sm['liveCalibration'].rpyCalib
|
||||
else:
|
||||
cal_rpy = [0.0, 0.0, 0.0]
|
||||
_, pitch, yaw = face_orientation_from_net(driver_orient, driver_data.facePosition, cal_rpy)
|
||||
yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention
|
||||
|
||||
pitch = self._pitch_filter.update(pitch)
|
||||
yaw = self._yaw_filter.update(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)
|
||||
|
||||
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 (bias pitch up since calib/DM pose isn't exact,
|
||||
# and halve yaw sensitivity)
|
||||
rotation = math.degrees(math.atan2((pitch + math.radians(6)) * 2, yaw))
|
||||
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)
|
||||
279
selfdrive/ui/mici/onroad/hud_renderer.py
Normal file
279
selfdrive/ui/mici/onroad/hud_renderer.py
Normal file
@@ -0,0 +1,279 @@
|
||||
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.raylib_compat import draw_circle_gradient
|
||||
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 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_right.png', 50, 20)
|
||||
|
||||
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', 44, 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.vCruiseDEPRECATED 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.iqpilot_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(self._txt_exclamation_point, int(exclamation_pos_x), int(exclamation_pos_y), 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
|
||||
draw_circle_gradient(int(x + circle_radius), int(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)
|
||||
514
selfdrive/ui/mici/onroad/model_renderer.py
Normal file
514
selfdrive/ui/mici/onroad/model_renderer.py
Normal file
@@ -0,0 +1,514 @@
|
||||
import colorsys
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from cereal import messaging, 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.iqpilot.ui.onroad.hud_overlays import ChevronMetrics
|
||||
from openpilot.iqpilot.ui.onroad.lead_confidence import driving_confidence
|
||||
from openpilot.selfdrive.locationd.calibration_helpers import get_render_path_height
|
||||
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.iqpilot.ui.mici.onroad.model_renderer import IQ_LANE_LINE_COLORS
|
||||
from openpilot.iqpilot.ui.theme import NeonTheme
|
||||
|
||||
# Engaged-ish statuses whose lane lines follow the live Konn3kt accent color (UIAccentColor)
|
||||
# instead of a baked-in teal, so changing the color in Konn3kt updates them without a reboot.
|
||||
ACCENT_LANE_LINE_STATUSES = (UIStatus.ENGAGED, UIStatus.LAT_ONLY, UIStatus.LONG_ONLY)
|
||||
|
||||
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(0x0C, 0x94, 0x96, 0xFF),
|
||||
**IQ_LANE_LINE_COLORS,
|
||||
}
|
||||
|
||||
|
||||
@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:
|
||||
center: tuple[float, float] | None = None
|
||||
radius: float = 0.0
|
||||
sz: float = 0.0
|
||||
fill_alpha: int = 0
|
||||
|
||||
|
||||
class ModelRenderer(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.chevron_metrics = ChevronMetrics()
|
||||
self._lead_orb = gui_app.texture("icons/lead_orb.png", 256, 256)
|
||||
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)
|
||||
|
||||
# Transform matrix (3x3 for car space to screen space)
|
||||
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
|
||||
driving_confidence.update()
|
||||
|
||||
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 = get_render_path_height(live_calib)
|
||||
|
||||
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()
|
||||
self.chevron_metrics.draw_lead_status(sm, radar_state, self._rect, self._lead_vehicles)
|
||||
|
||||
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.status:
|
||||
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.status:
|
||||
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
|
||||
|
||||
# reconstruct absolute (screen) points so the rect-space cull below stays correct
|
||||
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. Distance-scaled orb radius (closer lead -> bigger orb).
|
||||
sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 1
|
||||
radius = sz * 1.1
|
||||
# point is in absolute screen coords; clamp against the rect's absolute bounds so the orb stays
|
||||
# fully on-screen (rect-relative bounds mis-placed it when the camera pane is offset, e.g. split nav)
|
||||
x = np.clip(point[0], rect.x + radius, rect.x + rect.width - radius)
|
||||
y = np.clip(point[1], rect.y + radius, rect.y + rect.height - radius)
|
||||
|
||||
return LeadVehicle(center=(float(x), float(y)), radius=float(radius), sz=float(sz), 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:
|
||||
# Active states track the live accent color; others (disengaged/override) stay fixed.
|
||||
if ui_state.status in ACCENT_LANE_LINE_STATUSES:
|
||||
_base_color = NeonTheme.glow(255)
|
||||
else:
|
||||
_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)"""
|
||||
# projected_points are origin-relative (rect.x/y kept out of the transform so it stays cached);
|
||||
# translate to the view's screen position here.
|
||||
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
|
||||
|
||||
# projected_points are origin-relative; translate to the view's screen position
|
||||
path_pts = self._path.projected_points + np.array([self._rect.x, self._rect.y], dtype=np.float32)
|
||||
|
||||
allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
|
||||
self._blend_filter.update(int(allow_throttle))
|
||||
|
||||
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):
|
||||
tint, _ = driving_confidence.colors()
|
||||
src = rl.Rectangle(0, 0, self._lead_orb.width, self._lead_orb.height)
|
||||
for lead in self._lead_vehicles:
|
||||
if lead.center is None:
|
||||
continue
|
||||
cx, cy = lead.center
|
||||
r = lead.radius
|
||||
alpha = int(np.clip(140 + 115 * (lead.fill_alpha / 255.0), 0, 255))
|
||||
dest = rl.Rectangle(cx, cy, r * 2.0, r * 2.0)
|
||||
rl.draw_texture_pro(self._lead_orb, src, dest, rl.Vector2(r, r), 0.0, rl.Color(tint.r, tint.g, tint.b, 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)]
|
||||
263
selfdrive/ui/mici/onroad/torque_bar.py
Normal file
263
selfdrive/ui/mici/onroad/torque_bar.py
Normal file
@@ -0,0 +1,263 @@
|
||||
import math
|
||||
import time
|
||||
from functools import wraps
|
||||
from collections import OrderedDict
|
||||
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
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
|
||||
ANGLE_ARC_MAX_DEG = 45.0
|
||||
|
||||
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. The arc is computed at
|
||||
# the origin and translated at the call site, so cx/cy are NOT part of the key — that keeps
|
||||
# the cache hot while the bar translates during a scroll/transition (stock PR #37946).
|
||||
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.
|
||||
The caller translates the returned points by (cx, cy) so this can stay cached while the bar moves."""
|
||||
|
||||
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 (origin-centered)
|
||||
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
|
||||
|
||||
|
||||
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() == 'angleState':
|
||||
controls_state = ui_state.sm['controlsState']
|
||||
car_control = ui_state.sm['carControl']
|
||||
|
||||
if not car_control.latActive:
|
||||
self._torque_filter.update(0.0)
|
||||
else:
|
||||
desired_angle = controls_state.lateralControlState.angleState.steeringAngleDesiredDeg
|
||||
angle_offset = ui_state.sm['liveParameters'].angleOffsetAverageDeg
|
||||
# Angle-control cars should render the steering arc from the requested angle
|
||||
# directly, not from curvature/lateral acceleration, which collapses at low speed.
|
||||
# Subtract the liveParameters angle offset so the bar reads zero when going straight
|
||||
# despite sensor misalignment.
|
||||
self._torque_filter.update(np.clip(-(desired_angle - angle_offset) / ANGLE_ARC_MAX_DEG, -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
|
||||
# arc_bar_pts is origin-centered + cached; translate to (cx, cy) here so the cache stays hot
|
||||
# while the bar slides during a scroll/transition.
|
||||
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 the requested accent colors 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 (match stock)
|
||||
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 (match stock)
|
||||
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)))
|
||||
Reference in New Issue
Block a user