[TIZI/TICI] ui: Developer Metrics (#1523)

* devui

* clean up

* clean up

* optimize text measurement for better rendering performance

* sp dir

* decouple from stock HudRenderer

* rename

* fetch mode in _update_state

* wrong type

* start decoupling elements

* decouple elements

* un-ew this pls

* fully decouple developer UI elements

* rename

* more decouple

* full send

* final

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

selfdrive/ui/onroad/augmented_road_view.py

@@ -14,6 +14,9 @@ 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
 
+if gui_app.sunnypilot_ui():
+  from openpilot.selfdrive.ui.sunnypilot.onroad.hud_renderer import HudRendererSP as HudRenderer
+
 OpState = log.SelfdriveState.OpenpilotState
 CALIBRATED = log.LiveCalibrationData.Status.calibrated
 ROAD_CAM = VisionStreamType.VISION_STREAM_ROAD

selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py

@@ -0,0 +1,164 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pyray as rl
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.selfdrive.ui.sunnypilot.onroad.developer_ui.elements import (
+  UiElement, RelDistElement, RelSpeedElement, SteeringAngleElement,
+  DesiredLateralAccelElement, ActualLateralAccelElement, DesiredSteeringAngleElement,
+  AEgoElement, LeadSpeedElement, FrictionCoefficientElement, LatAccelFactorElement,
+  SteeringTorqueEpsElement, BearingDegElement, AltitudeElement
+)
+from openpilot.system.ui.lib.application import gui_app, FontWeight
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+
+
+class DeveloperUiRenderer(Widget):
+  DEV_UI_OFF = 0
+  DEV_UI_RIGHT = 1
+  DEV_UI_BOTTOM = 2
+  DEV_UI_BOTH = 3
+
+  def __init__(self):
+    super().__init__()
+    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
+    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
+    self.dev_ui_mode = self.DEV_UI_OFF
+
+    self.rel_dist_elem = RelDistElement()
+    self.rel_speed_elem = RelSpeedElement()
+    self.steering_angle_elem = SteeringAngleElement()
+    self.desired_lat_accel_elem = DesiredLateralAccelElement()
+    self.actual_lat_accel_elem = ActualLateralAccelElement()
+    self.desired_steer_elem = DesiredSteeringAngleElement()
+    self.a_ego_elem = AEgoElement()
+    self.lead_speed_elem = LeadSpeedElement()
+    self.friction_elem = FrictionCoefficientElement()
+    self.lat_accel_factor_elem = LatAccelFactorElement()
+    self.steering_torque_elem = SteeringTorqueEpsElement()
+    self.bearing_elem = BearingDegElement()
+    self.altitude_elem = AltitudeElement()
+
+  def _update_state(self) -> None:
+    self.dev_ui_mode = ui_state.developer_ui
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    if self.dev_ui_mode == self.DEV_UI_OFF:
+      return
+
+    sm = ui_state.sm
+    if sm.recv_frame["carState"] < ui_state.started_frame:
+      return
+
+    if self.dev_ui_mode == self.DEV_UI_RIGHT:
+      self._draw_right_dev_ui(rect)
+    elif self.dev_ui_mode == self.DEV_UI_BOTTOM:
+      self._draw_bottom_dev_ui(rect)
+    elif self.dev_ui_mode == self.DEV_UI_BOTH:
+      self._draw_right_dev_ui(rect)
+      self._draw_bottom_dev_ui(rect)
+
+  def _draw_right_dev_ui(self, rect: rl.Rectangle) -> None:
+    sm = ui_state.sm
+    controls_state = sm['controlsState']
+
+    UI_BORDER_SIZE = 20
+    container_width = 184
+    x = int(rect.x + rect.width - container_width - UI_BORDER_SIZE * 2)
+    y = int(rect.y + UI_BORDER_SIZE * 1.5)
+
+    elements = [
+      self.rel_dist_elem.update(sm, ui_state.is_metric),
+      self.rel_speed_elem.update(sm, ui_state.is_metric),
+      self.steering_angle_elem.update(sm, ui_state.is_metric),
+    ]
+    if controls_state.lateralControlState.which() == 'torqueState':
+      elements.append(self.desired_lat_accel_elem.update(sm, ui_state.is_metric))
+      elements.append(self.actual_lat_accel_elem.update(sm, ui_state.is_metric))
+    else:
+      elements.append(self.desired_steer_elem.update(sm, ui_state.is_metric))
+
+    current_y = y
+    for element in elements:
+      current_y += self._draw_right_dev_ui_element(x, current_y, element)
+
+  def _draw_right_dev_ui_element(self, x: int, y: int, element: UiElement) -> int:
+    x += 0
+    y += 230
+    container_width = 184
+    label_size = 28
+    value_size = 60
+    unit_size = 28
+    label_width = measure_text_cached(self._font_bold, element.label, label_size, 0).x
+    centered_label_x = x + (container_width - label_width) / 2
+    rl.draw_text_ex(self._font_bold, element.label, rl.Vector2(centered_label_x, y), label_size, 0, rl.WHITE)
+
+    y += 45
+    value_width = measure_text_cached(self._font_bold, element.value, value_size, 0).x
+    centered_value_x = x + (container_width - value_width) / 2
+    rl.draw_text_ex(self._font_bold, element.value, rl.Vector2(centered_value_x, y), value_size, 0, element.color)
+
+    if element.unit:
+      units_height = measure_text_cached(self._font_bold, element.unit, unit_size, 0).x
+
+      units_x = x + container_width - 10
+      units_y = y + (value_size / 2) + (units_height / 2)
+
+      rl.draw_text_pro(self._font_bold, element.unit, rl.Vector2(units_x, units_y), rl.Vector2(0, 0), -90.0, unit_size, 0, rl.WHITE)
+
+    return 130
+
+  def _draw_bottom_dev_ui(self, rect: rl.Rectangle) -> None:
+    sm = ui_state.sm
+    bar_height = 61
+    y = int(rect.y + rect.height - bar_height)
+
+    rl.draw_rectangle(int(rect.x), y, int(rect.width), bar_height,
+                      rl.Color(0, 0, 0, 100))
+
+    elements = [
+      self.a_ego_elem.update(sm, ui_state.is_metric),
+      self.lead_speed_elem.update(sm, ui_state.is_metric),
+    ]
+
+    # Add torque-specific elements if using torque control
+    if sm['controlsState'].lateralControlState.which() == 'torqueState':
+      if sm.valid['liveTorqueParameters']:
+        elements.extend([
+          self.friction_elem.update(sm, ui_state.is_metric),
+          self.lat_accel_factor_elem.update(sm, ui_state.is_metric),
+        ])
+    else:
+      # Non-torque: show steering torque and GPS data
+      elements.append(self.steering_torque_elem.update(sm, ui_state.is_metric))
+
+      if sm.valid['gpsLocationExternal'] or sm.valid['gpsLocation']:
+        elements.append(self.bearing_elem.update(sm, ui_state.is_metric))
+
+    # Add altitude if GPS available
+    if sm.valid['gpsLocationExternal'] or sm.valid['gpsLocation']:
+      elements.append(self.altitude_elem.update(sm, ui_state.is_metric))
+
+    current_x = int(rect.x + 90)
+    center_y = y + bar_height // 2
+    for element in elements:
+      current_x += self._draw_bottom_dev_ui_element(current_x, center_y, element)
+
+  def _draw_bottom_dev_ui_element(self, x: int, y: int, element: UiElement) -> int:
+    font_size = 38
+
+    label_text = f"{element.label} "
+    label_width = measure_text_cached(self._font_bold, label_text, font_size, 0).x
+    rl.draw_text_ex(self._font_bold, label_text, rl.Vector2(x, y - font_size // 2), font_size, 0, rl.WHITE)
+
+    value_width = measure_text_cached(self._font_bold, element.value, font_size, 0).x
+    rl.draw_text_ex(self._font_bold, element.value, rl.Vector2(x + label_width + 10, y - font_size // 2), font_size, 0, element.color)
+
+    if element.unit:
+      rl.draw_text_ex(self._font_bold, element.unit, rl.Vector2(x + label_width + value_width + 20, y - font_size // 2), font_size, 0, rl.WHITE)
+
+    return 400

selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py

@@ -0,0 +1,303 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pyray as rl
+from dataclasses import dataclass
+
+from openpilot.common.constants import CV
+
+
+@dataclass
+class UiElement:
+  value: str
+  label: str
+  unit: str
+  color: rl.Color
+
+
+class LeadInfoElement:
+  @staticmethod
+  def get_lead_status(sm):
+    lead_one = sm['radarState'].leadOne
+    return lead_one.status, lead_one.dRel, lead_one.vRel
+
+  @staticmethod
+  def get_lead_color(lead_d_rel: float, lead_v_rel: float = 0.0, use_v_rel: bool = False) -> rl.Color:
+    if use_v_rel:
+      if lead_v_rel < -4.4704:
+        return rl.RED
+      elif lead_v_rel < 0:
+        return rl.Color(255, 188, 0, 255)  # Orange
+    else:
+      if lead_d_rel < 5:
+        return rl.RED
+      elif lead_d_rel < 15:
+        return rl.Color(255, 188, 0, 255)  # Orange
+    return rl.WHITE
+
+
+class LateralControlElement:
+  @staticmethod
+  def get_lat_color(lat_active: bool, steer_override: bool, angle_steers: float = 0.0,
+                    check_angle: bool = False) -> rl.Color:
+    color = rl.WHITE
+    if lat_active:
+      color = rl.Color(145, 155, 149, 255) if steer_override else rl.Color(0, 255, 0, 255)
+
+    if check_angle and lat_active:
+      if abs(angle_steers) > 180:
+        color = rl.RED
+      elif abs(angle_steers) > 90:
+        color = rl.Color(255, 188, 0, 255)
+      else:
+        # Keep green/grey from above
+        pass
+    elif check_angle and not lat_active:
+      if abs(angle_steers) > 180:
+        color = rl.RED
+      elif abs(angle_steers) > 90:
+        color = rl.Color(255, 188, 0, 255)
+
+    return color
+
+
+class RelDistElement(LeadInfoElement):
+  def __init__(self):
+    self.unit = "m"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    lead_status, lead_d_rel, _ = self.get_lead_status(sm)
+    value = f"{lead_d_rel:.0f}" if lead_status else "-"
+    color = self.get_lead_color(lead_d_rel) if lead_status else rl.WHITE
+    return UiElement(value, "REL DIST", self.unit, color)
+
+
+class RelSpeedElement(LeadInfoElement):
+  def __init__(self):
+    self.unit = "km/h"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    lead_status, _, lead_v_rel = self.get_lead_status(sm)
+
+    self.unit = "km/h" if is_metric else "mph"
+
+    conversion = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
+    value = f"{lead_v_rel * conversion:.0f}" if lead_status else "-"
+    color = self.get_lead_color(0, lead_v_rel, use_v_rel=True) if lead_status else rl.WHITE
+
+    return UiElement(value, "REL SPEED", self.unit, color)
+
+
+class SteeringAngleElement(LateralControlElement):
+  def __init__(self):
+    self.unit = ""
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    car_state = sm['carState']
+    angle_steers = car_state.steeringAngleDeg
+    lat_active = sm['carControl'].latActive
+    steer_override = car_state.steeringPressed
+
+    value = f"{angle_steers:.1f}°"
+    color = self.get_lat_color(lat_active, steer_override, angle_steers, check_angle=True)
+
+    return UiElement(value, "REAL STEER", self.unit, color)
+
+
+class DesiredSteeringAngleElement(LateralControlElement):
+  def __init__(self):
+    self.unit = ""
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    car_state = sm['carState']
+    controls_state = sm['controlsState']
+    lat_active = sm['carControl'].latActive
+    angle_steers = car_state.steeringAngleDeg
+    steer_angle_desired = controls_state.lateralControlState.angleState.steeringAngleDeg
+
+    value = f"{steer_angle_desired:.1f}°" if lat_active else "-"
+
+    color = rl.WHITE
+    if lat_active:
+      if abs(angle_steers) > 180:
+        color = rl.RED
+      elif abs(angle_steers) > 90:
+        color = rl.Color(255, 188, 0, 255)
+      else:
+        color = rl.Color(0, 255, 0, 255)
+
+    return UiElement(value, "DESIRED STEER", self.unit, color)
+
+
+class ActualLateralAccelElement(LateralControlElement):
+  def __init__(self):
+    self.unit = "m/s^2"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    controls_state = sm['controlsState']
+    curvature = controls_state.curvature
+    v_ego = sm['carState'].vEgo
+    roll = sm['liveParameters'].roll if sm.valid['liveParameters'] else 0.0
+    lat_active = sm['carControl'].latActive
+    steer_override = sm['carState'].steeringPressed
+
+    actual_lat_accel = (curvature * v_ego ** 2) - (roll * 9.81)
+    value = f"{actual_lat_accel:.2f}"
+    color = self.get_lat_color(lat_active, steer_override)
+
+    return UiElement(value, "ACTUAL L.A.", self.unit, color)
+
+
+class DesiredLateralAccelElement(LateralControlElement):
+  def __init__(self):
+    self.unit = "m/s^2"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    controls_state = sm['controlsState']
+    desired_curvature = controls_state.desiredCurvature
+    v_ego = sm['carState'].vEgo
+    roll = sm['liveParameters'].roll if sm.valid['liveParameters'] else 0.0
+    lat_active = sm['carControl'].latActive
+    steer_override = sm['carState'].steeringPressed
+
+    desired_lat_accel = (desired_curvature * v_ego ** 2) - (roll * 9.81)
+    value = f"{desired_lat_accel:.2f}" if lat_active else "-"
+    color = self.get_lat_color(lat_active, steer_override)
+
+    return UiElement(value, "DESIRED L.A.", self.unit, color)
+
+
+class AEgoElement:
+  def __init__(self):
+    self.unit = "m/s^2"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    a_ego = sm['carState'].aEgo
+    value = f"{a_ego:.1f}"
+    return UiElement(value, "ACC.", self.unit, rl.WHITE)
+
+
+class LeadSpeedElement(LeadInfoElement):
+  def __init__(self):
+    self.unit = "km/h"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    lead_status, _, lead_v_rel = self.get_lead_status(sm)
+    v_ego = sm['carState'].vEgo
+
+    self.unit = "km/h" if is_metric else "mph"
+
+    conversion = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
+    value = f"{(lead_v_rel + v_ego) * conversion:.0f}" if lead_status else "-"
+    color = self.get_lead_color(0, lead_v_rel, use_v_rel=True) if lead_status else rl.WHITE
+
+    return UiElement(value, "L.S.", self.unit, color)
+
+
+class FrictionCoefficientElement:
+  def __init__(self):
+    self.unit = ""
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    ltp = sm['liveTorqueParameters']
+    friction_coef = ltp.frictionCoefficientFiltered
+    live_valid = ltp.liveValid
+
+    value = f"{friction_coef:.3f}"
+    color = rl.Color(0, 255, 0, 255) if live_valid else rl.WHITE
+    return UiElement(value, "FRIC.", self.unit, color)
+
+
+class LatAccelFactorElement:
+  def __init__(self):
+    self.unit = ""
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    ltp = sm['liveTorqueParameters']
+    lat_accel_factor = ltp.latAccelFactorFiltered
+    live_valid = ltp.liveValid
+
+    value = f"{lat_accel_factor:.3f}"
+    color = rl.Color(0, 255, 0, 255) if live_valid else rl.WHITE
+    return UiElement(value, "L.A.F.", self.unit, color)
+
+
+class SteeringTorqueEpsElement:
+  def __init__(self):
+    self.unit = "N·dm"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    steering_torque_eps = sm['carState'].steeringTorqueEps
+    value = f"{abs(steering_torque_eps):.1f}"
+    return UiElement(value, "E.T.", self.unit, rl.WHITE)
+
+
+class GpsInfoElement:
+  @staticmethod
+  def get_gps_data(sm):
+    if sm.valid['gpsLocationExternal']:
+      return sm['gpsLocationExternal'], True
+    elif sm.valid['gpsLocation']:
+      return sm['gpsLocation'], True
+    return None, False
+
+
+class BearingDegElement(GpsInfoElement):
+  def __init__(self):
+    self.unit = ""
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    gps_data, valid = self.get_gps_data(sm)
+    if not valid:
+      return UiElement("OFF | -", "B.D.", self.unit, rl.WHITE)
+
+    bearing_accuracy_deg = gps_data.bearingAccuracyDeg
+    bearing_deg = gps_data.bearingDeg
+
+    if bearing_accuracy_deg != 180.0:
+      value = f"{bearing_deg:.0f}°"
+      if (337.5 <= bearing_deg <= 360) or (0 <= bearing_deg <= 22.5):
+        dir_value = "N"
+      elif 22.5 < bearing_deg < 67.5:
+        dir_value = "NE"
+      elif 67.5 <= bearing_deg <= 112.5:
+        dir_value = "E"
+      elif 112.5 < bearing_deg < 157.5:
+        dir_value = "SE"
+      elif 157.5 <= bearing_deg <= 202.5:
+        dir_value = "S"
+      elif 202.5 < bearing_deg < 247.5:
+        dir_value = "SW"
+      elif 247.5 <= bearing_deg <= 292.5:
+        dir_value = "W"
+      else:  # 292.5 < bearing_deg < 337.5
+        dir_value = "NW"
+    else:
+      value = "-"
+      dir_value = "OFF"
+
+    return UiElement(f"{dir_value} | {value}", "B.D.", self.unit, rl.WHITE)
+
+
+class AltitudeElement(GpsInfoElement):
+  def __init__(self):
+    self.unit = "m"
+
+  def update(self, sm, is_metric: bool) -> UiElement:
+    gps_data, valid = self.get_gps_data(sm)
+
+    gps_accuracy = 0.0
+    altitude = 0.0
+
+    if valid:
+      altitude = gps_data.altitude
+      if sm.valid['gpsLocationExternal']:
+        gps_accuracy = gps_data.horizontalAccuracy
+      else:
+        gps_accuracy = 1.0  # Simulate valid for legacy check
+
+    value = f"{altitude:.1f}" if gps_accuracy != 0.0 else "-"
+    return UiElement(value, "ALT.", self.unit, rl.WHITE)

selfdrive/ui/sunnypilot/onroad/hud_renderer.py

@@ -0,0 +1,20 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pyray as rl
+
+from openpilot.selfdrive.ui.onroad.hud_renderer import HudRenderer
+from openpilot.selfdrive.ui.sunnypilot.onroad.developer_ui import DeveloperUiRenderer
+
+
+class HudRendererSP(HudRenderer):
+  def __init__(self):
+    super().__init__()
+    self.developer_ui = DeveloperUiRenderer()
+
+  def _render(self, rect: rl.Rectangle) -> None:
+    super()._render(rect)
+    self.developer_ui.render(rect)

selfdrive/ui/sunnypilot/ui_state.py

@@ -27,3 +27,4 @@ class UIStateSP:
     if CP_SP_bytes is not None:
       self.CP_SP = messaging.log_from_bytes(CP_SP_bytes, custom.CarParamsSP)
     self.sunnylink_enabled = self.params.get_bool("SunnylinkEnabled")
+    self.developer_ui = self.params.get("DevUIInfo")