diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index 1f202141c..a47c04053 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/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 diff --git a/selfdrive/ui/sunnypilot/onroad/__init__.py b/selfdrive/ui/sunnypilot/onroad/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py b/selfdrive/ui/sunnypilot/onroad/developer_ui/__init__.py new file mode 100644 index 000000000..14a224ae7 --- /dev/null +++ b/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 diff --git a/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py b/selfdrive/ui/sunnypilot/onroad/developer_ui/elements.py new file mode 100644 index 000000000..e8daca886 --- /dev/null +++ b/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) diff --git a/selfdrive/ui/sunnypilot/onroad/hud_renderer.py b/selfdrive/ui/sunnypilot/onroad/hud_renderer.py new file mode 100644 index 000000000..33582df19 --- /dev/null +++ b/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) diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index af625ee61..8a0bc24ad 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/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")