forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Prebuilt Release @ ab07000
This commit is contained in:
IQ.Lvbs history cleanup
0
selfdrive/ui/onroad/__init__.py
Normal file
0
selfdrive/ui/onroad/__init__.py
Normal file
186
selfdrive/ui/onroad/alert_renderer.py
Normal file
186
selfdrive/ui/onroad/alert_renderer.py
Normal file
@@ -0,0 +1,186 @@
|
||||
import time
|
||||
import pyray as rl
|
||||
from dataclasses import dataclass
|
||||
from cereal import messaging, log
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.hardware import TICI
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.system.ui.widgets.label import Label
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
AlertStatus = log.SelfdriveState.AlertStatus
|
||||
|
||||
ALERT_MARGIN = 40
|
||||
ALERT_PADDING = 60
|
||||
ALERT_LINE_SPACING = 45
|
||||
ALERT_BORDER_RADIUS = 30
|
||||
|
||||
ALERT_FONT_SMALL = 66
|
||||
ALERT_FONT_MEDIUM = 74
|
||||
ALERT_FONT_BIG = 88
|
||||
|
||||
ALERT_HEIGHTS = {
|
||||
AlertSize.small: 271,
|
||||
AlertSize.mid: 420,
|
||||
}
|
||||
|
||||
SELFDRIVE_STATE_TIMEOUT = 5 # Seconds
|
||||
SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds
|
||||
|
||||
# Constants
|
||||
ALERT_COLORS = {
|
||||
AlertStatus.normal: rl.Color(0x15, 0x15, 0x15, 0xF1), # #151515 with alpha 0xF1
|
||||
AlertStatus.userPrompt: rl.Color(0xDA, 0x6F, 0x25, 0xF1), # #DA6F25 with alpha 0xF1
|
||||
AlertStatus.critical: rl.Color(0xC9, 0x22, 0x31, 0xF1), # #C92231 with alpha 0xF1
|
||||
}
|
||||
|
||||
|
||||
@dataclass
|
||||
class Alert:
|
||||
text1: str = ""
|
||||
text2: str = ""
|
||||
size: int = 0
|
||||
status: int = 0
|
||||
|
||||
|
||||
# Pre-defined alert instances
|
||||
ALERT_STARTUP_PENDING = Alert(
|
||||
text1=tr("IQ.Pilot Unavailable"),
|
||||
text2=tr("Waiting to start"),
|
||||
size=AlertSize.mid,
|
||||
status=AlertStatus.normal,
|
||||
)
|
||||
|
||||
ALERT_CRITICAL_TIMEOUT = Alert(
|
||||
text1=tr("TAKE CONTROL IMMEDIATELY"),
|
||||
text2=tr("System Unresponsive"),
|
||||
size=AlertSize.full,
|
||||
status=AlertStatus.critical,
|
||||
)
|
||||
|
||||
ALERT_CRITICAL_REBOOT = Alert(
|
||||
text1=tr("System Unresponsive"),
|
||||
text2=tr("Reboot Device"),
|
||||
size=AlertSize.mid,
|
||||
status=AlertStatus.normal,
|
||||
)
|
||||
|
||||
|
||||
class AlertRenderer(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.font_regular: rl.Font = gui_app.font(FontWeight.NORMAL)
|
||||
self.font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
|
||||
|
||||
# font size is set dynamically
|
||||
self._full_text1_label = Label("", font_size=0, font_weight=FontWeight.BOLD, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
|
||||
text_alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP)
|
||||
self._full_text2_label = Label("", font_size=ALERT_FONT_BIG, text_alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
|
||||
text_alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP)
|
||||
|
||||
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
|
||||
recv_frame = sm.recv_frame['selfdriveState']
|
||||
if not sm.updated['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
|
||||
|
||||
# Don't get old alert
|
||||
if recv_frame < ui_state.started_frame:
|
||||
return None
|
||||
|
||||
event_name = ss.alertType.split('/')[0] if ss.alertType else ''
|
||||
if event_name in {'selfdrivedLagging', 'commIssue', 'commIssueAvgFreq'}:
|
||||
return None
|
||||
|
||||
# Return current alert
|
||||
return Alert(text1=ss.alertText1, text2=ss.alertText2, size=ss.alertSize.raw, status=ss.alertStatus.raw)
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
alert = self.get_alert(ui_state.sm)
|
||||
|
||||
if gui_app.iqpilot_ui():
|
||||
ui_state.onroad_brightness_handle_alerts(ui_state.started, alert)
|
||||
|
||||
if not alert:
|
||||
return
|
||||
|
||||
alert_rect = self._get_alert_rect(rect, alert.size)
|
||||
self._draw_background(alert_rect, alert)
|
||||
|
||||
text_rect = rl.Rectangle(
|
||||
alert_rect.x + ALERT_PADDING,
|
||||
alert_rect.y + ALERT_PADDING,
|
||||
alert_rect.width - 2 * ALERT_PADDING,
|
||||
alert_rect.height - 2 * ALERT_PADDING
|
||||
)
|
||||
self._draw_text(text_rect, alert)
|
||||
|
||||
def _get_alert_rect(self, rect: rl.Rectangle, size: int) -> rl.Rectangle:
|
||||
if size == AlertSize.full:
|
||||
return rect
|
||||
|
||||
h = ALERT_HEIGHTS.get(size, rect.height)
|
||||
return rl.Rectangle(rect.x + ALERT_MARGIN, rect.y + rect.height - h + ALERT_MARGIN,
|
||||
rect.width - ALERT_MARGIN * 2, h - ALERT_MARGIN * 2)
|
||||
|
||||
def _draw_background(self, rect: rl.Rectangle, alert: Alert) -> None:
|
||||
color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
|
||||
|
||||
if alert.size != AlertSize.full:
|
||||
roundness = ALERT_BORDER_RADIUS / (min(rect.width, rect.height) / 2)
|
||||
rl.draw_rectangle_rounded(rect, roundness, 10, color)
|
||||
else:
|
||||
rl.draw_rectangle_rec(rect, color)
|
||||
|
||||
def _draw_text(self, rect: rl.Rectangle, alert: Alert) -> None:
|
||||
if alert.size == AlertSize.small:
|
||||
self._draw_centered(alert.text1, rect, self.font_bold, ALERT_FONT_MEDIUM)
|
||||
|
||||
elif alert.size == AlertSize.mid:
|
||||
self._draw_centered(alert.text1, rect, self.font_bold, ALERT_FONT_BIG, center_y=False)
|
||||
rect.y += ALERT_FONT_BIG + ALERT_LINE_SPACING
|
||||
self._draw_centered(alert.text2, rect, self.font_regular, ALERT_FONT_SMALL, center_y=False)
|
||||
|
||||
else:
|
||||
is_long = len(alert.text1) > 15
|
||||
font_size1 = 132 if is_long else 177
|
||||
|
||||
top_offset = 200 if is_long or '\n' in alert.text1 else 270
|
||||
title_rect = rl.Rectangle(rect.x, rect.y + top_offset, rect.width, 600)
|
||||
self._full_text1_label.set_font_size(font_size1)
|
||||
self._full_text1_label.set_text(alert.text1)
|
||||
self._full_text1_label.render(title_rect)
|
||||
|
||||
bottom_offset = 361 if is_long else 420
|
||||
subtitle_rect = rl.Rectangle(rect.x, rect.y + rect.height - bottom_offset, rect.width, 300)
|
||||
self._full_text2_label.set_text(alert.text2)
|
||||
self._full_text2_label.render(subtitle_rect)
|
||||
|
||||
def _draw_centered(self, text, rect, font, font_size, center_y=True, color=rl.WHITE) -> None:
|
||||
text_size = measure_text_cached(font, text, font_size)
|
||||
x = rect.x + (rect.width - text_size.x) / 2
|
||||
y = rect.y + ((rect.height - text_size.y) / 2 if center_y else 0)
|
||||
rl.draw_text_ex(font, text, rl.Vector2(x, y), font_size, 0, color)
|
||||
312
selfdrive/ui/onroad/augmented_road_view.py
Normal file
312
selfdrive/ui/onroad/augmented_road_view.py
Normal file
@@ -0,0 +1,312 @@
|
||||
import time
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from cereal import log, messaging
|
||||
from msgq.visionipc import VisionStreamType
|
||||
from openpilot.selfdrive.ui import UI_BORDER_SIZE
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.selfdrive.ui.onroad.alert_renderer import AlertRenderer
|
||||
from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer as BaseDriverStateRenderer, BTN_SIZE
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import HudRenderer as BaseHudRenderer
|
||||
from openpilot.selfdrive.ui.onroad.model_renderer import ModelRenderer
|
||||
from openpilot.selfdrive.ui.onroad.environment_renderer import EnvironmentRenderer
|
||||
from openpilot.selfdrive.ui.onroad.cameraview import CameraView
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.common.issue_debug import log_issue_limited
|
||||
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 openpilot.iqpilot.ui.onroad.augmented_road_view import BORDER_COLORS_IQ, AugmentedRoadViewIQ
|
||||
from openpilot.iqpilot.ui.onroad.driver_state import DriverStateRendererIQ
|
||||
from openpilot.iqpilot.ui.onroad.hud_renderer import IQHudRenderer
|
||||
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"]
|
||||
|
||||
BORDER_COLORS = {
|
||||
UIStatus.DISENGAGED: rl.Color(0x12, 0x28, 0x39, 0xFF), # Blue for disengaged state
|
||||
UIStatus.OVERRIDE: rl.Color(0x89, 0x92, 0x8D, 0xFF), # Gray for override state
|
||||
UIStatus.ENGAGED: rl.Color(0x0C, 0x94, 0x96, 0xFF),
|
||||
**BORDER_COLORS_IQ,
|
||||
}
|
||||
|
||||
WIDE_CAM_MAX_SPEED = 10.0 # m/s (22 mph)
|
||||
ROAD_CAM_MIN_SPEED = 15.0 # m/s (34 mph)
|
||||
INF_POINT = np.array([1000.0, 0.0, 0.0])
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView, AugmentedRoadViewIQ):
|
||||
def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD):
|
||||
CameraView.__init__(self, "camerad", stream_type)
|
||||
AugmentedRoadViewIQ.__init__(self)
|
||||
self._set_placeholder_color(BORDER_COLORS[UIStatus.DISENGAGED])
|
||||
|
||||
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 = (0, 0.0, 0.0, stream_type)
|
||||
self._cached_matrix: np.ndarray | None = None
|
||||
self._content_rect = rl.Rectangle()
|
||||
self._split_nav_available = False
|
||||
|
||||
self.model_renderer = ModelRenderer()
|
||||
self.environment_renderer = EnvironmentRenderer()
|
||||
self.alert_renderer = AlertRenderer()
|
||||
self._hud_renderer = IQHudRenderer()
|
||||
self.driver_state_renderer = DriverStateRendererIQ()
|
||||
self._split_nav_available = hasattr(self._hud_renderer, "render_split_nav")
|
||||
|
||||
# debug
|
||||
self._pm = messaging.PubMaster(['uiDebug'])
|
||||
|
||||
def _render(self, rect):
|
||||
# Only render when system is started to avoid invalid data access
|
||||
start_draw = time.monotonic()
|
||||
if not ui_state.started:
|
||||
return
|
||||
|
||||
self._switch_stream_if_needed(ui_state.sm)
|
||||
|
||||
# Update calibration before rendering
|
||||
self._update_calibration()
|
||||
|
||||
# Create inner content area with border padding
|
||||
full_content_rect = rl.Rectangle(
|
||||
rect.x + UI_BORDER_SIZE,
|
||||
rect.y + UI_BORDER_SIZE,
|
||||
rect.width - 2 * UI_BORDER_SIZE,
|
||||
rect.height - 2 * UI_BORDER_SIZE,
|
||||
)
|
||||
split_nav_enabled = bool(getattr(self._hud_renderer, "split_nav_enabled", lambda: False)())
|
||||
if split_nav_enabled:
|
||||
split_width = full_content_rect.width * 0.5
|
||||
camera_rect = rl.Rectangle(full_content_rect.x, full_content_rect.y, split_width, full_content_rect.height)
|
||||
map_rect = rl.Rectangle(full_content_rect.x + split_width, full_content_rect.y, full_content_rect.width - split_width, full_content_rect.height)
|
||||
else:
|
||||
camera_rect = full_content_rect
|
||||
map_rect = None
|
||||
self._content_rect = camera_rect
|
||||
|
||||
if map_rect is not None:
|
||||
self._hud_renderer.render_split_nav(map_rect)
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(map_rect.x, map_rect.y + 20),
|
||||
rl.Vector2(map_rect.x, map_rect.y + map_rect.height - 20),
|
||||
2.0,
|
||||
rl.Color(255, 255, 255, 20),
|
||||
)
|
||||
|
||||
# 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(camera_rect.x),
|
||||
int(camera_rect.y),
|
||||
int(camera_rect.width),
|
||||
int(camera_rect.height)
|
||||
)
|
||||
|
||||
# Render the base camera view
|
||||
super()._render(camera_rect)
|
||||
|
||||
# Draw all UI overlays
|
||||
self.model_renderer.render(camera_rect)
|
||||
self.environment_renderer.render(camera_rect)
|
||||
AugmentedRoadViewIQ.update_fade_out_bottom_overlay(self, camera_rect)
|
||||
self._hud_renderer.render(camera_rect)
|
||||
|
||||
# Custom UI extension point - add custom overlays here
|
||||
# Use self._content_rect for positioning within camera bounds
|
||||
|
||||
# End clipping region
|
||||
rl.end_scissor_mode()
|
||||
|
||||
if hasattr(self._hud_renderer, "render_full_width_overlays"):
|
||||
self._hud_renderer.render_full_width_overlays(full_content_rect)
|
||||
|
||||
self.alert_renderer.render(full_content_rect)
|
||||
self.driver_state_renderer.render(full_content_rect)
|
||||
|
||||
# Draw colored border based on driving state
|
||||
self._draw_border(rect)
|
||||
|
||||
# publish uiDebug
|
||||
draw_time_ms = (time.monotonic() - start_draw) * 1000
|
||||
if draw_time_ms > 40.0:
|
||||
log_issue_limited(
|
||||
"ui_draw_slow",
|
||||
"ui",
|
||||
f"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)
|
||||
|
||||
def _handle_mouse_press(self, mouse_pos):
|
||||
dm = self.driver_state_renderer
|
||||
if ui_state.has_longitudinal_control and dm.is_visible:
|
||||
dx = mouse_pos.x - dm.position_x
|
||||
dy = mouse_pos.y - dm.position_y
|
||||
if dx * dx + dy * dy <= (BTN_SIZE / 2) ** 2:
|
||||
dm.cycle_personality()
|
||||
return
|
||||
|
||||
if not self._hud_renderer.user_interacting() and self._click_callback is not None:
|
||||
self._click_callback()
|
||||
|
||||
def _handle_mouse_release(self, _):
|
||||
# We only call click callback on press if not interacting with HUD
|
||||
pass
|
||||
|
||||
def _draw_border(self, rect: rl.Rectangle):
|
||||
rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, rl.BLACK)
|
||||
border_roundness = 0.12
|
||||
border_color = BORDER_COLORS.get(ui_state.status, BORDER_COLORS[UIStatus.DISENGAGED])
|
||||
border_rect = rl.Rectangle(rect.x + UI_BORDER_SIZE, rect.y + UI_BORDER_SIZE,
|
||||
rect.width - 2 * UI_BORDER_SIZE, rect.height - 2 * UI_BORDER_SIZE)
|
||||
aol = ui_state.sm["iqState"].aol
|
||||
if aol.active and not ui_state.sm["selfdriveState"].enabled:
|
||||
bottom_only_height = max(int(UI_BORDER_SIZE * 4), 60)
|
||||
clip_y = int(rect.y + rect.height - bottom_only_height)
|
||||
rl.begin_scissor_mode(int(rect.x), clip_y, int(rect.width), bottom_only_height)
|
||||
rl.draw_rectangle_rounded_lines_ex(border_rect, border_roundness, 10, UI_BORDER_SIZE, border_color)
|
||||
rl.end_scissor_mode()
|
||||
else:
|
||||
rl.draw_rectangle_rounded_lines_ex(border_rect, border_roundness, 10, UI_BORDER_SIZE, border_color)
|
||||
|
||||
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, 0.0, self.stream_type)
|
||||
self._cached_matrix = None
|
||||
|
||||
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
|
||||
# Check if we can use cached matrix
|
||||
cache_key = (
|
||||
ui_state.sm.recv_frame['liveCalibration'],
|
||||
self._content_rect.width,
|
||||
self._content_rect.height,
|
||||
self.stream_type
|
||||
)
|
||||
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
|
||||
zoom = 2.0 if is_wide_camera else 1.1
|
||||
|
||||
# Calculate transforms for vanishing point
|
||||
calib_transform = intrinsic @ calibration
|
||||
kep = calib_transform @ INF_POINT
|
||||
|
||||
# Calculate center points and dimensions
|
||||
x, y = self._content_rect.x, self._content_rect.y
|
||||
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, -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]
|
||||
])
|
||||
|
||||
video_transform = np.array([
|
||||
[zoom, 0.0, (w / 2 + x - x_offset) - (cx * zoom)],
|
||||
[0.0, zoom, (h / 2 + y - y_offset) - (cy * zoom)],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
self.model_renderer.set_transform(video_transform @ calib_transform)
|
||||
self.model_renderer.set_frame_transform(video_transform, is_wide_camera)
|
||||
self.environment_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()
|
||||
366
selfdrive/ui/onroad/cameraview.py
Normal file
366
selfdrive/ui/onroad/cameraview.py
Normal file
@@ -0,0 +1,366 @@
|
||||
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 EGL_DMA_BUF_SUPPORTED
|
||||
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
|
||||
|
||||
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 EGL_DMA_BUF_SUPPORTED:
|
||||
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;
|
||||
void main() {
|
||||
vec4 color = texture(texture0, fragTexCoord);
|
||||
fragColor = vec4(pow(color.rgb, vec3(1.0/1.28)), color.a);
|
||||
}
|
||||
"""
|
||||
else:
|
||||
FRAME_FRAGMENT_SHADER = VERSION + """
|
||||
in vec2 fragTexCoord;
|
||||
uniform sampler2D texture0;
|
||||
uniform sampler2D texture1;
|
||||
out vec4 fragColor;
|
||||
void main() {
|
||||
float y = texture(texture0, fragTexCoord).r;
|
||||
vec2 uv = texture(texture1, fragTexCoord).ra - 0.5;
|
||||
fragColor = vec4(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x, 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 EGL_DMA_BUF_SUPPORTED else -1
|
||||
|
||||
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 comma 3/3X.
|
||||
if EGL_DMA_BUF_SUPPORTED:
|
||||
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 EGL_DMA_BUF_SUPPORTED 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.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 EGL_DMA_BUF_SUPPORTED:
|
||||
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)
|
||||
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)
|
||||
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 _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 EGL_DMA_BUF_SUPPORTED:
|
||||
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 EGL_DMA_BUF_SUPPORTED:
|
||||
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))
|
||||
111
selfdrive/ui/onroad/driver_camera_dialog.py
Normal file
111
selfdrive/ui/onroad/driver_camera_dialog.py
Normal file
@@ -0,0 +1,111 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from msgq.visionipc import VisionStreamType
|
||||
from openpilot.selfdrive.ui.onroad.cameraview import CameraView
|
||||
from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, device
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.widgets.label import gui_label
|
||||
|
||||
|
||||
class DriverCameraDialog(CameraView):
|
||||
def __init__(self):
|
||||
super().__init__("camerad", VisionStreamType.VISION_STREAM_DRIVER)
|
||||
self.driver_state_renderer = DriverStateRenderer()
|
||||
# TODO: this can grow unbounded, should be given some thought
|
||||
device.add_interactive_timeout_callback(lambda: gui_app.set_modal_overlay(None))
|
||||
ui_state.params.put_bool("IsDriverViewEnabled", True)
|
||||
|
||||
def hide_event(self):
|
||||
super().hide_event()
|
||||
ui_state.params.put_bool("IsDriverViewEnabled", False)
|
||||
self.close()
|
||||
|
||||
def _handle_mouse_release(self, _):
|
||||
super()._handle_mouse_release(_)
|
||||
gui_app.set_modal_overlay(None)
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def _render(self, rect):
|
||||
super()._render(rect)
|
||||
|
||||
if not self.frame:
|
||||
gui_label(
|
||||
rect,
|
||||
tr("camera starting"),
|
||||
font_size=100,
|
||||
font_weight=FontWeight.BOLD,
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
|
||||
)
|
||||
return -1
|
||||
|
||||
self._draw_face_detection(rect)
|
||||
self.driver_state_renderer.render(rect)
|
||||
|
||||
return -1
|
||||
|
||||
def _draw_face_detection(self, rect: rl.Rectangle) -> None:
|
||||
driver_state = ui_state.sm["driverStateV2"]
|
||||
is_rhd = driver_state.wheelOnRightProb > 0.5
|
||||
driver_data = driver_state.rightDriverData if is_rhd else driver_state.leftDriverData
|
||||
face_detect = driver_data.faceProb > 0.7
|
||||
if not face_detect:
|
||||
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
|
||||
fbox_x = int(1080.0 - 1714.0 * face_x)
|
||||
fbox_y = int(-135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y)
|
||||
box_size = 220
|
||||
|
||||
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,
|
||||
10,
|
||||
10,
|
||||
line_color,
|
||||
)
|
||||
|
||||
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
|
||||
driver_view_ratio = 2.0
|
||||
|
||||
# Get stream dimensions
|
||||
if self.frame:
|
||||
stream_width = self.frame.width
|
||||
stream_height = self.frame.height
|
||||
else:
|
||||
# Default values if frame not available
|
||||
stream_width = 1928
|
||||
stream_height = 1208
|
||||
|
||||
yscale = stream_height * driver_view_ratio / stream_width
|
||||
xscale = yscale * rect.height / rect.width * stream_width / stream_height
|
||||
|
||||
return np.array([
|
||||
[xscale, 0.0, 0.0],
|
||||
[0.0, yscale, 0.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
gui_app.init_window("Driver Camera View")
|
||||
|
||||
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()
|
||||
231
selfdrive/ui/onroad/driver_state.py
Normal file
231
selfdrive/ui/onroad/driver_state.py
Normal file
@@ -0,0 +1,231 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from cereal import log
|
||||
from dataclasses import dataclass
|
||||
from openpilot.selfdrive.ui import UI_BORDER_SIZE
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
|
||||
# Default 3D coordinates for face keypoints as a NumPy array
|
||||
DEFAULT_FACE_KPTS_3D = np.array([
|
||||
[-5.98, -51.20, 8.00], [-17.64, -49.14, 8.00], [-23.81, -46.40, 8.00], [-29.98, -40.91, 8.00],
|
||||
[-32.04, -37.49, 8.00], [-34.10, -32.00, 8.00], [-36.16, -21.03, 8.00], [-36.16, 6.40, 8.00],
|
||||
[-35.47, 10.51, 8.00], [-32.73, 19.43, 8.00], [-29.30, 26.29, 8.00], [-24.50, 33.83, 8.00],
|
||||
[-19.01, 41.37, 8.00], [-14.21, 46.17, 8.00], [-12.16, 47.54, 8.00], [-4.61, 49.60, 8.00],
|
||||
[4.99, 49.60, 8.00], [12.53, 47.54, 8.00], [14.59, 46.17, 8.00], [19.39, 41.37, 8.00],
|
||||
[24.87, 33.83, 8.00], [29.67, 26.29, 8.00], [33.10, 19.43, 8.00], [35.84, 10.51, 8.00],
|
||||
[36.53, 6.40, 8.00], [36.53, -21.03, 8.00], [34.47, -32.00, 8.00], [32.42, -37.49, 8.00],
|
||||
[30.36, -40.91, 8.00], [24.19, -46.40, 8.00], [18.02, -49.14, 8.00], [6.36, -51.20, 8.00],
|
||||
[-5.98, -51.20, 8.00],
|
||||
], dtype=np.float32)
|
||||
|
||||
# UI constants
|
||||
BTN_SIZE = 192
|
||||
IMG_SIZE = 144
|
||||
ARC_LENGTH = 133
|
||||
ARC_THICKNESS_DEFAULT = 6.7
|
||||
ARC_THICKNESS_EXTEND = 12.0
|
||||
|
||||
SCALES_POS = np.array([0.9, 0.4, 0.4], dtype=np.float32)
|
||||
SCALES_NEG = np.array([0.7, 0.4, 0.4], dtype=np.float32)
|
||||
|
||||
ARC_POINT_COUNT = 37 # Number of points in the arc
|
||||
ARC_ANGLES = np.linspace(0.0, np.pi, ARC_POINT_COUNT, dtype=np.float32)
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArcData:
|
||||
"""Data structure for arc rendering parameters."""
|
||||
x: float
|
||||
y: float
|
||||
width: float
|
||||
height: float
|
||||
thickness: float
|
||||
|
||||
|
||||
class DriverStateRenderer(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
# Initial state with NumPy arrays
|
||||
self.face_kpts_draw = DEFAULT_FACE_KPTS_3D.copy()
|
||||
self.is_active = False
|
||||
self.is_rhd = False
|
||||
self.dm_fade_state = 0.0
|
||||
self.driver_pose_vals = np.zeros(3, dtype=np.float32)
|
||||
self.driver_pose_diff = np.zeros(3, dtype=np.float32)
|
||||
self.driver_pose_sins = np.zeros(3, dtype=np.float32)
|
||||
self.driver_pose_coss = np.zeros(3, dtype=np.float32)
|
||||
self.face_keypoints_transformed = np.zeros((DEFAULT_FACE_KPTS_3D.shape[0], 2), dtype=np.float32)
|
||||
self.position_x: float = 0.0
|
||||
self.position_y: float = 0.0
|
||||
self.h_arc_data = None
|
||||
self.v_arc_data = None
|
||||
|
||||
# Pre-allocate drawing arrays
|
||||
self.face_lines = [rl.Vector2(0, 0) for _ in range(len(DEFAULT_FACE_KPTS_3D))]
|
||||
self.h_arc_lines = [rl.Vector2(0, 0) for _ in range(ARC_POINT_COUNT)]
|
||||
self.v_arc_lines = [rl.Vector2(0, 0) for _ in range(ARC_POINT_COUNT)]
|
||||
|
||||
# Load the driver face icon
|
||||
self.dm_img = gui_app.texture("icons/driver_face.png", IMG_SIZE, IMG_SIZE)
|
||||
|
||||
# Colors
|
||||
self.white_color = rl.Color(255, 255, 255, 255)
|
||||
self.arc_color = rl.Color(26, 242, 66, 255)
|
||||
self.engaged_color = rl.Color(0x0C, 0x94, 0x96, 0xFF)
|
||||
self.disengaged_color = rl.Color(139, 139, 139, 255)
|
||||
|
||||
self.set_visible(lambda: (ui_state.sm["selfdriveState"].alertSize == AlertSize.none and
|
||||
ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame))
|
||||
|
||||
def _render(self, rect):
|
||||
# Set opacity based on active state
|
||||
opacity = 0.65 if self.is_active else 0.2
|
||||
|
||||
# Draw background circle
|
||||
rl.draw_circle(int(self.position_x), int(self.position_y), BTN_SIZE // 2, rl.Color(0, 0, 0, 70))
|
||||
|
||||
# Draw face icon
|
||||
icon_pos = rl.Vector2(self.position_x - self.dm_img.width // 2, self.position_y - self.dm_img.height // 2)
|
||||
rl.draw_texture_v(self.dm_img, icon_pos, rl.Color(255, 255, 255, int(255 * opacity)))
|
||||
|
||||
# Draw face outline
|
||||
self.white_color.a = int(255 * opacity)
|
||||
rl.draw_spline_linear(self.face_lines, len(self.face_lines), 5.2, self.white_color)
|
||||
|
||||
# Set arc color based on engaged state
|
||||
self.arc_color = self.engaged_color if ui_state.engaged else self.disengaged_color
|
||||
self.arc_color.a = int(0.4 * 255 * (1.0 - self.dm_fade_state)) # Fade out when inactive
|
||||
|
||||
# Draw arcs
|
||||
if self.h_arc_data:
|
||||
rl.draw_spline_linear(self.h_arc_lines, len(self.h_arc_lines), self.h_arc_data.thickness, self.arc_color)
|
||||
if self.v_arc_data:
|
||||
rl.draw_spline_linear(self.v_arc_lines, len(self.v_arc_lines), self.v_arc_data.thickness, self.arc_color)
|
||||
|
||||
def _update_state(self):
|
||||
"""Update the driver monitoring state based on model data"""
|
||||
sm = ui_state.sm
|
||||
if not self.is_visible:
|
||||
return
|
||||
|
||||
# Get monitoring state
|
||||
dm_state = sm["driverMonitoringState"]
|
||||
self.is_active = dm_state.isActiveMode
|
||||
self.is_rhd = dm_state.isRHD
|
||||
|
||||
# Update fade state (smoother transition between active/inactive)
|
||||
fade_target = 0.0 if self.is_active else 0.5
|
||||
self.dm_fade_state = np.clip(self.dm_fade_state + 0.2 * (fade_target - self.dm_fade_state), 0.0, 1.0)
|
||||
|
||||
# Get driver orientation data from appropriate camera
|
||||
driverstate = sm["driverStateV2"]
|
||||
driver_data = driverstate.rightDriverData if self.is_rhd else driverstate.leftDriverData
|
||||
driver_orient = driver_data.faceOrientation
|
||||
|
||||
# Update pose values with scaling and smoothing
|
||||
driver_orient = np.array(driver_orient)
|
||||
scales = np.where(driver_orient < 0, SCALES_NEG, SCALES_POS)
|
||||
v_this = driver_orient * scales
|
||||
self.driver_pose_diff = np.abs(self.driver_pose_vals - v_this)
|
||||
self.driver_pose_vals = 0.8 * v_this + 0.2 * self.driver_pose_vals # Smooth changes
|
||||
|
||||
# Apply fade to rotation and compute sin/cos
|
||||
rotation_amount = self.driver_pose_vals * (1.0 - self.dm_fade_state)
|
||||
self.driver_pose_sins = np.sin(rotation_amount)
|
||||
self.driver_pose_coss = np.cos(rotation_amount)
|
||||
|
||||
# Create rotation matrix for 3D face model
|
||||
sin_y, sin_x, sin_z = self.driver_pose_sins
|
||||
cos_y, cos_x, cos_z = self.driver_pose_coss
|
||||
r_xyz = np.array(
|
||||
[
|
||||
[cos_x * cos_z, cos_x * sin_z, -sin_x],
|
||||
[-sin_y * sin_x * cos_z - cos_y * sin_z, -sin_y * sin_x * sin_z + cos_y * cos_z, -sin_y * cos_x],
|
||||
[cos_y * sin_x * cos_z - sin_y * sin_z, cos_y * sin_x * sin_z + sin_y * cos_z, cos_y * cos_x],
|
||||
]
|
||||
)
|
||||
|
||||
# Transform face keypoints using vectorized matrix multiplication
|
||||
self.face_kpts_draw = DEFAULT_FACE_KPTS_3D @ r_xyz.T
|
||||
self.face_kpts_draw[:, 2] = self.face_kpts_draw[:, 2] * (1.0 - self.dm_fade_state) + 8 * self.dm_fade_state
|
||||
|
||||
# Pre-calculate the transformed keypoints
|
||||
kp_depth = (self.face_kpts_draw[:, 2] - 8) / 120.0 + 1.0
|
||||
self.face_keypoints_transformed = self.face_kpts_draw[:, :2] * kp_depth[:, None]
|
||||
|
||||
# Pre-calculate all drawing elements
|
||||
self._pre_calculate_drawing_elements()
|
||||
|
||||
def _pre_calculate_drawing_elements(self):
|
||||
"""Pre-calculate all drawing elements based on the current rectangle"""
|
||||
# Calculate icon position (bottom-left or bottom-right)
|
||||
width, height = self._rect.width, self._rect.height
|
||||
offset = UI_BORDER_SIZE + BTN_SIZE // 2
|
||||
self.position_x = self._rect.x + (width - offset if self.is_rhd else offset)
|
||||
self.position_y = self._rect.y + height - offset
|
||||
|
||||
# Pre-calculate the face lines positions
|
||||
positioned_keypoints = self.face_keypoints_transformed + np.array([self.position_x, self.position_y])
|
||||
for i in range(len(positioned_keypoints)):
|
||||
self.face_lines[i].x = positioned_keypoints[i][0]
|
||||
self.face_lines[i].y = positioned_keypoints[i][1]
|
||||
|
||||
# Calculate arc dimensions based on head rotation
|
||||
delta_x = -self.driver_pose_sins[1] * ARC_LENGTH / 2.0 # Horizontal movement
|
||||
delta_y = -self.driver_pose_sins[0] * ARC_LENGTH / 2.0 # Vertical movement
|
||||
|
||||
# Horizontal arc
|
||||
h_width = abs(delta_x)
|
||||
self.h_arc_data = self._calculate_arc_data(
|
||||
delta_x, h_width, self.position_x, self.position_y - ARC_LENGTH / 2,
|
||||
self.driver_pose_sins[1], self.driver_pose_diff[1], is_horizontal=True
|
||||
)
|
||||
|
||||
# Vertical arc
|
||||
v_height = abs(delta_y)
|
||||
self.v_arc_data = self._calculate_arc_data(
|
||||
delta_y, v_height, self.position_x - ARC_LENGTH / 2, self.position_y,
|
||||
self.driver_pose_sins[0], self.driver_pose_diff[0], is_horizontal=False
|
||||
)
|
||||
|
||||
def _calculate_arc_data(
|
||||
self, delta: float, size: float, x: float, y: float, sin_val: float, diff_val: float, is_horizontal: bool
|
||||
):
|
||||
"""Calculate arc data and pre-compute arc points."""
|
||||
if size <= 0:
|
||||
return None
|
||||
|
||||
thickness = ARC_THICKNESS_DEFAULT + ARC_THICKNESS_EXTEND * min(1.0, diff_val * 5.0)
|
||||
start_angle = (90 if sin_val > 0 else -90) if is_horizontal else (0 if sin_val > 0 else 180)
|
||||
x = min(x + delta, x) if is_horizontal else x
|
||||
y = y if is_horizontal else min(y + delta, y)
|
||||
|
||||
arc_data = ArcData(
|
||||
x=x,
|
||||
y=y,
|
||||
width=size if is_horizontal else ARC_LENGTH,
|
||||
height=ARC_LENGTH if is_horizontal else size,
|
||||
thickness=thickness,
|
||||
)
|
||||
|
||||
# Pre-calculate arc points
|
||||
angles = ARC_ANGLES + np.deg2rad(start_angle)
|
||||
|
||||
center_x = x + arc_data.width / 2
|
||||
center_y = y + arc_data.height / 2
|
||||
radius_x = arc_data.width / 2
|
||||
radius_y = arc_data.height / 2
|
||||
|
||||
x_coords = center_x + np.cos(angles) * radius_x
|
||||
y_coords = center_y - np.sin(angles) * radius_y
|
||||
|
||||
arc_lines = self.h_arc_lines if is_horizontal else self.v_arc_lines
|
||||
for i, (x_coord, y_coord) in enumerate(zip(x_coords, y_coords, strict=True)):
|
||||
arc_lines[i].x = x_coord
|
||||
arc_lines[i].y = y_coord
|
||||
|
||||
return arc_data
|
||||
141
selfdrive/ui/onroad/environment_renderer.py
Normal file
141
selfdrive/ui/onroad/environment_renderer.py
Normal file
@@ -0,0 +1,141 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from cereal import custom
|
||||
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
MODE_OFF = 0
|
||||
MODE_OVERLAY = 1
|
||||
MODE_REPLACE = 2
|
||||
|
||||
_ENV_LABEL = custom.IQEnvironment.Object.Label
|
||||
_OBJECT_COLORS = {
|
||||
_ENV_LABEL.car: (40, 210, 200),
|
||||
_ENV_LABEL.truck: (40, 210, 200),
|
||||
_ENV_LABEL.bus: (40, 210, 200),
|
||||
_ENV_LABEL.motorcycle: (90, 220, 255),
|
||||
_ENV_LABEL.bicycle: (90, 220, 255),
|
||||
_ENV_LABEL.person: (255, 210, 90),
|
||||
_ENV_LABEL.stopSign: (255, 60, 45),
|
||||
_ENV_LABEL.trafficLight: (255, 190, 0),
|
||||
}
|
||||
|
||||
_BOX_EDGES = (
|
||||
(0, 1), (1, 3), (3, 2), (2, 0),
|
||||
(4, 5), (5, 7), (7, 6), (6, 4),
|
||||
(0, 4), (1, 5), (2, 6), (3, 7),
|
||||
)
|
||||
|
||||
GRID_HALF_WIDTH = 12.0
|
||||
GRID_MAX_DISTANCE = 90.0
|
||||
GRID_STEP = 6.0
|
||||
|
||||
|
||||
class EnvironmentRenderer(Widget):
|
||||
def __init__(self):
|
||||
Widget.__init__(self)
|
||||
self._car_space_transform = np.zeros((3, 3), dtype=np.float32)
|
||||
self._mode = MODE_OFF
|
||||
self._counter = 0
|
||||
|
||||
def set_transform(self, transform: np.ndarray):
|
||||
self._car_space_transform = transform.astype(np.float32)
|
||||
|
||||
def _project(self, pt: np.ndarray):
|
||||
p = self._car_space_transform @ pt
|
||||
if abs(p[2]) < 1e-6:
|
||||
return None
|
||||
return p[0] / p[2], p[1] / p[2]
|
||||
|
||||
def _in_rect(self, x: float, y: float) -> bool:
|
||||
r = self._rect
|
||||
return r.x - 400 <= x <= r.x + r.width + 400 and r.y - 400 <= y <= r.y + r.height + 400
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
sm = ui_state.sm
|
||||
if self._counter % 30 == 0:
|
||||
self._mode = int(ui_state.params.get("EnvironmentView", return_default=True) or 0) if ui_state.active_bundle else 0
|
||||
self._counter += 1
|
||||
|
||||
if self._mode == MODE_OFF:
|
||||
return
|
||||
if sm.recv_frame["liveCalibration"] < ui_state.started_frame:
|
||||
return
|
||||
|
||||
if self._mode == MODE_REPLACE:
|
||||
self._draw_backdrop(rect)
|
||||
self._draw_ground_grid()
|
||||
if sm.valid["modelV2"]:
|
||||
self._draw_model_scene(sm["modelV2"])
|
||||
|
||||
if sm.alive["iqEnvironment"] and sm.valid["iqEnvironment"]:
|
||||
self._draw_objects(sm["iqEnvironment"])
|
||||
|
||||
def _draw_backdrop(self, rect: rl.Rectangle):
|
||||
rl.draw_rectangle_gradient_v(int(rect.x), int(rect.y), int(rect.width), int(rect.height),
|
||||
rl.Color(14, 17, 22, 255), rl.Color(6, 8, 11, 255))
|
||||
|
||||
def _draw_ground_grid(self):
|
||||
col = rl.Color(60, 70, 82, 90)
|
||||
dist = GRID_STEP
|
||||
while dist <= GRID_MAX_DISTANCE:
|
||||
a = self._project(np.array([dist, -GRID_HALF_WIDTH, 0.0]))
|
||||
b = self._project(np.array([dist, GRID_HALF_WIDTH, 0.0]))
|
||||
if a and b and self._in_rect(*a) and self._in_rect(*b):
|
||||
rl.draw_line_ex(rl.Vector2(*a), rl.Vector2(*b), 1.5, col)
|
||||
dist += GRID_STEP
|
||||
for off in np.arange(-GRID_HALF_WIDTH, GRID_HALF_WIDTH + 0.1, 3.0):
|
||||
a = self._project(np.array([GRID_STEP, float(off), 0.0]))
|
||||
b = self._project(np.array([GRID_MAX_DISTANCE, float(off), 0.0]))
|
||||
if a and b and self._in_rect(*a) and self._in_rect(*b):
|
||||
rl.draw_line_ex(rl.Vector2(*a), rl.Vector2(*b), 1.5, col)
|
||||
|
||||
def _draw_polyline(self, xs, ys, zs, color, thick):
|
||||
pts = []
|
||||
for x, y, z in zip(xs, ys, zs, strict=False):
|
||||
if x < 0:
|
||||
continue
|
||||
s = self._project(np.array([x, y, z], dtype=np.float32))
|
||||
if s and self._in_rect(*s):
|
||||
pts.append(rl.Vector2(*s))
|
||||
for i in range(len(pts) - 1):
|
||||
rl.draw_line_ex(pts[i], pts[i + 1], thick, color)
|
||||
|
||||
def _draw_model_scene(self, model):
|
||||
for i, lane in enumerate(model.laneLines):
|
||||
a = int(np.clip(model.laneLineProbs[i], 0.0, 0.9) * 255)
|
||||
self._draw_polyline(lane.x, lane.y, lane.z, rl.Color(235, 235, 235, a), 3.0)
|
||||
for edge in model.roadEdges:
|
||||
self._draw_polyline(edge.x, edge.y, edge.z, rl.Color(230, 70, 70, 180), 3.0)
|
||||
pos = model.position
|
||||
self._draw_polyline(pos.x, pos.y, pos.z, rl.Color(40, 210, 200, 220), 6.0)
|
||||
|
||||
def _draw_objects(self, env):
|
||||
for obj in env.objects:
|
||||
self._draw_box(obj)
|
||||
|
||||
def _draw_box(self, obj):
|
||||
hx, hy = obj.length / 2.0, obj.width / 2.0
|
||||
base = np.array([
|
||||
[obj.x - hx, obj.y - hy, obj.z], [obj.x - hx, obj.y + hy, obj.z],
|
||||
[obj.x + hx, obj.y - hy, obj.z], [obj.x + hx, obj.y + hy, obj.z],
|
||||
[obj.x - hx, obj.y - hy, obj.z + obj.height], [obj.x - hx, obj.y + hy, obj.z + obj.height],
|
||||
[obj.x + hx, obj.y - hy, obj.z + obj.height], [obj.x + hx, obj.y + hy, obj.z + obj.height],
|
||||
], dtype=np.float32)
|
||||
|
||||
screen = []
|
||||
for corner in base:
|
||||
s = self._project(corner)
|
||||
if s is None or not self._in_rect(*s):
|
||||
return
|
||||
screen.append(s)
|
||||
|
||||
r, g, b = _OBJECT_COLORS.get(obj.label, (40, 210, 200))
|
||||
a = int(np.clip(obj.prob, 0.3, 1.0) * 210)
|
||||
floor = [rl.Vector2(*screen[i]) for i in (0, 1, 3, 2)]
|
||||
rl.draw_triangle(floor[0], floor[1], floor[2], rl.Color(r, g, b, a // 5))
|
||||
rl.draw_triangle(floor[0], floor[2], floor[3], rl.Color(r, g, b, a // 5))
|
||||
for i, j in _BOX_EDGES:
|
||||
rl.draw_line_ex(rl.Vector2(*screen[i]), rl.Vector2(*screen[j]), 2.0, rl.Color(r, g, b, a))
|
||||
87
selfdrive/ui/onroad/exp_button.py
Normal file
87
selfdrive/ui/onroad/exp_button.py
Normal file
@@ -0,0 +1,87 @@
|
||||
import time
|
||||
import pyray as rl
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
|
||||
class ExpButton(Widget):
|
||||
def __init__(self, button_size: int, icon_size: int):
|
||||
super().__init__()
|
||||
self._params = Params()
|
||||
self._experimental_mode: bool = False
|
||||
self._iq_dynamic_mode: bool = False
|
||||
self._engageable: bool = False
|
||||
|
||||
# State hold mechanism
|
||||
self._hold_duration = 2.0 # seconds
|
||||
self._held_mode: tuple | None = None # (experimental, iq_dynamic) or None
|
||||
self._hold_end_time: float | None = None
|
||||
|
||||
self._white_color: rl.Color = rl.Color(255, 255, 255, 255)
|
||||
self._black_bg: rl.Color = rl.Color(0, 0, 0, 166)
|
||||
self._txt_wheel: rl.Texture = gui_app.texture('icons/chffr_wheel.png', icon_size, icon_size)
|
||||
self._txt_standard: rl.Texture = gui_app.texture('icons_mici/iqstandard_mode_tizi.png', icon_size, icon_size)
|
||||
self._txt_pilot: rl.Texture = gui_app.texture('icons_mici/experimental_mode_tizi.png', icon_size, icon_size)
|
||||
self._txt_dyn: rl.Texture = gui_app.texture('icons_mici/iqdynamic_mode_tizi.png', icon_size, icon_size)
|
||||
self._rect = rl.Rectangle(0, 0, button_size, button_size)
|
||||
|
||||
def set_rect(self, rect: rl.Rectangle) -> None:
|
||||
self._rect.x, self._rect.y = rect.x, rect.y
|
||||
|
||||
def _update_state(self) -> None:
|
||||
selfdrive_state = ui_state.sm["selfdriveState"]
|
||||
self._experimental_mode = selfdrive_state.experimentalMode
|
||||
self._iq_dynamic_mode = self._params.get_bool("IQDynamicMode")
|
||||
self._engageable = selfdrive_state.engageable or selfdrive_state.enabled
|
||||
|
||||
def _handle_mouse_release(self, _):
|
||||
super()._handle_mouse_release(_)
|
||||
if not self._is_toggle_allowed():
|
||||
return
|
||||
|
||||
exp, dyn = self._current_mode()
|
||||
# Cycle: IQ.Standard → IQ.Dynamic → IQ.Pilot → IQ.Standard
|
||||
if not exp:
|
||||
new_exp, new_dyn = True, True
|
||||
elif dyn:
|
||||
new_exp, new_dyn = True, False
|
||||
else:
|
||||
new_exp, new_dyn = False, False
|
||||
|
||||
self._params.put_bool("ExperimentalMode", new_exp)
|
||||
self._params.put_bool("IQDynamicMode", new_dyn)
|
||||
self._held_mode = (new_exp, new_dyn)
|
||||
self._hold_end_time = time.monotonic() + self._hold_duration
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
center_x = int(self._rect.x + self._rect.width // 2)
|
||||
center_y = int(self._rect.y + self._rect.height // 2)
|
||||
|
||||
self._white_color.a = 180 if self.is_pressed or not self._engageable else 255
|
||||
|
||||
exp, dyn = self._current_mode()
|
||||
if not ui_state.has_longitudinal_control:
|
||||
texture = self._txt_wheel
|
||||
elif exp and dyn:
|
||||
texture = self._txt_dyn
|
||||
elif exp:
|
||||
texture = self._txt_pilot
|
||||
else:
|
||||
texture = self._txt_standard
|
||||
rl.draw_circle(center_x, center_y, self._rect.width / 2, self._black_bg)
|
||||
rl.draw_texture(texture, center_x - texture.width // 2, center_y - texture.height // 2, self._white_color)
|
||||
|
||||
def _current_mode(self) -> tuple:
|
||||
now = time.monotonic()
|
||||
if self._hold_end_time and now < self._hold_end_time:
|
||||
return self._held_mode
|
||||
if self._hold_end_time and now >= self._hold_end_time:
|
||||
self._hold_end_time = self._held_mode = None
|
||||
return (self._experimental_mode, self._iq_dynamic_mode)
|
||||
|
||||
def _is_toggle_allowed(self):
|
||||
if not self._params.get_bool("ExperimentalModeConfirmed"):
|
||||
return False
|
||||
return ui_state.has_longitudinal_control
|
||||
251
selfdrive/ui/onroad/hud_renderer.py
Normal file
251
selfdrive/ui/onroad/hud_renderer.py
Normal file
@@ -0,0 +1,251 @@
|
||||
import pyray as rl
|
||||
from dataclasses import dataclass
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.onroad.exp_button import ExpButton
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
# Constants
|
||||
SET_SPEED_NA = 255
|
||||
KM_TO_MILE = 0.621371
|
||||
CRUISE_DISABLED_CHAR = '–'
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UIConfig:
|
||||
header_height: int = 300
|
||||
border_size: int = 30
|
||||
button_size: int = 192
|
||||
set_speed_width_metric: int = 186
|
||||
set_speed_width_imperial: int = 174
|
||||
set_speed_height: int = 228
|
||||
wheel_icon_size: int = 144
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FontSizes:
|
||||
current_speed: int = 176
|
||||
speed_unit: int = 66
|
||||
max_speed: int = 28
|
||||
set_speed: int = 74
|
||||
limit_speed: int = 64
|
||||
limit_offset: int = 30
|
||||
limit_unit: int = 22
|
||||
limit_label: int = 24
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Colors:
|
||||
WHITE = rl.WHITE
|
||||
DISENGAGED = rl.Color(145, 155, 149, 255)
|
||||
OVERRIDE = rl.Color(145, 155, 149, 255) # Added
|
||||
ENGAGED = rl.Color(0x0C, 0x94, 0x96, 0xFF)
|
||||
LIMIT_ENGAGED = rl.Color(0x27, 0xF5, 0xD3, 0xFF)
|
||||
DISENGAGED_BG = rl.Color(0, 0, 0, 153)
|
||||
OVERRIDE_BG = rl.Color(145, 155, 149, 204)
|
||||
ENGAGED_BG = rl.Color(128, 216, 166, 204)
|
||||
GREY = rl.Color(166, 166, 166, 255)
|
||||
DARK_GREY = rl.Color(114, 114, 114, 255)
|
||||
BLACK_TRANSLUCENT = rl.Color(0, 0, 0, 166)
|
||||
WHITE_TRANSLUCENT = rl.Color(255, 255, 255, 200)
|
||||
BORDER_TRANSLUCENT = rl.Color(255, 255, 255, 75)
|
||||
HEADER_GRADIENT_START = rl.Color(0, 0, 0, 114)
|
||||
HEADER_GRADIENT_END = rl.BLANK
|
||||
|
||||
|
||||
UI_CONFIG = UIConfig()
|
||||
FONT_SIZES = FontSizes()
|
||||
COLORS = Colors()
|
||||
|
||||
|
||||
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.speed: float = 0.0
|
||||
self.v_ego_cluster_seen: bool = False
|
||||
self.limit_speed_text: str = "---"
|
||||
self.limit_offset_text: str = ""
|
||||
self.limit_available: bool = False
|
||||
|
||||
self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
|
||||
self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
|
||||
|
||||
self._exp_button: ExpButton = ExpButton(UI_CONFIG.button_size, UI_CONFIG.wheel_icon_size)
|
||||
|
||||
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
|
||||
self.set_speed = (
|
||||
controls_state.vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster
|
||||
)
|
||||
self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA
|
||||
self.is_cruise_available = self.set_speed != -1
|
||||
|
||||
if self.is_cruise_set and not ui_state.is_metric:
|
||||
self.set_speed *= KM_TO_MILE
|
||||
|
||||
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."""
|
||||
# Draw the header background
|
||||
rl.draw_rectangle_gradient_v(
|
||||
int(rect.x),
|
||||
int(rect.y),
|
||||
int(rect.width),
|
||||
UI_CONFIG.header_height,
|
||||
COLORS.HEADER_GRADIENT_START,
|
||||
COLORS.HEADER_GRADIENT_END,
|
||||
)
|
||||
|
||||
if self.is_cruise_available:
|
||||
self._draw_set_speed(rect)
|
||||
|
||||
self._draw_current_speed(rect)
|
||||
|
||||
button_x = rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size
|
||||
button_y = rect.y + UI_CONFIG.border_size
|
||||
self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size))
|
||||
|
||||
def user_interacting(self) -> bool:
|
||||
return self._exp_button.is_pressed
|
||||
|
||||
def _draw_set_speed(self, rect: rl.Rectangle) -> None:
|
||||
"""Draw the compact stacked LIMIT / MAX speed indicator box."""
|
||||
set_speed_width = UI_CONFIG.set_speed_width_metric if ui_state.is_metric else UI_CONFIG.set_speed_width_imperial
|
||||
x = rect.x + 60 + (UI_CONFIG.set_speed_width_imperial - set_speed_width) // 2
|
||||
y = rect.y + 45
|
||||
|
||||
set_speed_rect = rl.Rectangle(x, y, set_speed_width, UI_CONFIG.set_speed_height)
|
||||
rl.draw_rectangle_rounded(set_speed_rect, 0.35, 10, COLORS.BLACK_TRANSLUCENT)
|
||||
rl.draw_rectangle_rounded_lines_ex(set_speed_rect, 0.35, 10, 6, COLORS.BORDER_TRANSLUCENT)
|
||||
split_y = y + 112
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(x + 18, split_y),
|
||||
rl.Vector2(x + set_speed_width - 18, split_y),
|
||||
3,
|
||||
COLORS.BORDER_TRANSLUCENT,
|
||||
)
|
||||
|
||||
max_color = COLORS.DARK_GREY
|
||||
set_speed_color = COLORS.WHITE
|
||||
limit_value_color = COLORS.WHITE if self.limit_available else COLORS.DARK_GREY
|
||||
if self.is_cruise_set:
|
||||
if ui_state.status == UIStatus.ENGAGED:
|
||||
max_color = COLORS.ENGAGED
|
||||
if self.limit_available:
|
||||
limit_value_color = COLORS.LIMIT_ENGAGED
|
||||
elif ui_state.status == UIStatus.DISENGAGED:
|
||||
max_color = COLORS.DISENGAGED
|
||||
elif ui_state.status == UIStatus.OVERRIDE:
|
||||
max_color = COLORS.OVERRIDE
|
||||
|
||||
limit_value_text = self.limit_speed_text
|
||||
if len(limit_value_text) <= 2:
|
||||
limit_value_size = FONT_SIZES.limit_speed
|
||||
elif len(limit_value_text) == 3:
|
||||
limit_value_size = 56
|
||||
else:
|
||||
limit_value_size = 48
|
||||
limit_value_width = measure_text_cached(self._font_bold, limit_value_text, limit_value_size).x
|
||||
limit_offset_text = self.limit_offset_text if self.limit_available else ""
|
||||
limit_offset_width = 0.0
|
||||
if limit_offset_text:
|
||||
limit_offset_width = measure_text_cached(self._font_semi_bold, limit_offset_text, FONT_SIZES.limit_offset).x + 8
|
||||
limit_value_x = x + (set_speed_width - limit_value_width - limit_offset_width) / 2
|
||||
rl.draw_text_ex(
|
||||
self._font_bold,
|
||||
limit_value_text,
|
||||
rl.Vector2(limit_value_x, y + 14),
|
||||
limit_value_size,
|
||||
0,
|
||||
limit_value_color,
|
||||
)
|
||||
if limit_offset_text:
|
||||
rl.draw_text_ex(
|
||||
self._font_semi_bold,
|
||||
limit_offset_text,
|
||||
rl.Vector2(limit_value_x + limit_value_width + 8, y + 22),
|
||||
FONT_SIZES.limit_offset,
|
||||
0,
|
||||
COLORS.WHITE_TRANSLUCENT,
|
||||
)
|
||||
|
||||
if self.limit_available:
|
||||
limit_unit_text = tr("LIMIT")
|
||||
limit_unit_width = measure_text_cached(self._font_medium, limit_unit_text, FONT_SIZES.limit_unit).x
|
||||
rl.draw_text_ex(
|
||||
self._font_medium,
|
||||
limit_unit_text,
|
||||
rl.Vector2(x + (set_speed_width - limit_unit_width) / 2, y + 78),
|
||||
FONT_SIZES.limit_unit,
|
||||
0,
|
||||
COLORS.WHITE_TRANSLUCENT,
|
||||
)
|
||||
else:
|
||||
limit_label_text = tr("LIMIT")
|
||||
limit_label_width = measure_text_cached(self._font_semi_bold, limit_label_text, FONT_SIZES.limit_label).x
|
||||
rl.draw_text_ex(
|
||||
self._font_semi_bold,
|
||||
limit_label_text,
|
||||
rl.Vector2(x + (set_speed_width - limit_label_width) / 2, y + 74),
|
||||
FONT_SIZES.limit_label,
|
||||
0,
|
||||
COLORS.GREY,
|
||||
)
|
||||
|
||||
max_text = tr("MAX")
|
||||
max_text_width = measure_text_cached(self._font_semi_bold, max_text, FONT_SIZES.max_speed).x
|
||||
rl.draw_text_ex(
|
||||
self._font_semi_bold,
|
||||
max_text,
|
||||
rl.Vector2(x + (set_speed_width - max_text_width) / 2, y + 118),
|
||||
FONT_SIZES.max_speed,
|
||||
0,
|
||||
max_color,
|
||||
)
|
||||
|
||||
set_speed_text = CRUISE_DISABLED_CHAR if not self.is_cruise_set else str(round(self.set_speed))
|
||||
speed_text_width = measure_text_cached(self._font_bold, set_speed_text, FONT_SIZES.set_speed).x
|
||||
rl.draw_text_ex(
|
||||
self._font_bold,
|
||||
set_speed_text,
|
||||
rl.Vector2(x + (set_speed_width - speed_text_width) / 2, y + 136),
|
||||
FONT_SIZES.set_speed,
|
||||
0,
|
||||
set_speed_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)
|
||||
612
selfdrive/ui/onroad/model_renderer.py
Normal file
612
selfdrive/ui/onroad/model_renderer.py
Normal file
@@ -0,0 +1,612 @@
|
||||
import colorsys
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from cereal import messaging, car, custom
|
||||
from dataclasses import dataclass, field
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
|
||||
from openpilot.selfdrive.locationd.calibration_helpers import get_render_path_height
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
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.onroad.hud_overlays import ChevronMetrics
|
||||
from openpilot.iqpilot.ui.onroad.lead_confidence import driving_confidence
|
||||
|
||||
CLIP_MARGIN = 500
|
||||
MIN_DRAW_DISTANCE = 10.0
|
||||
MAX_DRAW_DISTANCE = 100.0
|
||||
|
||||
_VT_LABEL = custom.IQVehicleTracks.Track.Label
|
||||
VEHICLE_TRACK_LABELS = (_VT_LABEL.car, _VT_LABEL.motorcycle, _VT_LABEL.bus, _VT_LABEL.truck)
|
||||
SIGN_TRACK_COLORS = {
|
||||
_VT_LABEL.stopSign: rl.Color(255, 60, 45, 200),
|
||||
_VT_LABEL.trafficLight: rl.Color(255, 190, 0, 200),
|
||||
}
|
||||
|
||||
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)
|
||||
]
|
||||
|
||||
@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
|
||||
|
||||
|
||||
@dataclass
|
||||
class VisionDot:
|
||||
x: float
|
||||
y: float
|
||||
tx: float
|
||||
ty: float
|
||||
radius: float
|
||||
tradius: float
|
||||
alpha: float = 0.0
|
||||
talpha: float = 1.0
|
||||
rgb: tuple[int, int, int] | None = None
|
||||
|
||||
|
||||
_VD_EASE = 0.4
|
||||
|
||||
|
||||
class ModelRenderer(Widget):
|
||||
def __init__(self):
|
||||
Widget.__init__(self)
|
||||
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._track_dots: list[LeadVehicle] = []
|
||||
self._vision_dots: list[VisionDot] = []
|
||||
self._vt_frame = -1
|
||||
self._frame_transform: np.ndarray | None = None
|
||||
self._frame_transform_wide = False
|
||||
self._path_offset_z = HEIGHT_INIT[0]
|
||||
self._counter = -1
|
||||
self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
|
||||
self._ambient_dots = bool(ui_state.params.get("AmbientTrackDots", return_default=True))
|
||||
# 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)
|
||||
|
||||
# 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._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 set_frame_transform(self, transform: np.ndarray, is_wide: bool):
|
||||
self._frame_transform = transform.astype(np.float32)
|
||||
self._frame_transform_wide = is_wide
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
sm = ui_state.sm
|
||||
driving_confidence.update()
|
||||
|
||||
# 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 self._counter % 60 == 0:
|
||||
self._camera_offset = ui_state.params.get("CameraOffset", return_default=True) if ui_state.active_bundle else 0.0
|
||||
self._ambient_dots = bool(ui_state.params.get("AmbientTrackDots", return_default=True))
|
||||
self._counter += 1
|
||||
|
||||
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)
|
||||
if self._ambient_dots:
|
||||
self._update_track_dots(sm, radar_state, path_x_array)
|
||||
else:
|
||||
self._track_dots = []
|
||||
self._transform_dirty = False
|
||||
|
||||
# Draw elements
|
||||
self._draw_lane_lines()
|
||||
self._draw_path(sm)
|
||||
|
||||
if self._ambient_dots:
|
||||
self._update_vision_dots(sm)
|
||||
self._draw_vision_dots()
|
||||
|
||||
if render_lead_indicator and radar_state:
|
||||
if self._ambient_dots:
|
||||
self._draw_track_dots()
|
||||
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_track_dots(self, sm, radar_state, path_x_array):
|
||||
self._track_dots = []
|
||||
if not sm.valid['liveTracks']:
|
||||
return
|
||||
|
||||
lead_track_ids = {lead.radarTrackId for lead in (radar_state.leadOne, radar_state.leadTwo)
|
||||
if lead.status and lead.radarTrackId >= 0}
|
||||
|
||||
for pt in sm['liveTracks'].points:
|
||||
if pt.trackId in lead_track_ids or pt.dRel < 1.0 or abs(pt.yRel) > 10.0:
|
||||
continue
|
||||
|
||||
idx = self._get_path_length_idx(path_x_array, pt.dRel)
|
||||
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
|
||||
point = self._map_to_screen(pt.dRel, -pt.yRel + self._camera_offset, z + self._path_offset_z)
|
||||
if point is None:
|
||||
continue
|
||||
|
||||
sz = np.clip((25 * 30) / (pt.dRel / 3 + 30), 15.0, 30.0) * 1.1
|
||||
radius = sz * 1.1
|
||||
x, y = point
|
||||
if not (self._rect.x <= x <= self._rect.x + self._rect.width and
|
||||
self._rect.y <= y <= self._rect.y + self._rect.height):
|
||||
continue
|
||||
|
||||
self._track_dots.append(LeadVehicle(center=(float(x), float(y)), radius=float(radius), sz=float(sz)))
|
||||
|
||||
def _update_vision_dots(self, sm):
|
||||
# iqVehicleTracks arrives at a few Hz; the dots are eased toward the latest
|
||||
# detection every render frame so they glide instead of teleporting.
|
||||
hidden = (self._frame_transform is None or
|
||||
not sm.alive['iqVehicleTracks'] or not sm.valid['iqVehicleTracks'])
|
||||
if not hidden:
|
||||
vt = sm['iqVehicleTracks']
|
||||
hidden = bool(vt.wide) != self._frame_transform_wide or vt.frameWidth == 0 or vt.frameHeight == 0
|
||||
|
||||
if hidden:
|
||||
for d in self._vision_dots:
|
||||
d.talpha = 0.0
|
||||
elif vt.frameId != self._vt_frame:
|
||||
self._vt_frame = vt.frameId
|
||||
self._retarget_vision_dots(vt)
|
||||
|
||||
for d in self._vision_dots:
|
||||
d.x += (d.tx - d.x) * _VD_EASE
|
||||
d.y += (d.ty - d.y) * _VD_EASE
|
||||
d.radius += (d.tradius - d.radius) * _VD_EASE
|
||||
d.alpha += (d.talpha - d.alpha) * _VD_EASE
|
||||
self._vision_dots = [d for d in self._vision_dots if d.alpha > 0.02 or d.talpha > 0.0]
|
||||
|
||||
def _retarget_vision_dots(self, vt):
|
||||
fw, fh = vt.frameWidth, vt.frameHeight
|
||||
m = self._frame_transform
|
||||
occupied = [d.center for d in self._lead_vehicles + self._track_dots if d.center is not None]
|
||||
|
||||
targets = []
|
||||
for t in vt.tracks:
|
||||
is_vehicle = t.label in VEHICLE_TRACK_LABELS
|
||||
sign_color = SIGN_TRACK_COLORS.get(t.label)
|
||||
if not is_vehicle and sign_color is None:
|
||||
continue
|
||||
cx_f = (t.x1 + t.x2) / 2.0 * fw
|
||||
cy_f = (t.y1 + t.y2) / 2.0 * fh
|
||||
x = m[0, 0] * cx_f + m[0, 2]
|
||||
y = m[1, 1] * cy_f + m[1, 2]
|
||||
if not (self._rect.x <= x <= self._rect.x + self._rect.width and
|
||||
self._rect.y <= y <= self._rect.y + self._rect.height):
|
||||
continue
|
||||
box_h = (t.y2 - t.y1) * fh * m[1, 1]
|
||||
radius = float(np.clip(box_h * 0.35, 14.0, 40.0))
|
||||
if sign_color is not None:
|
||||
targets.append((x, y, min(radius, 22.0), (sign_color.r, sign_color.g, sign_color.b)))
|
||||
elif not any((x - ox) ** 2 + (y - oy) ** 2 < (radius * 2.2) ** 2 for ox, oy in occupied):
|
||||
targets.append((x, y, radius, None))
|
||||
|
||||
dots = self._vision_dots
|
||||
used = [False] * len(dots)
|
||||
for tx, ty, tr, rgb in targets:
|
||||
best, best_d2 = -1, 1e18
|
||||
for i, d in enumerate(dots):
|
||||
if used[i] or (d.rgb is None) != (rgb is None):
|
||||
continue
|
||||
d2 = (d.x - tx) ** 2 + (d.y - ty) ** 2
|
||||
if d2 < best_d2:
|
||||
best, best_d2 = i, d2
|
||||
if best >= 0 and best_d2 <= (max(tr, dots[best].radius) * 3.0) ** 2:
|
||||
d = dots[best]
|
||||
used[best] = True
|
||||
d.tx, d.ty, d.tradius, d.talpha, d.rgb = tx, ty, tr, 1.0, rgb
|
||||
else:
|
||||
dots.append(VisionDot(x=tx, y=ty, tx=tx, ty=ty, radius=tr, tradius=tr, alpha=0.0, talpha=1.0, rgb=rgb))
|
||||
used.append(True)
|
||||
|
||||
for i, d in enumerate(dots):
|
||||
if not used[i]:
|
||||
d.talpha = 0.0
|
||||
|
||||
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
|
||||
for i, lane_line in enumerate(self._lane_lines):
|
||||
lane_line.projected_points = self._map_line_to_polygon(
|
||||
lane_line.raw_points, 0.025 * self._lane_line_probs[i], 0.0, max_idx, max_distance
|
||||
)
|
||||
|
||||
# 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, 0.025, 0.0, max_idx, max_distance)
|
||||
|
||||
# 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)
|
||||
|
||||
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, 0.9, self._path_offset_z, max_idx, max_distance, allow_invert=False
|
||||
)
|
||||
|
||||
self._update_experimental_gradient()
|
||||
|
||||
def _update_experimental_gradient(self):
|
||||
"""Pre-calculate experimental mode gradient colors"""
|
||||
if not self._experimental_mode:
|
||||
return
|
||||
|
||||
max_len = min(len(self._path.projected_points) // 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 = self._path.projected_points[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 = Gradient(
|
||||
start=(0.0, 1.0), # Bottom of path
|
||||
end=(0.0, 0.0), # Top of path
|
||||
colors=segment_colors,
|
||||
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) * 2.35
|
||||
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 _draw_lane_lines(self):
|
||||
"""Draw lane lines and road edges"""
|
||||
for i, lane_line in enumerate(self._lane_lines):
|
||||
if lane_line.projected_points.size == 0:
|
||||
continue
|
||||
|
||||
alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7)
|
||||
color = rl.Color(255, 255, 255, int(alpha * 255))
|
||||
draw_polygon(self._rect, lane_line.projected_points, color)
|
||||
|
||||
for i, road_edge in enumerate(self._road_edges):
|
||||
if road_edge.projected_points.size == 0:
|
||||
continue
|
||||
|
||||
alpha = np.clip(1.0 - self._road_edge_stds[i], 0.0, 1.0)
|
||||
color = rl.Color(255, 0, 0, int(alpha * 255))
|
||||
draw_polygon(self._rect, road_edge.projected_points, color)
|
||||
|
||||
def _draw_path(self, sm):
|
||||
"""Draw path with dynamic coloring based on mode and throttle state."""
|
||||
if not self._path.projected_points.size:
|
||||
return
|
||||
|
||||
allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
|
||||
self._blend_filter.update(int(allow_throttle))
|
||||
|
||||
|
||||
if self._experimental_mode:
|
||||
# Draw with acceleration coloring
|
||||
if len(self._exp_gradient.colors) > 1:
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient)
|
||||
else:
|
||||
draw_polygon(self._rect, self._path.projected_points, 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],
|
||||
)
|
||||
draw_polygon(self._rect, self._path.projected_points, gradient=gradient)
|
||||
|
||||
# concentric layers (outer faint -> inner bright) build a soft center-out glow using only
|
||||
# draw_circle, which is signature-stable across raylib versions (draw_circle_gradient is not)
|
||||
_VD_GLOW = ((1.0, 26), (0.72, 38), (0.48, 54), (0.26, 78))
|
||||
|
||||
def _draw_vision_dots(self):
|
||||
for dot in self._vision_dots:
|
||||
a = dot.alpha
|
||||
if a <= 0.02:
|
||||
continue
|
||||
x, y = int(dot.x), int(dot.y)
|
||||
cr, cg, cb = (40, 210, 200) if dot.rgb is None else dot.rgb
|
||||
for frac, base in self._VD_GLOW:
|
||||
rl.draw_circle(x, y, dot.radius * frac, rl.Color(cr, cg, cb, int(base * a)))
|
||||
|
||||
def _draw_track_dots(self):
|
||||
src = rl.Rectangle(0, 0, self._lead_orb.width, self._lead_orb.height)
|
||||
for dot in self._track_dots:
|
||||
cx, cy = dot.center
|
||||
r = dot.radius
|
||||
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(255, 255, 255, 90))
|
||||
|
||||
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_distance: float) -> int:
|
||||
"""Get the index corresponding to the given path distance"""
|
||||
if len(pos_x_array) == 0:
|
||||
return 0
|
||||
indices = np.where(pos_x_array <= path_distance)[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, max_distance: float, 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]
|
||||
|
||||
# Interpolate around max_idx so path end is smooth (max_distance is always >= p0.x)
|
||||
if 0 < max_idx < line.shape[0] - 1:
|
||||
p0 = line[max_idx]
|
||||
p1 = line[max_idx + 1]
|
||||
x0, x1 = p0[0], p1[0]
|
||||
interp_y = np.interp(max_distance, [x0, x1], [p0[1], p1[1]])
|
||||
interp_z = np.interp(max_distance, [x0, x1], [p0[2], p1[2]])
|
||||
interp_point = np.array([max_distance, interp_y, interp_z], dtype=points.dtype)
|
||||
points = np.concatenate((points, interp_point[None, :]), axis=0)
|
||||
|
||||
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)]
|
||||
Reference in New Issue
Block a user