IQ.Pilot Release Commit @ bec7652

iqpilot/ui/onroad/driver_state.py

@@ -0,0 +1,159 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import time
+import numpy as np
+import pyray as rl
+
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.ui import UI_BORDER_SIZE
+from iqpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer, BTN_SIZE, ARC_LENGTH
+from iqpilot.ui.onroad.hud_overlays import IQDevMetricsOverlay
+from iqpilot.system.ui.lib.application import gui_app, FontWeight
+from iqpilot.system.ui.lib.text_measure import measure_text_cached
+
+_PERSONALITY_RELAXED = 0
+_PERSONALITY_STANDARD = 1
+_PERSONALITY_AGGRESSIVE = 2
+
+PERSONALITY_COLORS = {
+  _PERSONALITY_RELAXED:    rl.Color(0x17, 0xC9, 0x64, 0xFF),
+  _PERSONALITY_STANDARD:   rl.Color(0x0C, 0x94, 0x96, 0xFF),
+  _PERSONALITY_AGGRESSIVE: rl.Color(0xE8, 0x2C, 0x2C, 0xFF),
+}
+
+PERSONALITY_NAMES = {
+  _PERSONALITY_RELAXED:    "Relaxed",
+  _PERSONALITY_STANDARD:   "Standard",
+  _PERSONALITY_AGGRESSIVE: "Aggressive",
+}
+
+_TOAST_DURATION = 2.0
+_TOAST_FONT_SIZE = 52
+_TOAST_PAD_X = 52
+_TOAST_PAD_Y = 22
+_TOAST_BOTTOM_MARGIN = UI_BORDER_SIZE + 36
+_TOAST_RADIUS = 0.45
+_TOAST_FADE = 0.25
+
+class DriverStateRendererIQ(DriverStateRenderer):
+  def __init__(self):
+    super().__init__()
+    self._params = Params()
+    self._personality: int = _PERSONALITY_STANDARD
+    self._personality_color: rl.Color = PERSONALITY_COLORS[_PERSONALITY_STANDARD]
+
+    self._toast_end_time: float = 0.0
+    self._toast_text: str = ""
+    self._toast_color: rl.Color = rl.WHITE
+    self._font = gui_app.font(FontWeight.SEMI_BOLD)
+
+    self.dev_ui_offset = IQDevMetricsOverlay.get_bottom_dev_ui_offset()
+    self._dm_background = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_background.png", BTN_SIZE, BTN_SIZE)
+    self._dm_person = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_person.png", 118, 118)
+    self._dm_cone = gui_app.texture("icons_mici/onroad/driver_monitoring/dm_cone.png", 118, 118)
+
+  def _update_state(self):
+    super()._update_state()
+    personality = self._params.get("LongitudinalPersonality", return_default=True)
+    if personality is not None:
+      self._personality = int(personality)
+    self._personality_color = PERSONALITY_COLORS.get(self._personality, PERSONALITY_COLORS[_PERSONALITY_STANDARD])
+
+  def cycle_personality(self):
+    next_p = (self._personality + 1) % 3
+    self._params.put_nonblocking("LongitudinalPersonality", next_p)
+    self._personality = next_p
+    self._personality_color = PERSONALITY_COLORS[next_p]
+    self._toast_text = PERSONALITY_NAMES[next_p]
+    self._toast_color = PERSONALITY_COLORS[next_p]
+    self._toast_end_time = time.monotonic() + _TOAST_DURATION
+
+  def _render(self, _):
+    fade = max(0.35, 1.0 - self.dm_fade_state)
+    alpha = int(255 * fade)
+    pc = self._personality_color
+
+    rl.draw_texture(
+      self._dm_background,
+      int(self.position_x - self._dm_background.width / 2),
+      int(self.position_y - self._dm_background.height / 2),
+      rl.Color(pc.r, pc.g, pc.b, alpha),
+    )
+
+    rl.draw_texture(
+      self._dm_person,
+      int(self.position_x - self._dm_person.width / 2),
+      int(self.position_y - self._dm_person.height / 2),
+      rl.Color(255, 255, 255, int(alpha * 0.9)),
+    )
+
+    if self.is_active:
+      dest_rect = rl.Rectangle(self.position_x, self.position_y, self._dm_cone.width, self._dm_cone.height)
+      rl.draw_texture_pro(
+        self._dm_cone,
+        rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height),
+        dest_rect,
+        rl.Vector2(dest_rect.width / 2, dest_rect.height / 2),
+        180.0,
+        rl.Color(pc.r, pc.g, pc.b, alpha),
+      )
+    else:
+      rl.draw_circle(int(self.position_x), int(self.position_y), 14, rl.Color(255, 255, 255, alpha))
+
+    self._draw_personality_toast()
+
+  def _draw_personality_toast(self):
+    now = time.monotonic()
+    remaining = self._toast_end_time - now
+    if remaining <= 0 or not self._toast_text:
+      return
+
+    elapsed = _TOAST_DURATION - remaining
+    fade_in = min(1.0, elapsed / _TOAST_FADE)
+    fade_out = min(1.0, remaining / _TOAST_FADE)
+    a = int(255 * fade_in * fade_out)
+
+    text_size = measure_text_cached(self._font, self._toast_text, _TOAST_FONT_SIZE)
+    toast_w = text_size.x + _TOAST_PAD_X * 2
+    toast_h = text_size.y + _TOAST_PAD_Y * 2
+
+    cx = self._rect.x + self._rect.width / 2
+    toast_x = cx - toast_w / 2
+    toast_y = self._rect.y + self._rect.height - _TOAST_BOTTOM_MARGIN - toast_h
+
+    tc = self._toast_color
+    toast_rect = rl.Rectangle(toast_x, toast_y, toast_w, toast_h)
+    rl.draw_rectangle_rounded(toast_rect, _TOAST_RADIUS, 10, rl.Color(tc.r, tc.g, tc.b, a))
+    rl.draw_text_ex(
+      self._font, self._toast_text,
+      rl.Vector2(toast_x + _TOAST_PAD_X, toast_y + _TOAST_PAD_Y),
+      _TOAST_FONT_SIZE, 0,
+      rl.Color(255, 255, 255, a),
+    )
+
+  def _pre_calculate_drawing_elements(self):
+    width, height = self._rect.width, self._rect.height
+    offset = UI_BORDER_SIZE + BTN_SIZE // 2
+    self.position_x = self._rect.x + (width - offset if self.is_rhd else offset)
+    self.position_y = self._rect.y + height - offset - self.dev_ui_offset
+
+    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]
+
+    delta_x = -self.driver_pose_sins[1] * ARC_LENGTH / 2.0
+    delta_y = -self.driver_pose_sins[0] * ARC_LENGTH / 2.0
+
+    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
+    )
+
+    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
+    )