import pyray as rl import numpy as np import math from cereal import log from openpilot.common.filter_simple import FirstOrderFilter from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.ui_state import ui_state AlertSize = log.SelfdriveState.AlertSize DEBUG = False # TODO: Only left for DM preview, remove LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6) LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3) CONE_COLOR_GREEN = (0, 255, 64) CONE_COLOR_ORANGE = (255, 115, 0) class DriverStateRenderer(Widget): BASE_SIZE = 60 LINES_ANGLE_INCREMENT = 5 LINES_STALE_ANGLES = 3.0 # seconds AWARENESS_UNFULL_PERCENT = 95 # ~0.5s def __init__(self, lines: bool = False, inset: bool = False): super().__init__() self.set_rect(rl.Rectangle(0, 0, self.BASE_SIZE, self.BASE_SIZE)) self._lines = lines self._inset = inset # In line mode, track smoothed angles assert 360 % self.LINES_ANGLE_INCREMENT == 0 self._head_angles = {i * self.LINES_ANGLE_INCREMENT: FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) for i in range(360 // self.LINES_ANGLE_INCREMENT)} self._is_active = False self._is_rhd = False self._face_detected = False self._face_pitch = 0. self._face_yaw = 0. self._should_draw = False self._force_active = False self._looking_center = False self._awareness_unfull = False self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps) self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False) self._looking_center_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) # Load the driver face icons self.load_icons() def load_icons(self): """Load or reload the driver face icon texture""" cone_and_person_size = round(52 / self.BASE_SIZE * self._rect.width) # If inset is enabled, push cone and person smaller by 2x the current inset space if self._inset: # Current inset space = (rect.width - cone_and_person_size) / 2 current_inset = (self._rect.width - cone_and_person_size) / 2 # Reduce size by 2x the current inset (1x on each side) cone_and_person_size = round(cone_and_person_size - current_inset * 2) self._dm_person = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_person.png", cone_and_person_size, cone_and_person_size) self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", cone_and_person_size, cone_and_person_size) self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", int(self._rect.width), int(self._rect.height)) def set_should_draw(self, should_draw: bool): self._should_draw = should_draw @property def should_draw(self): return (self._should_draw and ui_state.sm["selfdriveState"].alertSize == AlertSize.none and ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame) def set_force_active(self, force_active: bool): """Force the dmoji to always appear active (green) regardless of actual state""" self._force_active = force_active @property def effective_active(self) -> bool: """Returns True if dmoji should appear active (either actually active or forced)""" return bool(self._force_active or self._is_active) @property def is_rhd(self) -> bool: return self._is_rhd def _render(self, _): if DEBUG: rl.draw_rectangle_lines_ex(self._rect, 1, rl.RED) rl.draw_texture_ex(self._dm_background, rl.Vector2(self._rect.x, self._rect.y), 0.0, 1.0, rl.Color(255, 255, 255, int(255 * self._fade_filter.x))) rl.draw_texture_ex(self._dm_person, rl.Vector2(self._rect.x + (self._rect.width - self._dm_person.width) / 2, self._rect.y + (self._rect.height - self._dm_person.height) / 2), 0.0, 1.0, rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x))) green_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0) if self.effective_active: source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height) dest_rect = rl.Rectangle( self._rect.x + self._rect.width / 2, self._rect.y + self._rect.height / 2, self._dm_cone.width, self._dm_cone.height, ) if not self._lines: r = int(round(CONE_COLOR_GREEN[0] * green_amount + CONE_COLOR_ORANGE[0] * (1 - green_amount))) g = int(round(CONE_COLOR_GREEN[1] * green_amount + CONE_COLOR_ORANGE[1] * (1 - green_amount))) b = int(round(CONE_COLOR_GREEN[2] * green_amount + CONE_COLOR_ORANGE[2] * (1 - green_amount))) rl.draw_texture_pro( self._dm_cone, source_rect, dest_rect, rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), self._rotation_filter.x - 90, rl.Color(r, g, b, int(255 * self._fade_filter.x)), ) else: # remove old angles for angle, f in self._head_angles.items(): dst_from_current = ((angle - self._rotation_filter.x) % 360) - 180 target = 1.0 if abs(dst_from_current) <= self.LINES_ANGLE_INCREMENT * 5 else 0.0 if not self._face_detected: target = 0.0 # Reduce all line lengths when looking center if self._looking_center: target = np.interp(self._looking_center_filter.x, [0.0, 1.0], [target, 0.45]) f.update(target) self._draw_line(angle, f, self._looking_center) def _draw_line(self, angle: int, f: FirstOrderFilter, grey: bool): line_length = self._rect.width / 6 line_length = round(np.interp(f.x, [0.0, 1.0], [0, line_length])) line_offset = self._rect.width / 2 - line_length * 2 # ensure line ends within rect center_x = self._rect.x + self._rect.width / 2 center_y = self._rect.y + self._rect.height / 2 start_x = center_x + (line_offset + line_length) * math.cos(math.radians(angle)) start_y = center_y + (line_offset + line_length) * math.sin(math.radians(angle)) end_x = start_x + line_length * math.cos(math.radians(angle)) end_y = start_y + line_length * math.sin(math.radians(angle)) color = rl.Color(0, 255, 64, 255) if grey: color = rl.Color(166, 166, 166, 255) if f.x > 0.01: rl.draw_line_ex((start_x, start_y), (end_x, end_y), 12, color) def get_driver_data(self): sm = ui_state.sm dm_state = sm["driverMonitoringState"] self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision self._is_rhd = dm_state.isRHD self._face_detected = dm_state.visionPolicyState.faceDetected self._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6) # calib or DM pose is not accurate, add a fake upward pitch to bias forward self._face_yaw = -dm_state.visionPolicyState.pose.yaw # undo sign flip in face_orientation_from_model to match UI convention driverstate = sm["driverStateV2"] driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData return driver_data def _update_state(self): # Get monitoring state _ = self.get_driver_data() pitch = self._pitch_filter.update(self._face_pitch) yaw = self._yaw_filter.update(self._face_yaw) # hysteresis on looking center if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER: self._looking_center = True elif abs(pitch) > LOOKING_CENTER_THRESHOLD_UPPER or abs(yaw) > LOOKING_CENTER_THRESHOLD_UPPER: self._looking_center = False self._looking_center_filter.update(1 if self._looking_center else 0) if DEBUG: pitchd = math.degrees(pitch) yawd = math.degrees(yaw) rl.draw_line_ex((0, 100), (200, 100), 3, rl.RED) rl.draw_line_ex((0, 120), (200, 120), 3, rl.RED) rl.draw_line_ex((0, 140), (200, 140), 3, rl.RED) pitch_x = 100 + pitchd yaw_x = 100 + yawd rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN) rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN) # filter head rotation, handling wrap-around rotation = math.degrees(math.atan2(pitch * 2, yaw)) # reduce yaw sensitivity angle_diff = rotation - self._rotation_filter.x angle_diff = ((angle_diff + 180) % 360) - 180 self._rotation_filter.update(self._rotation_filter.x + angle_diff) if not self.should_draw: self._fade_filter.update(0.0) elif not self.effective_active: self._fade_filter.update(0.35) else: self._fade_filter.update(1.0)