From bdde8907bf5790117e53c12182c533cfbcd6e055 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Fri, 10 Jul 2026 05:04:37 -0400 Subject: [PATCH] BigUI WIP: Mici Mouseface --- selfdrive/ui/onroad/driver_state.py | 269 ++++++-------------- selfdrive/ui/onroad/starpilot/torque_bar.py | 2 +- 2 files changed, 76 insertions(+), 195 deletions(-) diff --git a/selfdrive/ui/onroad/driver_state.py b/selfdrive/ui/onroad/driver_state.py index 2b9cc8f02..29027c087 100644 --- a/selfdrive/ui/onroad/driver_state.py +++ b/selfdrive/ui/onroad/driver_state.py @@ -1,7 +1,7 @@ -import numpy as np +import math import pyray as rl from cereal import log -from dataclasses import dataclass +from openpilot.common.filter_simple import FirstOrderFilter 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 @@ -9,226 +9,107 @@ 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 +DMOJI_SIZE = 128 +CONE_SIZE = round(52 / 60 * DMOJI_SIZE) -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 +CONE_COLOR_GREEN = (0, 255, 64) +CONE_COLOR_ORANGE = (255, 115, 0) +AWARENESS_UNFULL_PERCENT = 95 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 - self.x_shift: float = 0.0 + self.is_active = False + self.is_rhd = False + self.is_face_detected = False + self._awareness_unfull = False - # 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)] + self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) + self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps) + self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) + self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) + self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False) - # 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) - - self.set_visible(lambda: (ui_state.sm["selfdriveState"].alertSize == AlertSize.none and - ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame and - not ui_state.starpilot_toggles.get("hide_dm_icon", False))) + self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", DMOJI_SIZE, DMOJI_SIZE) + self._dm_person = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_person.png", CONE_SIZE, CONE_SIZE) + self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", CONE_SIZE, CONE_SIZE) def _render(self, rect): - # Set opacity based on active state - opacity = 0.65 if self.is_active else 0.2 + bg = self._dm_background + rl.draw_texture_ex(bg, + rl.Vector2(self.position_x - bg.width / 2, + self.position_y - bg.height / 2), + 0.0, 1.0, + rl.Color(255, 255, 255, int(255 * self._fade_filter.x))) - # Draw background circle - rl.draw_circle(int(self.position_x), int(self.position_y), BTN_SIZE // 2, rl.Color(0, 0, 0, 70)) + person = self._dm_person + rl.draw_texture_ex(person, + rl.Vector2(self.position_x - person.width / 2, + self.position_y - person.height / 2), + 0.0, 1.0, + rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x))) - # 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) + green_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0) + if self.is_active: + cone = self._dm_cone + source_rect = rl.Rectangle(0, 0, cone.width, cone.height) + dest_rect = rl.Rectangle( + self.position_x, + self.position_y, + cone.width, + cone.height, + ) + r = int(round(CONE_COLOR_GREEN[0] * green_amount + CONE_COLOR_ORANGE[0] * (1 - green_amount))) + g = int(round(CONE_COLOR_GREEN[1] * green_amount + CONE_COLOR_ORANGE[1] * (1 - green_amount))) + b = int(round(CONE_COLOR_GREEN[2] * green_amount + CONE_COLOR_ORANGE[2] * (1 - green_amount))) + rl.draw_texture_pro( + cone, + source_rect, + dest_rect, + rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), + self._rotation_filter.x - 90, + rl.Color(r, g, b, int(255 * self._fade_filter.x)), + ) 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.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision self.is_rhd = dm_state.isRHD + self.is_face_detected = dm_state.visionPolicyState.faceDetected + self._awareness_unfull = self.is_active and dm_state.visionPolicyState.awarenessPercent < AWARENESS_UNFULL_PERCENT + face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6) + face_yaw = -dm_state.visionPolicyState.pose.yaw - # 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) + pitch = self._pitch_filter.update(face_pitch) + yaw = self._yaw_filter.update(face_yaw) - # 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 + rotation = math.degrees(math.atan2(pitch * 2, yaw)) + angle_diff = rotation - self._rotation_filter.x + angle_diff = ((angle_diff + 180) % 360) - 180 + self._rotation_filter.update(self._rotation_filter.x + angle_diff) - # 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 + alert_ok = sm["selfdriveState"].alertSize == AlertSize.none + data_fresh = sm.recv_frame["driverStateV2"] > ui_state.started_frame + not_hidden = not ui_state.starpilot_toggles.get("hide_dm_icon", False) + should_draw = alert_ok and data_fresh and not_hidden - # 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) + if not should_draw: + self._fade_filter.update(0.0) + elif not self.is_active: + self._fade_filter.update(0.35) + else: + self._fade_filter.update(1.0) - # 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], - ] - ) + self._pre_calculate_position() - # 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) + def _pre_calculate_position(self): width, height = self._rect.width, self._rect.height - offset = UI_BORDER_SIZE + BTN_SIZE // 2 + offset = UI_BORDER_SIZE + BTN_SIZE // 2 + 20 self.position_x = self._rect.x + (width - offset if self.is_rhd else offset) + self.x_shift 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 diff --git a/selfdrive/ui/onroad/starpilot/torque_bar.py b/selfdrive/ui/onroad/starpilot/torque_bar.py index 5916ada1d..134187b19 100644 --- a/selfdrive/ui/onroad/starpilot/torque_bar.py +++ b/selfdrive/ui/onroad/starpilot/torque_bar.py @@ -189,7 +189,7 @@ class TorqueBar(Widget): self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) def _render(self, rect: rl.Rectangle) -> None: - SCALE = rect.height / 240.0 + SCALE = rect.height / 240.0 * (rect.width / gui_app.width) # adjust y pos with torque torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22 * SCALE, 26 * SCALE]) torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14 * SCALE, 56 * SCALE])