import pyray as rl import numpy as np import math from iqpilot.cereal import log from iqpilot.common.filter_simple import FirstOrderFilter from iqpilot.system.ui.lib.application import gui_app from iqpilot.system.ui.widgets import Widget from iqpilot.selfdrive.ui.ui_state import ui_state from iqpilot.selfdrive.monitoring.helpers import face_orientation_from_net AlertSize = log.SelfdriveState.AlertSize DEBUG = False ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF) LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6) LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3) class DriverStateRenderer(Widget): BASE_SIZE = 60 LINES_ANGLE_INCREMENT = 5 LINES_STALE_ANGLES = 3.0 # seconds def __init__(self, lines: bool = False, inset: bool = False): super().__init__() self.set_rect(rl.Rectangle(0, 0, self.BASE_SIZE, self.BASE_SIZE)) self._lines = lines self._inset = inset # In line mode, track smoothed angles assert 360 % self.LINES_ANGLE_INCREMENT == 0 self._head_angles = {i * self.LINES_ANGLE_INCREMENT: FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) for i in range(360 // self.LINES_ANGLE_INCREMENT)} self._is_active = False self._is_rhd = False self._face_detected = False self._should_draw = False self._force_active = False self._looking_center = False self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False) self._looking_center_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) # Load the driver face icons self.load_icons() def load_icons(self): cone_and_person_size = round(56 / self.BASE_SIZE * self._rect.width) if self._inset: current_inset = (self._rect.width - cone_and_person_size) / 2 cone_and_person_size = round(cone_and_person_size - current_inset * 2) self._dm_person = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_person.png", cone_and_person_size, cone_and_person_size) self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", cone_and_person_size, cone_and_person_size) self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", self._rect.width, self._rect.height) def set_should_draw(self, should_draw: bool): self._should_draw = should_draw @property def should_draw(self): return (self._should_draw and ui_state.sm["selfdriveState"].alertSize == AlertSize.none and ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame) def set_force_active(self, force_active: bool): """Force the dmoji to always appear active (green) regardless of actual state""" self._force_active = force_active @property def effective_active(self) -> bool: """Returns True if dmoji should appear active (either actually active or forced)""" return bool(self._force_active or self._is_active) @property def is_rhd(self) -> bool: return self._is_rhd def _render(self, _): if DEBUG: rl.draw_rectangle_lines_ex(self._rect, 1, rl.RED) rl.draw_texture(self._dm_background, int(self._rect.x), int(self._rect.y), rl.Color(255, 255, 255, int(255 * self._fade_filter.x))) rl.draw_texture(self._dm_person, int(self._rect.x + (self._rect.width - self._dm_person.width) / 2), int(self._rect.y + (self._rect.height - self._dm_person.height) / 2), rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x))) if self.effective_active: source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height) dest_rect = rl.Rectangle( self._rect.x + self._rect.width / 2, self._rect.y + self._rect.height / 2, self._dm_cone.width, self._dm_cone.height, ) if not self._lines: rl.draw_texture_pro( self._dm_cone, source_rect, dest_rect, rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), self._rotation_filter.x - 90, rl.Color(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.b, int(255 * self._fade_filter.x)), ) else: # remove old angles for angle, f in self._head_angles.items(): dst_from_current = ((angle - self._rotation_filter.x) % 360) - 180 target = 1.0 if abs(dst_from_current) <= self.LINES_ANGLE_INCREMENT * 5 else 0.0 if not self._face_detected: target = 0.0 # Reduce all line lengths when looking center if self._looking_center: target = np.interp(self._looking_center_filter.x, [0.0, 1.0], [target, 0.45]) f.update(target) self._draw_line(angle, f, self._looking_center) def _draw_line(self, angle: int, f: FirstOrderFilter, grey: bool): line_length = self._rect.width / 6 line_length = round(np.interp(f.x, [0.0, 1.0], [0, line_length])) line_offset = self._rect.width / 2 - line_length * 2 # ensure line ends within rect center_x = self._rect.x + self._rect.width / 2 center_y = self._rect.y + self._rect.height / 2 start_x = center_x + (line_offset + line_length) * math.cos(math.radians(angle)) start_y = center_y + (line_offset + line_length) * math.sin(math.radians(angle)) end_x = start_x + line_length * math.cos(math.radians(angle)) end_y = start_y + line_length * math.sin(math.radians(angle)) color = ACTIVE_ACCENT if grey: color = rl.Color(166, 166, 166, 255) if f.x > 0.01: rl.draw_line_ex((start_x, start_y), (end_x, end_y), 12, color) def get_driver_data(self): sm = ui_state.sm dm_state = sm["driverMonitoringState"] self._is_active = dm_state.isActiveMode self._is_rhd = dm_state.isRHD self._face_detected = dm_state.faceDetected driverstate = sm["driverStateV2"] driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData return driver_data def _update_state(self): # Get monitoring state driver_data = self.get_driver_data() driver_orient = driver_data.faceOrientation if len(driver_orient) != 3: return # Calibrate orientation so looking straight ahead at the road (instead of at the device) reads # (0, 0), using live calibration. Makes the cone point in the correct direction. (stock PR #37149) sm = ui_state.sm if sm.valid['extrinsicsCalibration'] and len(sm['extrinsicsCalibration'].rpyCalib) == 3: cal_rpy = sm['extrinsicsCalibration'].rpyCalib else: cal_rpy = [0.0, 0.0, 0.0] _, pitch, yaw = face_orientation_from_net(driver_orient, driver_data.facePosition, cal_rpy) yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention pitch = self._pitch_filter.update(pitch) yaw = self._yaw_filter.update(yaw) # hysteresis on looking center if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER: self._looking_center = True elif abs(pitch) > LOOKING_CENTER_THRESHOLD_UPPER or abs(yaw) > LOOKING_CENTER_THRESHOLD_UPPER: self._looking_center = False self._looking_center_filter.update(1 if self._looking_center else 0) if DEBUG: pitchd = math.degrees(pitch) yawd = math.degrees(yaw) rl.draw_line_ex((0, 100), (200, 100), 3, rl.RED) rl.draw_line_ex((0, 120), (200, 120), 3, rl.RED) pitch_x = 100 + pitchd yaw_x = 100 + yawd rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN) rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN) # filter head rotation, handling wrap-around (bias pitch up since calib/DM pose isn't exact, # and halve yaw sensitivity) rotation = math.degrees(math.atan2((pitch + math.radians(6)) * 2, yaw)) angle_diff = rotation - self._rotation_filter.x angle_diff = ((angle_diff + 180) % 360) - 180 self._rotation_filter.update(self._rotation_filter.x + angle_diff) if not self.should_draw: self._fade_filter.update(0.0) elif not self.effective_active: self._fade_filter.update(0.35) else: self._fade_filter.update(1.0)