mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-29 15:03:43 +08:00
Merge branch 'master-new' into feature/external-storage
This commit is contained in:
royjr
@@ -44,6 +44,8 @@ def manager_init() -> None:
|
||||
sunnypilot_default_params: list[tuple[str, str | bytes]] = [
|
||||
("AutoLaneChangeTimer", "0"),
|
||||
("AutoLaneChangeBsmDelay", "0"),
|
||||
("BlinkerMinLateralControlSpeed", "20"), # MPH or km/h
|
||||
("BlinkerPauseLateralControl", "0"),
|
||||
("DynamicExperimentalControl", "0"),
|
||||
("HyundaiLongitudinalTuning", "0"),
|
||||
("Mads", "1"),
|
||||
|
||||
@@ -96,7 +96,6 @@ class ManagerProcess(ABC):
|
||||
try:
|
||||
fn = WATCHDOG_FN + str(self.proc.pid)
|
||||
with open(fn, "rb") as f:
|
||||
# TODO: why can't pylint find struct.unpack?
|
||||
self.last_watchdog_time = struct.unpack('Q', f.read())[0]
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
+1
-1
@@ -3,7 +3,7 @@
|
||||
The user interfaces here are built with [raylib](https://www.raylib.com/).
|
||||
|
||||
Quick start:
|
||||
* set `DEBUG_FPS=1` to show the FPS
|
||||
* set `SHOW_FPS=1` to show the FPS
|
||||
* set `STRICT_MODE=1` to kill the app if it drops too much below 60fps
|
||||
* set `SCALE=1.5` to scale the entire UI by 1.5x
|
||||
* https://www.raylib.com/cheatsheet/cheatsheet.html
|
||||
|
||||
@@ -13,7 +13,7 @@ FPS_DROP_THRESHOLD = 0.9 # FPS drop threshold for triggering a warning
|
||||
FPS_CRITICAL_THRESHOLD = 0.5 # Critical threshold for triggering strict actions
|
||||
|
||||
ENABLE_VSYNC = os.getenv("ENABLE_VSYNC") == "1"
|
||||
DEBUG_FPS = os.getenv("DEBUG_FPS") == '1'
|
||||
SHOW_FPS = os.getenv("SHOW_FPS") == '1'
|
||||
STRICT_MODE = os.getenv("STRICT_MODE") == '1'
|
||||
SCALE = float(os.getenv("SCALE", "1.0"))
|
||||
|
||||
@@ -158,7 +158,7 @@ class GuiApplication:
|
||||
dst_rect = rl.Rectangle(0, 0, float(self._scaled_width), float(self._scaled_height))
|
||||
rl.draw_texture_pro(self._render_texture.texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
|
||||
|
||||
if DEBUG_FPS:
|
||||
if SHOW_FPS:
|
||||
rl.draw_fps(10, 10)
|
||||
|
||||
rl.end_drawing()
|
||||
@@ -192,10 +192,12 @@ class GuiApplication:
|
||||
|
||||
# Create a character set from our keyboard layouts
|
||||
from openpilot.system.ui.widgets.keyboard import KEYBOARD_LAYOUTS
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import CRUISE_DISABLED_CHAR
|
||||
all_chars = set()
|
||||
for layout in KEYBOARD_LAYOUTS.values():
|
||||
all_chars.update(key for row in layout for key in row)
|
||||
all_chars = "".join(all_chars)
|
||||
all_chars += CRUISE_DISABLED_CHAR
|
||||
|
||||
codepoint_count = rl.ffi.new("int *", 1)
|
||||
codepoints = rl.load_codepoints(all_chars, codepoint_count)
|
||||
|
||||
@@ -56,7 +56,8 @@ def gui_text_box(
|
||||
font_size: int = DEFAULT_TEXT_SIZE,
|
||||
color: rl.Color = DEFAULT_TEXT_COLOR,
|
||||
alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT,
|
||||
alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP
|
||||
alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
|
||||
font_weight: FontWeight = FontWeight.NORMAL,
|
||||
):
|
||||
styles = [
|
||||
(rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_COLOR_NORMAL, rl.color_to_int(color)),
|
||||
@@ -66,6 +67,12 @@ def gui_text_box(
|
||||
(rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_ALIGNMENT_VERTICAL, alignment_vertical),
|
||||
(rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_WRAP_MODE, rl.GuiTextWrapMode.TEXT_WRAP_WORD)
|
||||
]
|
||||
if font_weight != FontWeight.NORMAL:
|
||||
rl.gui_set_font(gui_app.font(font_weight))
|
||||
|
||||
with GuiStyleContext(styles):
|
||||
rl.gui_label(rect, text)
|
||||
|
||||
if font_weight != FontWeight.NORMAL:
|
||||
rl.gui_set_font(gui_app.font(FontWeight.NORMAL))
|
||||
|
||||
|
||||
@@ -16,13 +16,12 @@ uniform int pointCount;
|
||||
uniform vec4 fillColor;
|
||||
uniform vec2 resolution;
|
||||
|
||||
uniform bool useGradient;
|
||||
uniform int useGradient;
|
||||
uniform vec2 gradientStart;
|
||||
uniform vec2 gradientEnd;
|
||||
uniform vec4 gradientColors[15];
|
||||
uniform float gradientStops[15];
|
||||
uniform int gradientColorCount;
|
||||
uniform vec2 visibleGradientRange;
|
||||
|
||||
vec4 getGradientColor(vec2 pos) {
|
||||
vec2 gradientDir = gradientEnd - gradientStart;
|
||||
@@ -30,22 +29,9 @@ vec4 getGradientColor(vec2 pos) {
|
||||
if (gradientLength < 0.001) return gradientColors[0];
|
||||
|
||||
vec2 normalizedDir = gradientDir / gradientLength;
|
||||
vec2 pointVec = pos - gradientStart;
|
||||
float projection = dot(pointVec, normalizedDir);
|
||||
float t = clamp(dot(pos - gradientStart, normalizedDir) / gradientLength, 0.0, 1.0);
|
||||
|
||||
float t = projection / gradientLength;
|
||||
|
||||
// Gradient clipping: remap t to visible range
|
||||
float visibleStart = visibleGradientRange.x;
|
||||
float visibleEnd = visibleGradientRange.y;
|
||||
float visibleRange = visibleEnd - visibleStart;
|
||||
|
||||
// Remap t to visible range
|
||||
if (visibleRange > 0.001) {
|
||||
t = visibleStart + t * visibleRange;
|
||||
}
|
||||
|
||||
t = clamp(t, 0.0, 1.0);
|
||||
if (gradientColorCount <= 1) return gradientColors[0];
|
||||
for (int i = 0; i < gradientColorCount - 1; i++) {
|
||||
if (t >= gradientStops[i] && t <= gradientStops[i+1]) {
|
||||
float segmentT = (t - gradientStops[i]) / (gradientStops[i+1] - gradientStops[i]);
|
||||
@@ -58,16 +44,11 @@ vec4 getGradientColor(vec2 pos) {
|
||||
|
||||
bool isPointInsidePolygon(vec2 p) {
|
||||
if (pointCount < 3) return false;
|
||||
|
||||
int crossings = 0;
|
||||
for (int i = 0, j = pointCount - 1; i < pointCount; j = i++) {
|
||||
vec2 pi = points[i];
|
||||
vec2 pj = points[j];
|
||||
|
||||
// Skip degenerate edges
|
||||
if (distance(pi, pj) < 0.001) continue;
|
||||
|
||||
// Ray-casting
|
||||
if (((pi.y > p.y) != (pj.y > p.y)) &&
|
||||
(p.x < (pj.x - pi.x) * (p.y - pi.y) / (pj.y - pi.y + 0.001) + pi.x)) {
|
||||
crossings++;
|
||||
@@ -104,24 +85,24 @@ float distanceToEdge(vec2 p) {
|
||||
return minDist;
|
||||
}
|
||||
|
||||
float signedDistanceToPolygon(vec2 p) {
|
||||
float dist = distanceToEdge(p);
|
||||
bool inside = isPointInsidePolygon(p);
|
||||
return inside ? dist : -dist;
|
||||
}
|
||||
|
||||
void main() {
|
||||
vec2 pixel = fragTexCoord * resolution;
|
||||
|
||||
float signedDist = signedDistanceToPolygon(pixel);
|
||||
|
||||
// Compute pixel size for anti-aliasing
|
||||
vec2 pixelGrad = vec2(dFdx(pixel.x), dFdy(pixel.y));
|
||||
float pixelSize = length(pixelGrad);
|
||||
float aaWidth = max(0.5, pixelSize * 0.5); // Sharper anti-aliasing
|
||||
float aaWidth = max(0.5, pixelSize * 1.5);
|
||||
|
||||
float alpha = smoothstep(-aaWidth, aaWidth, signedDist);
|
||||
if (alpha > 0.0) {
|
||||
vec4 color = useGradient ? getGradientColor(fragTexCoord) : fillColor;
|
||||
bool inside = isPointInsidePolygon(pixel);
|
||||
if (inside) {
|
||||
finalColor = useGradient == 1 ? getGradientColor(pixel) : fillColor;
|
||||
return;
|
||||
}
|
||||
|
||||
float sd = -distanceToEdge(pixel);
|
||||
float alpha = smoothstep(-aaWidth, aaWidth, sd);
|
||||
if (alpha > 0.0){
|
||||
vec4 color = useGradient == 1 ? getGradientColor(pixel) : fillColor;
|
||||
finalColor = vec4(color.rgb, color.a * alpha);
|
||||
} else {
|
||||
finalColor = vec4(0.0);
|
||||
@@ -180,7 +161,6 @@ class ShaderState:
|
||||
'gradientStops': None,
|
||||
'gradientColorCount': None,
|
||||
'mvp': None,
|
||||
'visibleGradientRange': None,
|
||||
}
|
||||
|
||||
# Pre-allocated FFI objects
|
||||
@@ -191,7 +171,6 @@ class ShaderState:
|
||||
self.gradient_start_ptr = rl.ffi.new("float[]", [0.0, 0.0])
|
||||
self.gradient_end_ptr = rl.ffi.new("float[]", [0.0, 0.0])
|
||||
self.color_count_ptr = rl.ffi.new("int[]", [0])
|
||||
self.visible_gradient_range_ptr = rl.ffi.new("float[]", [0.0, 0.0])
|
||||
self.gradient_colors_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS * 4)
|
||||
self.gradient_stops_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS)
|
||||
|
||||
@@ -232,66 +211,40 @@ class ShaderState:
|
||||
self.initialized = False
|
||||
|
||||
|
||||
def _configure_shader_color(state, color, gradient, rect, min_xy, max_xy):
|
||||
"""Configure shader uniforms for solid color or gradient rendering"""
|
||||
def _configure_shader_color(state, color, gradient, clipped_rect, original_rect):
|
||||
use_gradient = 1 if gradient else 0
|
||||
state.use_gradient_ptr[0] = use_gradient
|
||||
rl.set_shader_value(state.shader, state.locations['useGradient'], state.use_gradient_ptr, UNIFORM_INT)
|
||||
|
||||
if use_gradient:
|
||||
# Set gradient start/end
|
||||
state.gradient_start_ptr[0:2] = gradient['start']
|
||||
state.gradient_end_ptr[0:2] = gradient['end']
|
||||
start = np.array(gradient['start']) * np.array([original_rect.width, original_rect.height]) + np.array([original_rect.x, original_rect.y])
|
||||
end = np.array(gradient['end']) * np.array([original_rect.width, original_rect.height]) + np.array([original_rect.x, original_rect.y])
|
||||
start = start - np.array([clipped_rect.x, clipped_rect.y])
|
||||
end = end - np.array([clipped_rect.x, clipped_rect.y])
|
||||
state.gradient_start_ptr[0:2] = start.astype(np.float32)
|
||||
state.gradient_end_ptr[0:2] = end.astype(np.float32)
|
||||
rl.set_shader_value(state.shader, state.locations['gradientStart'], state.gradient_start_ptr, UNIFORM_VEC2)
|
||||
rl.set_shader_value(state.shader, state.locations['gradientEnd'], state.gradient_end_ptr, UNIFORM_VEC2)
|
||||
|
||||
# Calculate visible gradient range
|
||||
width = max_xy[0] - min_xy[0]
|
||||
height = max_xy[1] - min_xy[1]
|
||||
|
||||
gradient_dir = (gradient['end'][0] - gradient['start'][0], gradient['end'][1] - gradient['start'][1])
|
||||
is_vertical = abs(gradient_dir[1]) > abs(gradient_dir[0])
|
||||
|
||||
visible_start = 0.0
|
||||
visible_end = 1.0
|
||||
|
||||
if is_vertical and height > 0:
|
||||
visible_start = (rect.y - min_xy[1]) / height
|
||||
visible_end = visible_start + rect.height / height
|
||||
elif width > 0:
|
||||
visible_start = (rect.x - min_xy[0]) / width
|
||||
visible_end = visible_start + rect.width / width
|
||||
|
||||
# Clamp visible range
|
||||
visible_start = max(0.0, min(1.0, visible_start))
|
||||
visible_end = max(0.0, min(1.0, visible_end))
|
||||
|
||||
state.visible_gradient_range_ptr[0:2] = [visible_start, visible_end]
|
||||
rl.set_shader_value(state.shader, state.locations['visibleGradientRange'], state.visible_gradient_range_ptr, UNIFORM_VEC2)
|
||||
|
||||
# Set gradient colors
|
||||
colors = gradient['colors']
|
||||
color_count = min(len(colors), MAX_GRADIENT_COLORS)
|
||||
state.color_count_ptr[0] = color_count
|
||||
for i, c in enumerate(colors[:color_count]):
|
||||
base_idx = i * 4
|
||||
state.gradient_colors_ptr[base_idx:base_idx+4] = [c.r / 255.0, c.g / 255.0, c.b / 255.0, c.a / 255.0]
|
||||
rl.set_shader_value_v(state.shader, state.locations['gradientColors'], state.gradient_colors_ptr, UNIFORM_VEC4, color_count)
|
||||
|
||||
# Set gradient stops
|
||||
stops = gradient.get('stops', [i / (color_count - 1) for i in range(color_count)])
|
||||
state.gradient_stops_ptr[0:color_count] = stops[:color_count]
|
||||
stops = gradient.get('stops', [i / max(1, color_count - 1) for i in range(color_count)])
|
||||
stops = np.clip(stops[:color_count], 0.0, 1.0)
|
||||
state.gradient_stops_ptr[0:color_count] = stops
|
||||
rl.set_shader_value_v(state.shader, state.locations['gradientStops'], state.gradient_stops_ptr, UNIFORM_FLOAT, color_count)
|
||||
|
||||
# Set color count
|
||||
state.color_count_ptr[0] = color_count
|
||||
rl.set_shader_value(state.shader, state.locations['gradientColorCount'], state.color_count_ptr, UNIFORM_INT)
|
||||
else:
|
||||
color = color or rl.WHITE # Default to white if no color provided
|
||||
color = color or rl.WHITE
|
||||
state.fill_color_ptr[0:4] = [color.r / 255.0, color.g / 255.0, color.b / 255.0, color.a / 255.0]
|
||||
rl.set_shader_value(state.shader, state.locations['fillColor'], state.fill_color_ptr, UNIFORM_VEC4)
|
||||
|
||||
|
||||
def draw_polygon(rect: rl.Rectangle, points: np.ndarray, color=None, gradient=None):
|
||||
def draw_polygon(origin_rect: rl.Rectangle, points: np.ndarray, color=None, gradient=None):
|
||||
"""
|
||||
Draw a complex polygon using shader-based even-odd fill rule
|
||||
|
||||
@@ -317,21 +270,16 @@ def draw_polygon(rect: rl.Rectangle, points: np.ndarray, color=None, gradient=No
|
||||
# Find bounding box
|
||||
min_xy = np.min(points, axis=0)
|
||||
max_xy = np.max(points, axis=0)
|
||||
|
||||
# Clip coordinates to rectangle
|
||||
clip_x = max(rect.x, min_xy[0])
|
||||
clip_y = max(rect.y, min_xy[1])
|
||||
clip_right = min(rect.x + rect.width, max_xy[0])
|
||||
clip_bottom = min(rect.y + rect.height, max_xy[1])
|
||||
clip_x = max(origin_rect.x, min_xy[0])
|
||||
clip_y = max(origin_rect.y, min_xy[1])
|
||||
clip_right = min(origin_rect.x + origin_rect.width, max_xy[0])
|
||||
clip_bottom = min(origin_rect.y + origin_rect.height, max_xy[1])
|
||||
|
||||
# Check if polygon is completely off-screen
|
||||
if clip_x >= clip_right or clip_y >= clip_bottom:
|
||||
return
|
||||
|
||||
clipped_width = clip_right - clip_x
|
||||
clipped_height = clip_bottom - clip_y
|
||||
|
||||
clip_rect = rl.Rectangle(clip_x, clip_y, clipped_width, clipped_height)
|
||||
clipped_rect = rl.Rectangle(clip_x, clip_y, clip_right - clip_x, clip_bottom - clip_y)
|
||||
|
||||
# Transform points relative to the CLIPPED area
|
||||
transformed_points = points - np.array([clip_x, clip_y])
|
||||
@@ -340,21 +288,21 @@ def draw_polygon(rect: rl.Rectangle, points: np.ndarray, color=None, gradient=No
|
||||
state.point_count_ptr[0] = len(transformed_points)
|
||||
rl.set_shader_value(state.shader, state.locations['pointCount'], state.point_count_ptr, UNIFORM_INT)
|
||||
|
||||
state.resolution_ptr[0:2] = [clipped_width, clipped_height]
|
||||
state.resolution_ptr[0:2] = [clipped_rect.width, clipped_rect.height]
|
||||
rl.set_shader_value(state.shader, state.locations['resolution'], state.resolution_ptr, UNIFORM_VEC2)
|
||||
|
||||
flat_points = np.ascontiguousarray(transformed_points.flatten().astype(np.float32))
|
||||
points_ptr = rl.ffi.cast("float *", flat_points.ctypes.data)
|
||||
rl.set_shader_value_v(state.shader, state.locations['points'], points_ptr, UNIFORM_VEC2, len(transformed_points))
|
||||
|
||||
_configure_shader_color(state, color, gradient, clip_rect, min_xy, max_xy)
|
||||
_configure_shader_color(state, color, gradient, clipped_rect, origin_rect)
|
||||
|
||||
# Render
|
||||
rl.begin_shader_mode(state.shader)
|
||||
rl.draw_texture_pro(
|
||||
state.white_texture,
|
||||
rl.Rectangle(0, 0, 2, 2),
|
||||
clip_rect,
|
||||
clipped_rect,
|
||||
rl.Vector2(0, 0),
|
||||
0.0,
|
||||
rl.WHITE,
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
import pyray as rl
|
||||
|
||||
_cache: dict[int, rl.Vector2] = {}
|
||||
|
||||
|
||||
def measure_text_cached(font: rl.Font, text: str, font_size: int, spacing: int = 0) -> rl.Vector2:
|
||||
key = hash((font.texture.id, text, font_size, spacing))
|
||||
if key in _cache:
|
||||
return _cache[key]
|
||||
|
||||
result = rl.measure_text_ex(font, text, font_size, spacing)
|
||||
_cache[key] = result
|
||||
return result
|
||||
@@ -1,234 +0,0 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from dataclasses import dataclass
|
||||
from cereal import messaging, log
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
|
||||
# Constants
|
||||
ALERT_COLORS = {
|
||||
log.SelfdriveState.AlertStatus.normal: rl.Color(0, 0, 0, 150), # Black
|
||||
log.SelfdriveState.AlertStatus.userPrompt: rl.Color(0xFE, 0x8C, 0x34, 100), # Orange
|
||||
log.SelfdriveState.AlertStatus.critical: rl.Color(0xC9, 0x22, 0x31, 150), # Red
|
||||
}
|
||||
|
||||
ALERT_HEIGHTS = {
|
||||
log.SelfdriveState.AlertSize.small: 271,
|
||||
log.SelfdriveState.AlertSize.mid: 420,
|
||||
}
|
||||
|
||||
SELFDRIVE_STATE_TIMEOUT = 5 # Seconds
|
||||
SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds
|
||||
|
||||
|
||||
@dataclass
|
||||
class Alert:
|
||||
text1: str = ""
|
||||
text2: str = ""
|
||||
alert_type: str = ""
|
||||
size: log.SelfdriveState.AlertSize = log.SelfdriveState.AlertSize.none
|
||||
status: log.SelfdriveState.AlertStatus = log.SelfdriveState.AlertStatus.normal
|
||||
|
||||
def is_equal(self, other: 'Alert') -> bool:
|
||||
"""Check if two alerts are equal."""
|
||||
return (
|
||||
self.text1 == other.text1
|
||||
and self.text2 == other.text2
|
||||
and self.alert_type == other.alert_type
|
||||
and self.size == other.size
|
||||
and self.status == other.status
|
||||
)
|
||||
|
||||
|
||||
class AlertRenderer:
|
||||
def __init__(self):
|
||||
"""Initialize the alert renderer."""
|
||||
self.alert: Alert = Alert()
|
||||
self.started_frame: int = 0
|
||||
self.font_regular: rl.Font = gui_app.font(FontWeight.NORMAL)
|
||||
self.font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
|
||||
self.font_metrics_cache: dict[tuple[str, int, str], rl.Vector2] = {}
|
||||
|
||||
def clear(self) -> None:
|
||||
"""Reset the alert to its default state."""
|
||||
self.alert = Alert()
|
||||
|
||||
def update_state(self, sm: messaging.SubMaster, started_frame: int) -> None:
|
||||
"""Update alert state based on SubMaster data."""
|
||||
self.started_frame = started_frame
|
||||
new_alert = self.get_alert(sm)
|
||||
if not self.alert.is_equal(new_alert):
|
||||
self.alert = new_alert
|
||||
|
||||
def get_alert(self, sm: messaging.SubMaster) -> Alert:
|
||||
"""Generate the current alert based on selfdrive state."""
|
||||
if not sm.valid['selfdriveState']:
|
||||
return Alert()
|
||||
|
||||
ss = sm['selfdriveState']
|
||||
selfdrive_frame = sm.recv_frame['selfdriveState']
|
||||
alert_status = self._get_enum_value(ss.alertStatus, log.SelfdriveState.AlertStatus)
|
||||
|
||||
# Return current alert if selfdrive state is recent
|
||||
if selfdrive_frame >= self.started_frame:
|
||||
return Alert(
|
||||
text1=ss.alertText1,
|
||||
text2=ss.alertText2,
|
||||
alert_type=ss.alertType,
|
||||
size=self._get_enum_value(ss.alertSize, log.SelfdriveState.AlertSize),
|
||||
status=alert_status,
|
||||
)
|
||||
|
||||
# Handle selfdrive timeout
|
||||
ss_missing = (np.uint64(rl.get_time() * 1e9) - sm.recv_time['selfdriveState']) / 1e9
|
||||
if selfdrive_frame < self.started_frame:
|
||||
return Alert(
|
||||
text1="openpilot Unavailable",
|
||||
text2="Waiting to start",
|
||||
alert_type="selfdriveWaiting",
|
||||
size=log.SelfdriveState.AlertSize.mid,
|
||||
status=log.SelfdriveState.AlertStatus.normal,
|
||||
)
|
||||
elif ss_missing > SELFDRIVE_STATE_TIMEOUT:
|
||||
if ss.enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < SELFDRIVE_UNRESPONSIVE_TIMEOUT:
|
||||
return Alert(
|
||||
text1="TAKE CONTROL IMMEDIATELY",
|
||||
text2="System Unresponsive",
|
||||
alert_type="selfdriveUnresponsive",
|
||||
size=log.SelfdriveState.AlertSize.full,
|
||||
status=log.SelfdriveState.AlertStatus.critical,
|
||||
)
|
||||
return Alert(
|
||||
text1="System Unresponsive",
|
||||
text2="Reboot Device",
|
||||
alert_type="selfdriveUnresponsivePermanent",
|
||||
size=log.SelfdriveState.AlertSize.mid,
|
||||
status=log.SelfdriveState.AlertStatus.normal,
|
||||
)
|
||||
|
||||
return Alert()
|
||||
|
||||
def draw(self, rect: rl.Rectangle, sm: messaging.SubMaster) -> None:
|
||||
"""Render the alert within the specified rectangle."""
|
||||
self.update_state(sm, sm.recv_frame['selfdriveState'])
|
||||
alert_size = self._get_enum_value(self.alert.size, log.SelfdriveState.AlertSize)
|
||||
if alert_size == log.SelfdriveState.AlertSize.none:
|
||||
return
|
||||
|
||||
# Calculate alert rectangle
|
||||
margin = 0 if alert_size == log.SelfdriveState.AlertSize.full else 40
|
||||
radius = 0 if alert_size == log.SelfdriveState.AlertSize.full else 30
|
||||
height = ALERT_HEIGHTS.get(alert_size, rect.height)
|
||||
alert_rect = rl.Rectangle(
|
||||
rect.x + margin,
|
||||
rect.y + rect.height - height + margin,
|
||||
rect.width - margin * 2,
|
||||
height - margin * 2,
|
||||
)
|
||||
|
||||
# Draw background
|
||||
alert_status = self._get_enum_value(self.alert.status, log.SelfdriveState.AlertStatus)
|
||||
color = ALERT_COLORS.get(alert_status, ALERT_COLORS[log.SelfdriveState.AlertStatus.normal])
|
||||
if alert_size != log.SelfdriveState.AlertSize.full:
|
||||
roundness = radius / (min(alert_rect.width, alert_rect.height) / 2)
|
||||
rl.draw_rectangle_rounded(alert_rect, roundness, 10, color)
|
||||
else:
|
||||
rl.draw_rectangle_rec(alert_rect, color)
|
||||
|
||||
# Draw text
|
||||
center_x = rect.x + rect.width / 2
|
||||
center_y = alert_rect.y + alert_rect.height / 2
|
||||
self._draw_text(alert_size, alert_rect, center_x, center_y)
|
||||
|
||||
def _draw_text(
|
||||
self, alert_size: log.SelfdriveState.AlertSize, alert_rect: rl.Rectangle, center_x: float, center_y: float
|
||||
) -> None:
|
||||
"""Draw text based on alert size."""
|
||||
if alert_size == log.SelfdriveState.AlertSize.small:
|
||||
font_size = 74
|
||||
text_width = self._measure_text(self.font_bold, self.alert.text1, font_size, 'bold').x
|
||||
rl.draw_text_ex(
|
||||
self.font_bold,
|
||||
self.alert.text1,
|
||||
rl.Vector2(center_x - text_width / 2, center_y - font_size / 2),
|
||||
font_size,
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
elif alert_size == log.SelfdriveState.AlertSize.mid:
|
||||
font_size1 = 88
|
||||
text1_width = self._measure_text(self.font_bold, self.alert.text1, font_size1, 'bold').x
|
||||
rl.draw_text_ex(
|
||||
self.font_bold,
|
||||
self.alert.text1,
|
||||
rl.Vector2(center_x - text1_width / 2, center_y - 125),
|
||||
font_size1,
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
font_size2 = 66
|
||||
text2_width = self._measure_text(self.font_regular, self.alert.text2, font_size2, 'regular').x
|
||||
rl.draw_text_ex(
|
||||
self.font_regular,
|
||||
self.alert.text2,
|
||||
rl.Vector2(center_x - text2_width / 2, center_y + 21),
|
||||
font_size2,
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
elif alert_size == log.SelfdriveState.AlertSize.full:
|
||||
is_long = len(self.alert.text1) > 15
|
||||
font_size1 = 132 if is_long else 177
|
||||
text1_y = alert_rect.y + (240 if is_long else 270)
|
||||
wrapped_text1 = self._wrap_text(self.alert.text1, alert_rect.width - 100, font_size1, self.font_bold)
|
||||
for i, line in enumerate(wrapped_text1):
|
||||
line_width = self._measure_text(self.font_bold, line, font_size1, 'bold').x
|
||||
rl.draw_text_ex(
|
||||
self.font_bold,
|
||||
line,
|
||||
rl.Vector2(center_x - line_width / 2, text1_y + i * font_size1),
|
||||
font_size1,
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
font_size2 = 88
|
||||
text2_y = alert_rect.y + alert_rect.height - (361 if is_long else 420)
|
||||
wrapped_text2 = self._wrap_text(self.alert.text2, alert_rect.width - 100, font_size2, self.font_regular)
|
||||
for i, line in enumerate(wrapped_text2):
|
||||
line_width = self._measure_text(self.font_regular, line, font_size2, 'regular').x
|
||||
rl.draw_text_ex(
|
||||
self.font_regular,
|
||||
line,
|
||||
rl.Vector2(center_x - line_width / 2, text2_y + i * font_size2),
|
||||
font_size2,
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
def _wrap_text(self, text: str, max_width: float, font_size: int, font: rl.Font) -> list[str]:
|
||||
"""Wrap text to fit within max width."""
|
||||
words = text.split()
|
||||
lines = []
|
||||
current_line = ""
|
||||
for word in words:
|
||||
test_line = f"{current_line} {word}" if current_line else word
|
||||
if self._measure_text(font, test_line, font_size, 'bold' if font == self.font_bold else 'regular').x <= max_width:
|
||||
current_line = test_line
|
||||
else:
|
||||
if current_line:
|
||||
lines.append(current_line)
|
||||
current_line = word
|
||||
if current_line:
|
||||
lines.append(current_line)
|
||||
return lines
|
||||
|
||||
def _measure_text(self, font: rl.Font, text: str, font_size: int, font_type: str) -> rl.Vector2:
|
||||
"""Measure text dimensions with caching."""
|
||||
key = (text, font_size, font_type)
|
||||
if key not in self.font_metrics_cache:
|
||||
self.font_metrics_cache[key] = rl.measure_text_ex(font, text, font_size, 0)
|
||||
return self.font_metrics_cache[key]
|
||||
|
||||
@staticmethod
|
||||
def _get_enum_value(enum_value, enum_type: type):
|
||||
"""Safely convert capnp enum to Python enum value."""
|
||||
return enum_value.raw if hasattr(enum_value, 'raw') else enum_value
|
||||
@@ -1,192 +0,0 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from enum import Enum
|
||||
|
||||
from cereal import messaging, log
|
||||
from msgq.visionipc import VisionStreamType
|
||||
from openpilot.system.ui.onroad.alert_renderer import AlertRenderer
|
||||
from openpilot.system.ui.onroad.driver_state import DriverStateRenderer
|
||||
from openpilot.system.ui.onroad.hud_renderer import HudRenderer
|
||||
from openpilot.system.ui.onroad.model_renderer import ModelRenderer
|
||||
from openpilot.system.ui.widgets.cameraview import CameraView
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame
|
||||
from openpilot.common.transformations.orientation import rot_from_euler
|
||||
|
||||
|
||||
OpState = log.SelfdriveState.OpenpilotState
|
||||
CALIBRATED = log.LiveCalibrationData.Status.calibrated
|
||||
DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"]
|
||||
UI_BORDER_SIZE = 30
|
||||
|
||||
class BorderStatus(Enum):
|
||||
DISENGAGED = rl.Color(0x17, 0x33, 0x49, 0xc8) # Blue for disengaged state
|
||||
OVERRIDE = rl.Color(0x91, 0x9b, 0x95, 0xf1) # Gray for override state
|
||||
ENGAGED = rl.Color(0x17, 0x86, 0x44, 0xf1) # Green for engaged state
|
||||
|
||||
|
||||
class AugmentedRoadView(CameraView):
|
||||
def __init__(self, sm: messaging.SubMaster, stream_type: VisionStreamType):
|
||||
super().__init__("camerad", stream_type)
|
||||
|
||||
self.sm = sm
|
||||
self.stream_type = stream_type
|
||||
self.is_wide_camera = stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD
|
||||
|
||||
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._last_calib_time: float = 0
|
||||
self._last_rect_dims = (0.0, 0.0)
|
||||
self._cached_matrix: np.ndarray | None = None
|
||||
self._content_rect = rl.Rectangle()
|
||||
|
||||
self.model_renderer = ModelRenderer()
|
||||
self._hud_renderer = HudRenderer()
|
||||
self.alert_renderer = AlertRenderer()
|
||||
self.driver_state_renderer = DriverStateRenderer()
|
||||
|
||||
def render(self, rect):
|
||||
# Update calibration before rendering
|
||||
self._update_calibration()
|
||||
|
||||
# Create inner content area with border padding
|
||||
self._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,
|
||||
)
|
||||
|
||||
# Draw colored border based on driving state
|
||||
self._draw_border(rect)
|
||||
|
||||
# Enable scissor mode to clip all rendering within content rectangle boundaries
|
||||
# This creates a rendering viewport that prevents graphics from drawing outside the border
|
||||
rl.begin_scissor_mode(
|
||||
int(self._content_rect.x),
|
||||
int(self._content_rect.y),
|
||||
int(self._content_rect.width),
|
||||
int(self._content_rect.height)
|
||||
)
|
||||
|
||||
# Render the base camera view
|
||||
super().render(rect)
|
||||
|
||||
# Draw all UI overlays
|
||||
self.model_renderer.draw(self._content_rect, self.sm)
|
||||
self._hud_renderer.draw(self._content_rect, self.sm)
|
||||
self.alert_renderer.draw(self._content_rect, self.sm)
|
||||
self.driver_state_renderer.draw(self._content_rect, self.sm)
|
||||
|
||||
# Custom UI extension point - add custom overlays here
|
||||
# Use self._content_rect for positioning within camera bounds
|
||||
|
||||
# End clipping region
|
||||
rl.end_scissor_mode()
|
||||
|
||||
def _draw_border(self, rect: rl.Rectangle):
|
||||
state = self.sm["selfdriveState"]
|
||||
if state.state in (OpState.preEnabled, OpState.overriding):
|
||||
status = BorderStatus.OVERRIDE
|
||||
elif state.enabled:
|
||||
status = BorderStatus.ENGAGED
|
||||
else:
|
||||
status = BorderStatus.DISENGAGED
|
||||
|
||||
rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, status.value)
|
||||
|
||||
def _update_calibration(self):
|
||||
# Update device camera if not already set
|
||||
if not self.device_camera and sm.seen['roadCameraState'] and sm.seen['deviceState']:
|
||||
self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
|
||||
|
||||
# Check if live calibration data is available and valid
|
||||
if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']):
|
||||
return
|
||||
|
||||
calib = self.sm['liveCalibration']
|
||||
if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED:
|
||||
return
|
||||
|
||||
# Update view_from_calib matrix
|
||||
device_from_calib = rot_from_euler(calib.rpyCalib)
|
||||
self.view_from_calib = view_frame_from_device_frame @ device_from_calib
|
||||
|
||||
# Update wide calibration if available
|
||||
if hasattr(calib, 'wideFromDeviceEuler') and len(calib.wideFromDeviceEuler) == 3:
|
||||
wide_from_device = rot_from_euler(calib.wideFromDeviceEuler)
|
||||
self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib
|
||||
|
||||
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
|
||||
# Check if we can use cached matrix
|
||||
calib_time = self.sm.recv_frame['liveCalibration']
|
||||
current_dims = (self._content_rect.width, self._content_rect.height)
|
||||
if (self._last_calib_time == calib_time and
|
||||
self._last_rect_dims == current_dims and
|
||||
self._cached_matrix is not None):
|
||||
return self._cached_matrix
|
||||
|
||||
# Get camera configuration
|
||||
device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA
|
||||
intrinsic = device_camera.ecam.intrinsics if self.is_wide_camera else device_camera.fcam.intrinsics
|
||||
calibration = self.view_from_wide_calib if self.is_wide_camera else self.view_from_calib
|
||||
zoom = 2.0 if self.is_wide_camera else 1.1
|
||||
|
||||
# Calculate transforms for vanishing point
|
||||
inf_point = np.array([1000.0, 0.0, 0.0])
|
||||
calib_transform = intrinsic @ calibration
|
||||
kep = calib_transform @ inf_point
|
||||
|
||||
# Calculate center points and dimensions
|
||||
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
|
||||
|
||||
# Update cache values
|
||||
self._last_calib_time = calib_time
|
||||
self._last_rect_dims = current_dims
|
||||
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)
|
||||
|
||||
return self._cached_matrix
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
gui_app.init_window("OnRoad Camera View")
|
||||
sm = messaging.SubMaster(["modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
|
||||
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
|
||||
"roadCameraState", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan"])
|
||||
road_camera_view = AugmentedRoadView(sm, VisionStreamType.VISION_STREAM_ROAD)
|
||||
try:
|
||||
for _ in gui_app.render():
|
||||
sm.update(0)
|
||||
road_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
|
||||
finally:
|
||||
road_camera_view.close()
|
||||
@@ -1,238 +0,0 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from dataclasses import dataclass
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
|
||||
# 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
|
||||
UI_BORDER_SIZE = 30
|
||||
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)
|
||||
|
||||
@dataclass
|
||||
class ArcData:
|
||||
"""Data structure for arc rendering parameters."""
|
||||
x: float
|
||||
y: float
|
||||
width: float
|
||||
height: float
|
||||
thickness: float
|
||||
|
||||
class DriverStateRenderer:
|
||||
def __init__(self):
|
||||
# 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.state_updated = False
|
||||
self.last_rect: rl.Rectangle = rl.Rectangle(0, 0, 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(37)] # 37 points for horizontal arc
|
||||
self.v_arc_lines = [rl.Vector2(0, 0) for _ in range(37)] # 37 points for vertical arc
|
||||
|
||||
# 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(26, 242, 66, 255)
|
||||
self.disengaged_color = rl.Color(139, 139, 139, 255)
|
||||
|
||||
def draw(self, rect, sm):
|
||||
if not self._is_visible(sm):
|
||||
return
|
||||
|
||||
self._update_state(sm, rect)
|
||||
if not self.state_updated:
|
||||
return
|
||||
|
||||
# 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
|
||||
engaged = True
|
||||
self.arc_color = self.engaged_color if 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 _is_visible(self, sm):
|
||||
"""Check if the visualization should be rendered."""
|
||||
return (sm.seen['driverStateV2'] and
|
||||
sm.seen['driverMonitoringState'] and
|
||||
sm['selfdriveState'].alertSize == 0)
|
||||
|
||||
def _update_state(self, sm, rect):
|
||||
"""Update the driver monitoring state based on model data"""
|
||||
if not sm.updated["driverMonitoringState"]:
|
||||
if self.state_updated and (rect.x != self.last_rect.x or rect.y != self.last_rect.y or \
|
||||
rect.width != self.last_rect.width or rect.height != self.last_rect.height):
|
||||
self._pre_calculate_drawing_elements(rect)
|
||||
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(rect)
|
||||
self.state_updated = True
|
||||
|
||||
def _pre_calculate_drawing_elements(self, rect):
|
||||
"""Pre-calculate all drawing elements based on the current rectangle"""
|
||||
# Calculate icon position (bottom-left or bottom-right)
|
||||
width, height = rect.width, rect.height
|
||||
offset = UI_BORDER_SIZE + BTN_SIZE // 2
|
||||
self.position_x = rect.x + (width - offset if self.is_rhd else offset)
|
||||
self.position_y = 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
|
||||
start_rad = np.deg2rad(start_angle)
|
||||
end_rad = np.deg2rad(start_angle + 180)
|
||||
angles = np.linspace(start_rad, end_rad, 37)
|
||||
|
||||
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
|
||||
@@ -1,194 +0,0 @@
|
||||
import pyray as rl
|
||||
from dataclasses import dataclass
|
||||
from cereal.messaging import SubMaster
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from enum import IntEnum
|
||||
|
||||
# Constants
|
||||
SET_SPEED_NA = 255
|
||||
KM_TO_MILE = 0.621371
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UIConfig:
|
||||
header_height: int = 300
|
||||
border_size: int = 30
|
||||
button_size: int = 192
|
||||
set_speed_width_metric: int = 200
|
||||
set_speed_width_imperial: int = 172
|
||||
set_speed_height: int = 204
|
||||
wheel_icon_size: int = 144
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FontSizes:
|
||||
current_speed: int = 176
|
||||
speed_unit: int = 66
|
||||
max_speed: int = 40
|
||||
set_speed: int = 90
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Colors:
|
||||
white: rl.Color = rl.Color(255, 255, 255, 255)
|
||||
disengaged: rl.Color = rl.Color(145, 155, 149, 255)
|
||||
override: rl.Color = rl.Color(145, 155, 149, 255) # Added
|
||||
engaged: rl.Color = rl.Color(128, 216, 166, 255)
|
||||
disengaged_bg: rl.Color = rl.Color(0, 0, 0, 153)
|
||||
override_bg: rl.Color = rl.Color(145, 155, 149, 204)
|
||||
engaged_bg: rl.Color = rl.Color(128, 216, 166, 204)
|
||||
grey: rl.Color = rl.Color(166, 166, 166, 255)
|
||||
dark_grey: rl.Color = rl.Color(114, 114, 114, 255)
|
||||
black_translucent: rl.Color = rl.Color(0, 0, 0, 166)
|
||||
white_translucent: rl.Color = rl.Color(255, 255, 255, 200)
|
||||
border_translucent: rl.Color = rl.Color(255, 255, 255, 75)
|
||||
header_gradient_start: rl.Color = rl.Color(0, 0, 0, 114)
|
||||
header_gradient_end: rl.Color = rl.Color(0, 0, 0, 0)
|
||||
|
||||
|
||||
UI_CONFIG = UIConfig()
|
||||
FONT_SIZES = FontSizes()
|
||||
COLORS = Colors()
|
||||
|
||||
|
||||
class HudStatus(IntEnum):
|
||||
DISENGAGED = 0
|
||||
OVERRIDE = 1
|
||||
ENGAGED = 2
|
||||
|
||||
|
||||
class HudRenderer:
|
||||
def __init__(self):
|
||||
"""Initialize the HUD renderer."""
|
||||
self.is_metric: bool = False
|
||||
self.status: HudStatus = HudStatus.DISENGAGED
|
||||
self.is_cruise_set: bool = False
|
||||
self.is_cruise_available: bool = False
|
||||
self.set_speed: float = SET_SPEED_NA
|
||||
self.speed: float = 0.0
|
||||
self.v_ego_cluster_seen: bool = False
|
||||
self.font_metrics_cache: dict[[str, int, str], rl.Vector2] = {}
|
||||
self._wheel_texture: rl.Texture = gui_app.texture('icons/chffr_wheel.png', UI_CONFIG.wheel_icon_size, UI_CONFIG.wheel_icon_size)
|
||||
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)
|
||||
|
||||
def _update_state(self, sm: SubMaster) -> None:
|
||||
"""Update HUD state based on car state and controls state."""
|
||||
self.is_metric = True
|
||||
self.status = HudStatus.DISENGAGED
|
||||
|
||||
if not sm.valid['carState']:
|
||||
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 self.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 self.is_metric else CV.MS_TO_MPH
|
||||
self.speed = max(0.0, v_ego * speed_conversion)
|
||||
|
||||
def draw(self, rect: rl.Rectangle, sm: SubMaster) -> None:
|
||||
"""Render HUD elements to the screen."""
|
||||
self._update_state(sm)
|
||||
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)
|
||||
self._draw_wheel_icon(rect)
|
||||
|
||||
def _draw_set_speed(self, rect: rl.Rectangle) -> None:
|
||||
"""Draw the MAX speed indicator box."""
|
||||
set_speed_width = UI_CONFIG.set_speed_width_metric if self.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.2, 30, COLORS.black_translucent)
|
||||
rl.draw_rectangle_rounded_lines_ex(set_speed_rect, 0.2, 30, 6, COLORS.border_translucent)
|
||||
|
||||
max_color = COLORS.grey
|
||||
set_speed_color = COLORS.dark_grey
|
||||
if self.is_cruise_set:
|
||||
set_speed_color = COLORS.white
|
||||
max_color = {
|
||||
HudStatus.DISENGAGED: COLORS.disengaged,
|
||||
HudStatus.OVERRIDE: COLORS.override,
|
||||
HudStatus.ENGAGED: COLORS.engaged,
|
||||
}.get(self.status, COLORS.grey)
|
||||
|
||||
max_text = "MAX"
|
||||
max_text_width = self._measure_text(max_text, self._font_semi_bold, FONT_SIZES.max_speed, 'semi_bold').x
|
||||
rl.draw_text_ex(
|
||||
self._font_semi_bold,
|
||||
max_text,
|
||||
rl.Vector2(x + (set_speed_width - max_text_width) / 2, y + 27),
|
||||
FONT_SIZES.max_speed,
|
||||
0,
|
||||
max_color,
|
||||
)
|
||||
|
||||
set_speed_text = "–" if not self.is_cruise_set else str(round(self.set_speed))
|
||||
speed_text_width = self._measure_text(set_speed_text, self._font_bold, FONT_SIZES.set_speed, 'bold').x
|
||||
rl.draw_text_ex(
|
||||
self._font_bold,
|
||||
set_speed_text,
|
||||
rl.Vector2(x + (set_speed_width - speed_text_width) / 2, y + 77),
|
||||
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 = self._measure_text(speed_text, self._font_bold, FONT_SIZES.current_speed, 'bold')
|
||||
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 = "km/h" if self.is_metric else "mph"
|
||||
unit_text_size = self._measure_text(unit_text, self._font_medium, FONT_SIZES.speed_unit, 'medium')
|
||||
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)
|
||||
|
||||
def _draw_wheel_icon(self, rect: rl.Rectangle) -> None:
|
||||
"""Draw the steering wheel icon with status-based opacity."""
|
||||
center_x = int(rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size / 2)
|
||||
center_y = int(rect.y + UI_CONFIG.border_size + UI_CONFIG.button_size / 2)
|
||||
rl.draw_circle(center_x, center_y, UI_CONFIG.button_size / 2, COLORS.black_translucent)
|
||||
|
||||
opacity = 0.7 if self.status == HudStatus.DISENGAGED else 1.0
|
||||
img_pos = rl.Vector2(center_x - self._wheel_texture.width / 2, center_y - self._wheel_texture.height / 2)
|
||||
rl.draw_texture_v(self._wheel_texture, img_pos, rl.Color(255, 255, 255, int(255 * opacity)))
|
||||
|
||||
def _measure_text(self, text: str, font: rl.Font, font_size: int, font_type: str) -> rl.Vector2:
|
||||
"""Measure text dimensions with caching."""
|
||||
key = (text, font_size, font_type)
|
||||
if key not in self.font_metrics_cache:
|
||||
self.font_metrics_cache[key] = rl.measure_text_ex(font, text, font_size, 0)
|
||||
return self.font_metrics_cache[key]
|
||||
@@ -1,414 +0,0 @@
|
||||
import colorsys
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from cereal import messaging, car
|
||||
from dataclasses import dataclass, field
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.ui.lib.application import DEFAULT_FPS
|
||||
from openpilot.system.ui.lib.shader_polygon import draw_polygon
|
||||
|
||||
|
||||
CLIP_MARGIN = 500
|
||||
MIN_DRAW_DISTANCE = 10.0
|
||||
MAX_DRAW_DISTANCE = 100.0
|
||||
PATH_COLOR_TRANSITION_DURATION = 0.5 # Seconds for color transition animation
|
||||
PATH_BLEND_INCREMENT = 1.0 / (PATH_COLOR_TRANSITION_DURATION * DEFAULT_FPS)
|
||||
|
||||
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:
|
||||
glow: list[float] = field(default_factory=list)
|
||||
chevron: list[float] = field(default_factory=list)
|
||||
fill_alpha: int = 0
|
||||
|
||||
|
||||
class ModelRenderer:
|
||||
def __init__(self):
|
||||
self._longitudinal_control = False
|
||||
self._experimental_mode = False
|
||||
self._blend_factor = 1.0
|
||||
self._prev_allow_throttle = True
|
||||
self._lane_line_probs = np.zeros(4, dtype=np.float32)
|
||||
self._road_edge_stds = np.zeros(2, dtype=np.float32)
|
||||
self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
|
||||
self._path_offset_z = 1.22
|
||||
|
||||
# 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._rect = None
|
||||
self._exp_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 draw(self, rect: rl.Rectangle, sm: messaging.SubMaster):
|
||||
# Check if data is up-to-date
|
||||
if not sm.valid['modelV2'] or not sm.valid['liveCalibration']:
|
||||
return
|
||||
|
||||
# Set up clipping region
|
||||
self._rect = rect
|
||||
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
|
||||
self._path_offset_z = sm['liveCalibration'].height[0]
|
||||
if sm.updated['carParams']:
|
||||
self._longitudinal_control = sm['carParams'].openpilotLongitudinalControl
|
||||
|
||||
model = sm['modelV2']
|
||||
radar_state = sm['radarState'] if sm.valid['radarState'] else None
|
||||
lead_one = radar_state.leadOne if radar_state else None
|
||||
render_lead_indicator = self._longitudinal_control and radar_state is not None
|
||||
|
||||
# Update model data when needed
|
||||
model_updated = sm.updated['modelV2']
|
||||
if model_updated or sm.updated['radarState'] or self._transform_dirty:
|
||||
if model_updated:
|
||||
self._update_raw_points(model)
|
||||
|
||||
pos_x_array = self._path.raw_points[:, 0]
|
||||
if pos_x_array.size == 0:
|
||||
return
|
||||
|
||||
self._update_model(lead_one, pos_x_array)
|
||||
if render_lead_indicator:
|
||||
self._update_leads(radar_state, pos_x_array)
|
||||
self._transform_dirty = False
|
||||
|
||||
|
||||
# Draw elements
|
||||
self._draw_lane_lines()
|
||||
self._draw_path(sm)
|
||||
|
||||
if render_lead_indicator and radar_state:
|
||||
self._draw_lead_indicator()
|
||||
|
||||
def _update_raw_points(self, model):
|
||||
"""Update raw 3D points from model data"""
|
||||
self._path.raw_points = np.array([model.position.x, model.position.y, 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, lane_line.y, 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, road_edge.y, 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, pos_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(pos_x_array, d_rel)
|
||||
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, z + self._path_offset_z)
|
||||
if point:
|
||||
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
|
||||
|
||||
def _update_model(self, lead, pos_x_array):
|
||||
"""Update model visualization data based on model message"""
|
||||
max_distance = np.clip(pos_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE)
|
||||
max_idx = self._get_path_length_idx(pos_x_array, 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
|
||||
)
|
||||
|
||||
# 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)
|
||||
|
||||
# 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(pos_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, allow_invert=False
|
||||
)
|
||||
|
||||
self._update_experimental_gradient(self._rect.height)
|
||||
|
||||
def _update_experimental_gradient(self, height):
|
||||
"""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:
|
||||
track_idx = max_len - i - 1 # flip idx to start from bottom right
|
||||
track_y = self._path.projected_points[track_idx][1]
|
||||
if track_y < 0 or track_y > height:
|
||||
i += 1
|
||||
continue
|
||||
|
||||
# Calculate color based on acceleration
|
||||
lin_grad_point = (height - track_y) / height
|
||||
|
||||
# speed up: 120, slow down: 0
|
||||
path_hue = max(min(60 + self._acceleration_x[i] * 35, 120), 0)
|
||||
path_hue = int(path_hue * 100 + 0.5) / 100
|
||||
|
||||
saturation = min(abs(self._acceleration_x[i] * 1.5), 1)
|
||||
lightness = self._map_val(saturation, 0.0, 1.0, 0.95, 0.62)
|
||||
alpha = self._map_val(lin_grad_point, 0.75 / 2.0, 0.75, 0.4, 0.0)
|
||||
|
||||
# Use HSL to RGB conversion
|
||||
color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha)
|
||||
|
||||
gradient_stops.append(lin_grad_point)
|
||||
segment_colors.append(color)
|
||||
|
||||
# Skip a point, unless next is last
|
||||
i += 1 + (1 if (i + 2) < max_len else 0)
|
||||
|
||||
# Store the gradient in the path object
|
||||
self._exp_gradient['colors'] = segment_colors
|
||||
self._exp_gradient['stops'] = gradient_stops
|
||||
|
||||
def _update_lead_vehicle(self, d_rel, v_rel, point, rect):
|
||||
speed_buff, lead_buff = 10.0, 40.0
|
||||
|
||||
# Calculate fill alpha
|
||||
fill_alpha = 0
|
||||
if d_rel < lead_buff:
|
||||
fill_alpha = 255 * (1.0 - (d_rel / lead_buff))
|
||||
if v_rel < 0:
|
||||
fill_alpha += 255 * (-1 * (v_rel / speed_buff))
|
||||
fill_alpha = min(fill_alpha, 255)
|
||||
|
||||
# Calculate size and position
|
||||
sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 2.35
|
||||
x = np.clip(point[0], 0.0, rect.width - sz / 2)
|
||||
y = min(point[1], rect.height - sz * 0.6)
|
||||
|
||||
g_xo = sz / 5
|
||||
g_yo = sz / 10
|
||||
|
||||
glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)]
|
||||
chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)]
|
||||
|
||||
return LeadVehicle(glow=glow,chevron=chevron, fill_alpha=int(fill_alpha))
|
||||
|
||||
def _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
|
||||
|
||||
if self._experimental_mode:
|
||||
# Draw with acceleration coloring
|
||||
if len(self._exp_gradient['colors']) > 2:
|
||||
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:
|
||||
# Draw with throttle/no throttle gradient
|
||||
allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
|
||||
|
||||
# Start transition if throttle state changes
|
||||
if allow_throttle != self._prev_allow_throttle:
|
||||
self._prev_allow_throttle = allow_throttle
|
||||
self._blend_factor = max(1.0 - self._blend_factor, 0.0)
|
||||
|
||||
# Update blend factor
|
||||
if self._blend_factor < 1.0:
|
||||
self._blend_factor = min(self._blend_factor + PATH_BLEND_INCREMENT, 1.0)
|
||||
|
||||
begin_colors = NO_THROTTLE_COLORS if allow_throttle else THROTTLE_COLORS
|
||||
end_colors = THROTTLE_COLORS if allow_throttle else NO_THROTTLE_COLORS
|
||||
|
||||
# Blend colors based on transition
|
||||
blended_colors = self._blend_colors(begin_colors, end_colors, self._blend_factor)
|
||||
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)
|
||||
|
||||
def _draw_lead_indicator(self):
|
||||
# Draw lead vehicles if available
|
||||
for lead in self._lead_vehicles:
|
||||
if not lead.glow or not lead.chevron:
|
||||
continue
|
||||
|
||||
rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255))
|
||||
rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha))
|
||||
|
||||
@staticmethod
|
||||
def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
|
||||
"""Get the index corresponding to the given path height"""
|
||||
idx = np.searchsorted(pos_x_array, path_height, side='right')
|
||||
return int(np.clip(idx - 1, 0, len(pos_x_array) - 1))
|
||||
|
||||
def _map_to_screen(self, in_x, in_y, in_z):
|
||||
"""Project a point in car space to screen space"""
|
||||
input_pt = np.array([in_x, in_y, in_z])
|
||||
pt = self._car_space_transform @ input_pt
|
||||
|
||||
if abs(pt[2]) < 1e-6:
|
||||
return None
|
||||
|
||||
x, y = pt[0] / pt[2], pt[1] / pt[2]
|
||||
|
||||
clip = self._clip_region
|
||||
if not (clip.x <= x <= clip.x + clip.width and clip.y <= y <= clip.y + clip.height):
|
||||
return None
|
||||
|
||||
return (x, y)
|
||||
|
||||
def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, allow_invert: bool = True) -> np.ndarray:
|
||||
"""Convert 3D line to 2D polygon for rendering."""
|
||||
if line.shape[0] == 0:
|
||||
return np.empty((0, 2), dtype=np.float32)
|
||||
|
||||
# Slice points and filter non-negative x-coordinates
|
||||
points = line[:max_idx + 1][line[:max_idx + 1, 0] >= 0]
|
||||
if points.shape[0] == 0:
|
||||
return np.empty((0, 2), dtype=np.float32)
|
||||
|
||||
# Create left and right 3D points in one array
|
||||
n_points = points.shape[0]
|
||||
points_3d = np.empty((n_points * 2, 3), dtype=np.float32)
|
||||
points_3d[:n_points, 0] = points_3d[n_points:, 0] = points[:, 0]
|
||||
points_3d[:n_points, 1] = points[:, 1] - y_off
|
||||
points_3d[n_points:, 1] = points[:, 1] + y_off
|
||||
points_3d[:n_points, 2] = points_3d[n_points:, 2] = points[:, 2] + z_off
|
||||
|
||||
# Single matrix multiplication for projections
|
||||
proj = self._car_space_transform @ points_3d.T
|
||||
valid_z = np.abs(proj[2]) > 1e-6
|
||||
if not np.any(valid_z):
|
||||
return np.empty((0, 2), dtype=np.float32)
|
||||
|
||||
# Compute screen coordinates
|
||||
screen = proj[:2, valid_z] / proj[2, valid_z][None, :]
|
||||
left_screen = screen[:, :n_points].T
|
||||
right_screen = screen[:, n_points:].T
|
||||
|
||||
# Ensure consistent shapes by re-aligning valid points
|
||||
valid_points = np.minimum(left_screen.shape[0], right_screen.shape[0])
|
||||
if valid_points == 0:
|
||||
return np.empty((0, 2), dtype=np.float32)
|
||||
left_screen = left_screen[:valid_points]
|
||||
right_screen = right_screen[:valid_points]
|
||||
|
||||
if self._clip_region:
|
||||
clip = self._clip_region
|
||||
bounds_mask = (
|
||||
(left_screen[:, 0] >= clip.x) & (left_screen[:, 0] <= clip.x + clip.width) &
|
||||
(left_screen[:, 1] >= clip.y) & (left_screen[:, 1] <= clip.y + clip.height) &
|
||||
(right_screen[:, 0] >= clip.x) & (right_screen[:, 0] <= clip.x + clip.width) &
|
||||
(right_screen[:, 1] >= clip.y) & (right_screen[:, 1] <= clip.y + clip.height)
|
||||
)
|
||||
if not np.any(bounds_mask):
|
||||
return np.empty((0, 2), dtype=np.float32)
|
||||
left_screen = left_screen[bounds_mask]
|
||||
right_screen = right_screen[bounds_mask]
|
||||
|
||||
if not allow_invert and left_screen.shape[0] > 1:
|
||||
keep = np.concatenate(([True], np.diff(left_screen[:, 1]) < 0))
|
||||
left_screen = left_screen[keep]
|
||||
right_screen = right_screen[keep]
|
||||
if left_screen.shape[0] == 0:
|
||||
return np.empty((0, 2), dtype=np.float32)
|
||||
|
||||
return np.vstack((left_screen, right_screen[::-1])).astype(np.float32)
|
||||
|
||||
@staticmethod
|
||||
def _map_val(x, x0, x1, y0, y1):
|
||||
x = max(x0, min(x, x1))
|
||||
ra = x1 - x0
|
||||
rb = y1 - y0
|
||||
return (x - x0) * rb / ra + y0 if ra != 0 else y0
|
||||
|
||||
@staticmethod
|
||||
def _hsla_to_color(h, s, l, a):
|
||||
r, g, b = [max(0, min(255, int(v * 255))) for v in colorsys.hls_to_rgb(h, l, s)]
|
||||
return rl.Color(r, g, b, max(0, min(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)]
|
||||
@@ -1,249 +0,0 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.system.hardware import TICI
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
||||
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
|
||||
|
||||
|
||||
CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts
|
||||
|
||||
VERTEX_SHADER = """
|
||||
#version 300 es
|
||||
in vec3 vertexPosition;
|
||||
in vec2 vertexTexCoord;
|
||||
in vec3 vertexNormal;
|
||||
in vec4 vertexColor;
|
||||
uniform mat4 mvp;
|
||||
out vec2 fragTexCoord;
|
||||
out vec4 fragColor;
|
||||
void main() {
|
||||
fragTexCoord = vertexTexCoord;
|
||||
fragColor = vertexColor;
|
||||
gl_Position = mvp * vec4(vertexPosition, 1.0);
|
||||
}
|
||||
"""
|
||||
|
||||
# Choose fragment shader based on platform capabilities
|
||||
if TICI:
|
||||
FRAME_FRAGMENT_SHADER = """
|
||||
#version 300 es
|
||||
#extension GL_OES_EGL_image_external_essl3 : enable
|
||||
precision mediump float;
|
||||
in vec2 fragTexCoord;
|
||||
uniform samplerExternalOES texture0;
|
||||
out vec4 fragColor;
|
||||
void main() {
|
||||
vec4 color = texture(texture0, fragTexCoord);
|
||||
fragColor = vec4(pow(color.rgb, vec3(1.0/1.28)), color.a);
|
||||
}
|
||||
"""
|
||||
else:
|
||||
FRAME_FRAGMENT_SHADER = """
|
||||
#version 300 es
|
||||
precision mediump float;
|
||||
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:
|
||||
def __init__(self, name: str, stream_type: VisionStreamType):
|
||||
self.client = VisionIpcClient(name, stream_type, False)
|
||||
self._texture_needs_update = True
|
||||
self.last_connection_attempt: float = 0.0
|
||||
self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER)
|
||||
self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not TICI else -1
|
||||
|
||||
self.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
|
||||
|
||||
# Initialize EGL for zero-copy rendering on TICI
|
||||
if TICI:
|
||||
if not init_egl():
|
||||
raise RuntimeError("Failed to initialize EGL")
|
||||
|
||||
# Create a 1x1 pixel placeholder texture for EGL image binding
|
||||
temp_image = rl.gen_image_color(1, 1, rl.BLACK)
|
||||
self.egl_texture = rl.load_texture_from_image(temp_image)
|
||||
rl.unload_image(temp_image)
|
||||
|
||||
def close(self) -> None:
|
||||
self._clear_textures()
|
||||
|
||||
# Clean up EGL texture
|
||||
if TICI and self.egl_texture:
|
||||
rl.unload_texture(self.egl_texture)
|
||||
self.egl_texture = None
|
||||
|
||||
# Clean up shader
|
||||
if self.shader and self.shader.id:
|
||||
rl.unload_shader(self.shader)
|
||||
|
||||
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 not self._ensure_connection():
|
||||
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
|
||||
|
||||
if not self.frame:
|
||||
return
|
||||
|
||||
transform = self._calc_frame_matrix(rect)
|
||||
src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height))
|
||||
|
||||
# Calculate scale
|
||||
scale_x = rect.width * transform[0, 0] # zx
|
||||
scale_y = rect.height * transform[1, 1] # zy
|
||||
|
||||
# Calculate base position (centered)
|
||||
x_offset = rect.x + (rect.width - scale_x) / 2
|
||||
y_offset = rect.y + (rect.height - scale_y) / 2
|
||||
|
||||
x_offset += transform[0, 2] * rect.width / 2
|
||||
y_offset += transform[1, 2] * rect.height / 2
|
||||
|
||||
dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y)
|
||||
|
||||
# Render with appropriate method
|
||||
if TICI:
|
||||
self._render_egl(src_rect, dst_rect)
|
||||
else:
|
||||
self._render_textures(src_rect, dst_rect)
|
||||
|
||||
def _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
|
||||
|
||||
# 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
|
||||
|
||||
self._clear_textures()
|
||||
|
||||
if not TICI:
|
||||
self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride),
|
||||
int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE))
|
||||
self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2),
|
||||
int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA))
|
||||
|
||||
return True
|
||||
|
||||
def _clear_textures(self):
|
||||
if self.texture_y and self.texture_y.id:
|
||||
rl.unload_texture(self.texture_y)
|
||||
self.texture_y = None
|
||||
|
||||
if self.texture_uv and self.texture_uv.id:
|
||||
rl.unload_texture(self.texture_uv)
|
||||
self.texture_uv = None
|
||||
|
||||
# Clean up EGL resources
|
||||
if TICI:
|
||||
for data in self.egl_images.values():
|
||||
destroy_egl_image(data)
|
||||
self.egl_images = {}
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
gui_app.init_window("watch3")
|
||||
road_camera_view = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD)
|
||||
driver_camera_view = CameraView("camerad", VisionStreamType.VISION_STREAM_DRIVER)
|
||||
wide_road_camera_view = CameraView("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD)
|
||||
try:
|
||||
for _ in gui_app.render():
|
||||
road_camera_view.render(rl.Rectangle(gui_app.width // 4, 0, gui_app.width // 2, gui_app.height // 2))
|
||||
driver_camera_view.render(rl.Rectangle(0, gui_app.height // 2, gui_app.width // 2, gui_app.height // 2))
|
||||
wide_road_camera_view.render(rl.Rectangle(gui_app.width // 2, gui_app.height // 2, gui_app.width // 2, gui_app.height // 2))
|
||||
finally:
|
||||
road_camera_view.close()
|
||||
driver_camera_view.close()
|
||||
wide_road_camera_view.close()
|
||||
@@ -1,46 +0,0 @@
|
||||
import numpy as np
|
||||
import pyray as rl
|
||||
from openpilot.system.ui.widgets.cameraview import CameraView
|
||||
from msgq.visionipc import VisionStreamType
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
|
||||
|
||||
class DriverCameraView(CameraView):
|
||||
def __init__(self, stream_type: VisionStreamType):
|
||||
super().__init__("camerad", stream_type)
|
||||
|
||||
def render(self, rect):
|
||||
super().render(rect)
|
||||
|
||||
# TODO: Add additional rendering logic
|
||||
|
||||
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 = DriverCameraView(VisionStreamType.VISION_STREAM_DRIVER)
|
||||
try:
|
||||
for _ in gui_app.render():
|
||||
driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
|
||||
finally:
|
||||
driver_camera_view.close()
|
||||
@@ -15,7 +15,7 @@ NM_DEVICE_STATE_NEED_AUTH = 60
|
||||
MIN_PASSWORD_LENGTH = 8
|
||||
MAX_PASSWORD_LENGTH = 64
|
||||
ITEM_HEIGHT = 160
|
||||
ICON_SIZE = 49
|
||||
ICON_SIZE = 50
|
||||
|
||||
STRENGTH_ICONS = [
|
||||
"icons/wifi_strength_low.png",
|
||||
|
||||
Reference in New Issue
Block a user