sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
Whitespace-only changes.
@@ -0,0 +1,103 @@
|
||||
"""
|
||||
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.alert_renderer import AlertRenderer, AlertSize, ALERT_FONT_MEDIUM, ALERT_FONT_BIG, \
|
||||
ALERT_FONT_SMALL, ALERT_MARGIN, ALERT_HEIGHTS, ALERT_PADDING, Alert
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.lib.wrap_text import wrap_text
|
||||
|
||||
ALERT_LINE_SPACING = 15
|
||||
|
||||
|
||||
class AlertRendererSP(AlertRenderer):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
|
||||
def _draw_text(self, rect: rl.Rectangle, alert: Alert) -> None:
|
||||
if alert.size == AlertSize.small:
|
||||
self._draw_multiline_centered(alert.text1, rect, self.font_bold, ALERT_FONT_MEDIUM)
|
||||
|
||||
elif alert.size == AlertSize.mid:
|
||||
wrap_width = int(rect.width)
|
||||
lines1 = wrap_text(self.font_bold, alert.text1, ALERT_FONT_BIG, wrap_width)
|
||||
lines2 = wrap_text(self.font_regular, alert.text2, ALERT_FONT_SMALL, wrap_width) if alert.text2 else []
|
||||
|
||||
total_text_height = len(lines1) * measure_text_cached(self.font_bold, "A", ALERT_FONT_BIG).y
|
||||
if lines2:
|
||||
total_text_height += ALERT_LINE_SPACING + len(lines2) * measure_text_cached(self.font_regular, "A", ALERT_FONT_SMALL).y
|
||||
|
||||
curr_y = rect.y + (rect.height - total_text_height) / 2
|
||||
|
||||
for line in lines1:
|
||||
line_height = measure_text_cached(self.font_bold, alert.text1, ALERT_FONT_BIG).y
|
||||
self._draw_line_centered(line, rl.Rectangle(rect.x, curr_y, rect.width, line_height), self.font_bold, ALERT_FONT_BIG)
|
||||
curr_y += line_height
|
||||
|
||||
if lines2:
|
||||
curr_y += ALERT_LINE_SPACING
|
||||
for line in lines2:
|
||||
line_height = measure_text_cached(self.font_regular, alert.text2, ALERT_FONT_SMALL).y
|
||||
self._draw_line_centered(line, rl.Rectangle(rect.x, curr_y, rect.width, line_height), self.font_regular, ALERT_FONT_SMALL)
|
||||
curr_y += line_height
|
||||
|
||||
else:
|
||||
super()._draw_text(rect, alert)
|
||||
|
||||
def _draw_multiline_centered(self, text, rect, font, font_size, color=rl.WHITE) -> None:
|
||||
lines = wrap_text(font, text, font_size, rect.width)
|
||||
line_height = measure_text_cached(font, text, font_size).y
|
||||
total_height = len(lines) * line_height
|
||||
curr_y = rect.y + (rect.height - total_height) / 2
|
||||
for line in lines:
|
||||
self._draw_line_centered(line, rl.Rectangle(rect.x, curr_y, rect.width, line_height), font, font_size, color)
|
||||
curr_y += line_height
|
||||
|
||||
def _draw_line_centered(self, text, rect, font, font_size, color=rl.WHITE) -> None:
|
||||
text_size = measure_text_cached(font, text, font_size)
|
||||
x = rect.x + (rect.width - text_size.x) / 2
|
||||
y = rect.y
|
||||
rl.draw_text_ex(font, text, rl.Vector2(x, y), font_size, 0, color)
|
||||
|
||||
def _get_alert_rect(self, rect: rl.Rectangle, size: int) -> rl.Rectangle:
|
||||
if size == AlertSize.full:
|
||||
return rect
|
||||
|
||||
dev_ui_info = ui_state.developer_ui
|
||||
v_adjustment = 40 if dev_ui_info in {2, 3} and size != AlertSize.full else 0
|
||||
h_adjustment = 230 if dev_ui_info in {1, 3} and size != AlertSize.full else 0
|
||||
|
||||
w = int(rect.width - ALERT_MARGIN * 2 - h_adjustment)
|
||||
h = self._calculate_dynamic_height(size, w)
|
||||
return rl.Rectangle(rect.x + ALERT_MARGIN, rect.y + rect.height - h + ALERT_MARGIN - v_adjustment, w,
|
||||
h - ALERT_MARGIN * 2)
|
||||
|
||||
def _calculate_dynamic_height(self, size: int, width: int) -> int:
|
||||
alert = self.get_alert(ui_state.sm)
|
||||
if not alert:
|
||||
return ALERT_HEIGHTS.get(size, 271)
|
||||
|
||||
height = 2 * ALERT_PADDING
|
||||
wrap_width = width - 2 * ALERT_PADDING
|
||||
|
||||
if size == AlertSize.small:
|
||||
lines = wrap_text(self.font_bold, alert.text1, ALERT_FONT_MEDIUM, wrap_width)
|
||||
line_height = measure_text_cached(self.font_bold, alert.text1, ALERT_FONT_MEDIUM).y
|
||||
height += int(len(lines) * line_height)
|
||||
elif size == AlertSize.mid:
|
||||
lines1 = wrap_text(self.font_bold, alert.text1, ALERT_FONT_BIG, wrap_width)
|
||||
line_height1 = measure_text_cached(self.font_bold, alert.text1, ALERT_FONT_BIG).y
|
||||
height += int(len(lines1) * line_height1)
|
||||
|
||||
if alert.text2:
|
||||
lines2 = wrap_text(self.font_regular, alert.text2, ALERT_FONT_SMALL, wrap_width)
|
||||
line_height2 = measure_text_cached(self.font_regular, alert.text2, ALERT_FONT_SMALL).y
|
||||
height += int(ALERT_LINE_SPACING + len(lines2) * line_height2)
|
||||
else:
|
||||
height = ALERT_HEIGHTS.get(size, 271)
|
||||
|
||||
return int(height)
|
||||
@@ -0,0 +1,31 @@
|
||||
"""
|
||||
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.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.ui.ui_state import UIStatus, ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
|
||||
BORDER_COLORS_SP = {
|
||||
UIStatus.LAT_ONLY: rl.Color(0x00, 0xC8, 0xC8, 0xFF), # Cyan for lateral-only state
|
||||
UIStatus.LONG_ONLY: rl.Color(0x96, 0x1C, 0xA8, 0xFF), # Purple for longitudinal-only state
|
||||
}
|
||||
|
||||
|
||||
class AugmentedRoadViewSP:
|
||||
def __init__(self):
|
||||
self._fade_texture = gui_app.texture("icons_mici/onroad/onroad_fade.png")
|
||||
self._fade_alpha_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps)
|
||||
|
||||
def update_fade_out_bottom_overlay(self, _content_rect):
|
||||
# Fade out bottom of overlays for looks (only when engaged)
|
||||
fade_alpha = self._fade_alpha_filter.update(ui_state.status != UIStatus.DISENGAGED)
|
||||
if ui_state.torque_bar and fade_alpha > 1e-2:
|
||||
# Scale the fade texture to the content rect
|
||||
rl.draw_texture_pro(self._fade_texture,
|
||||
rl.Rectangle(0, 0, self._fade_texture.width, self._fade_texture.height),
|
||||
_content_rect, rl.Vector2(0, 0), 0.0,
|
||||
rl.Color(255, 255, 255, int(255 * fade_alpha)))
|
||||
@@ -0,0 +1,52 @@
|
||||
"""
|
||||
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.system.ui.lib.application import gui_app
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
|
||||
|
||||
class BlindSpotIndicators:
|
||||
def __init__(self):
|
||||
self._txt_blind_spot_left: rl.Texture = gui_app.texture('icons_mici/onroad/blind_spot_left.png', 108, 128)
|
||||
self._txt_blind_spot_right: rl.Texture = gui_app.texture('icons_mici/onroad/blind_spot_left.png', 108, 128, flip_x=True)
|
||||
|
||||
self._blind_spot_left_alpha_filter = FirstOrderFilter(0, 0.15, 1 / gui_app.target_fps)
|
||||
self._blind_spot_right_alpha_filter = FirstOrderFilter(0, 0.15, 1 / gui_app.target_fps)
|
||||
|
||||
def update(self) -> None:
|
||||
sm = ui_state.sm
|
||||
CS = sm['carState']
|
||||
|
||||
self._blind_spot_left_alpha_filter.update(1.0 if CS.leftBlindspot else 0.0)
|
||||
self._blind_spot_right_alpha_filter.update(1.0 if CS.rightBlindspot else 0.0)
|
||||
|
||||
@property
|
||||
def detected(self) -> bool:
|
||||
return ui_state.blindspot and (self._blind_spot_left_alpha_filter.x > 0.01 or self._blind_spot_right_alpha_filter.x > 0.01)
|
||||
|
||||
def render(self, rect: rl.Rectangle) -> None:
|
||||
if not ui_state.blindspot:
|
||||
return
|
||||
|
||||
BLIND_SPOT_MARGIN_X = 20 # Distance from edge of screen
|
||||
BLIND_SPOT_Y_OFFSET = 100 # Distance from top of screen
|
||||
|
||||
if self._blind_spot_left_alpha_filter.x > 0.01:
|
||||
pos_x = int(rect.x + BLIND_SPOT_MARGIN_X)
|
||||
pos_y = int(rect.y + BLIND_SPOT_Y_OFFSET)
|
||||
alpha = int(255 * self._blind_spot_left_alpha_filter.x)
|
||||
color = rl.Color(255, 255, 255, alpha)
|
||||
rl.draw_texture_ex(self._txt_blind_spot_left, rl.Vector2(pos_x, pos_y), 0.0, 1.0, color)
|
||||
|
||||
if self._blind_spot_right_alpha_filter.x > 0.01:
|
||||
pos_x = int(rect.x + rect.width - BLIND_SPOT_MARGIN_X - self._txt_blind_spot_right.width)
|
||||
pos_y = int(rect.y + BLIND_SPOT_Y_OFFSET)
|
||||
alpha = int(255 * self._blind_spot_right_alpha_filter.x)
|
||||
color = rl.Color(255, 255, 255, alpha)
|
||||
rl.draw_texture_ex(self._txt_blind_spot_right, rl.Vector2(pos_x, pos_y), 0.0, 1.0, color)
|
||||
@@ -0,0 +1,147 @@
|
||||
"""
|
||||
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 numpy as np
|
||||
|
||||
import pyray as rl
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
|
||||
|
||||
class ChevronOptions:
|
||||
OFF = 0
|
||||
DISTANCE_ONLY = 1
|
||||
SPEED_ONLY = 2
|
||||
TTC_ONLY = 3
|
||||
ALL = 4
|
||||
|
||||
|
||||
class ChevronMetrics:
|
||||
def __init__(self):
|
||||
self._lead_status_alpha: float = 0.0
|
||||
self._font = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
|
||||
def update_alpha(self, has_lead: bool):
|
||||
"""Update the alpha value for fade in/out animation"""
|
||||
if not has_lead:
|
||||
self._lead_status_alpha = max(0.0, self._lead_status_alpha - 0.05)
|
||||
else:
|
||||
self._lead_status_alpha = min(1.0, self._lead_status_alpha + 0.1)
|
||||
|
||||
def should_render(self) -> bool:
|
||||
"""Check if dev UI should be rendered"""
|
||||
return ui_state.chevron_metrics != ChevronOptions.OFF and self._lead_status_alpha > 0.0
|
||||
|
||||
def _draw_lead(self, lead_data, lead_vehicle, v_ego: float, rect: rl.Rectangle):
|
||||
"""Draw lead vehicle status information (distance, speed, TTC)"""
|
||||
if not self.should_render():
|
||||
return
|
||||
|
||||
d_rel = lead_data.dRel
|
||||
v_rel = lead_data.vRel
|
||||
|
||||
if not lead_vehicle.chevron or len(lead_vehicle.chevron) < 2:
|
||||
return
|
||||
|
||||
chevron_x = lead_vehicle.chevron[1][0]
|
||||
chevron_y = lead_vehicle.chevron[1][1]
|
||||
sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 2.35
|
||||
|
||||
text_lines = self._build_text_lines(d_rel, v_rel, v_ego)
|
||||
if not text_lines:
|
||||
return
|
||||
|
||||
self._render_text_lines(text_lines, chevron_x, chevron_y, sz, rect)
|
||||
|
||||
@staticmethod
|
||||
def _build_text_lines(d_rel: float, v_rel: float, v_ego: float) -> list[str]:
|
||||
"""Build text lines based on chevron info setting"""
|
||||
text_lines = []
|
||||
|
||||
# Distance
|
||||
if ui_state.chevron_metrics == ChevronOptions.DISTANCE_ONLY or ui_state.chevron_metrics == ChevronOptions.ALL:
|
||||
val = max(0.0, d_rel)
|
||||
unit = "m" if ui_state.is_metric else "ft"
|
||||
if not ui_state.is_metric:
|
||||
val *= 3.28084
|
||||
text_lines.append(f"{val:.0f} {unit}")
|
||||
|
||||
# Speed
|
||||
if ui_state.chevron_metrics == ChevronOptions.SPEED_ONLY or ui_state.chevron_metrics == ChevronOptions.ALL:
|
||||
multiplier = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
val = max(0.0, (v_rel + v_ego) * multiplier)
|
||||
unit = "km/h" if ui_state.is_metric else "mph"
|
||||
text_lines.append(f"{val:.0f} {unit}")
|
||||
|
||||
# Time to collision
|
||||
if ui_state.chevron_metrics == ChevronOptions.TTC_ONLY or ui_state.chevron_metrics == ChevronOptions.ALL:
|
||||
val = (d_rel / v_ego) if (d_rel > 0 and v_ego > 0) else 0.0
|
||||
ttc_text = f"{val:.1f} s" if (0 < val < 200) else "---"
|
||||
text_lines.append(ttc_text)
|
||||
|
||||
return text_lines
|
||||
|
||||
def _render_text_lines(self, text_lines: list[str], chevron_x: float, chevron_y: float,
|
||||
sz: float, rect: rl.Rectangle):
|
||||
"""Render text lines with proper centering and positioning"""
|
||||
font_size = 40
|
||||
line_height = 50
|
||||
margin = 20
|
||||
|
||||
text_y = chevron_y + sz + 15
|
||||
total_height = len(text_lines) * line_height
|
||||
|
||||
# Adjust Y position if text would go off screen
|
||||
if text_y + total_height > rect.height - margin:
|
||||
y_max = min(chevron_y, rect.height - margin)
|
||||
text_y = y_max - 15 - total_height
|
||||
text_y = max(margin, text_y)
|
||||
|
||||
alpha = int(255 * self._lead_status_alpha)
|
||||
text_color = rl.Color(255, 255, 255, alpha)
|
||||
shadow_color = rl.Color(0, 0, 0, int(200 * self._lead_status_alpha))
|
||||
|
||||
for i, line in enumerate(text_lines):
|
||||
y = int(text_y + (i * line_height))
|
||||
if y + line_height > rect.height - margin:
|
||||
break
|
||||
|
||||
# Measure actual text width for proper centering
|
||||
text_size = measure_text_cached(self._font, line, font_size, 0)
|
||||
text_width = text_size.x
|
||||
|
||||
# Center the text horizontally on the chevron
|
||||
x = int(chevron_x - text_width / 2)
|
||||
x = int(np.clip(x, margin, rect.width - text_width - margin))
|
||||
|
||||
# Draw shadow
|
||||
rl.draw_text_ex(self._font, line, rl.Vector2(x + 2, y + 2), font_size, 0, shadow_color)
|
||||
# Draw text
|
||||
rl.draw_text_ex(self._font, line, rl.Vector2(x, y), font_size, 0, text_color)
|
||||
|
||||
def draw_lead_status(self, sm, radar_state, rect, lead_vehicles):
|
||||
lead_one = radar_state.leadOne
|
||||
lead_two = radar_state.leadTwo
|
||||
|
||||
has_lead_one = lead_one.present if lead_one else False
|
||||
has_lead_two = lead_two.present if lead_two else False
|
||||
|
||||
self.update_alpha(has_lead_one or has_lead_two)
|
||||
|
||||
if not self.should_render():
|
||||
return
|
||||
|
||||
v_ego = sm['carState'].vEgo
|
||||
|
||||
if has_lead_one and lead_vehicles[0].chevron:
|
||||
self._draw_lead(lead_one, lead_vehicles[0], v_ego, rect)
|
||||
|
||||
if has_lead_two and lead_vehicles[1].chevron:
|
||||
d_rel_diff = abs(lead_one.dRel - lead_two.dRel) if has_lead_one else float('inf')
|
||||
if d_rel_diff > 3.0:
|
||||
self._draw_lead(lead_two, lead_vehicles[1], v_ego, rect)
|
||||
@@ -0,0 +1,140 @@
|
||||
"""
|
||||
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.cereal import log
|
||||
from openpilot.selfdrive.ui import UI_BORDER_SIZE
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.developer_ui import DeveloperUiState
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
|
||||
|
||||
class CircularAlertsRenderer:
|
||||
def __init__(self):
|
||||
self._green_light_alert_img = gui_app.texture("../../sunnypilot/selfdrive/assets/images/green_light.png", 250, 250)
|
||||
self._lead_depart_alert_img = gui_app.texture("../../sunnypilot/selfdrive/assets/images/lead_depart.png", 250, 250)
|
||||
|
||||
self._e2e_alert_display_timer = 0
|
||||
self._e2e_alert_frame = 0
|
||||
self._green_light_alert = False
|
||||
self._lead_depart_alert = False
|
||||
self._standstill_elapsed_time = 0.0
|
||||
self._is_standstill = False
|
||||
self._alert_text = ""
|
||||
self._alert_img = None
|
||||
self._allow_e2e_alerts = False
|
||||
|
||||
def update(self) -> None:
|
||||
sm = ui_state.sm
|
||||
lp_sp = sm['longitudinalPlanSP']
|
||||
car_state = sm['carState']
|
||||
self._green_light_alert = lp_sp.e2eAlerts.greenLightAlert
|
||||
self._lead_depart_alert = lp_sp.e2eAlerts.leadDepartAlert
|
||||
self._is_standstill = car_state.standstill
|
||||
|
||||
if not ui_state.started:
|
||||
self._standstill_elapsed_time = 0.0
|
||||
|
||||
self._allow_e2e_alerts = sm['selfdriveState'].alertSize == log.SelfdriveState.AlertSize.none and \
|
||||
sm.recv_frame['driverStateV2'] > ui_state.started_frame
|
||||
|
||||
if self._green_light_alert or self._lead_depart_alert:
|
||||
self._e2e_alert_display_timer = 3 * gui_app.target_fps
|
||||
# reset onroad sleep timer for e2e alerts
|
||||
ui_state.reset_onroad_sleep_timer()
|
||||
|
||||
if self._e2e_alert_display_timer > 0:
|
||||
self._e2e_alert_frame += 1
|
||||
self._e2e_alert_display_timer -= 1
|
||||
|
||||
if self._green_light_alert:
|
||||
self._alert_text = "GREEN\nLIGHT"
|
||||
self._alert_img = self._green_light_alert_img
|
||||
elif self._lead_depart_alert:
|
||||
self._alert_text = "LEAD VEHICLE\nDEPARTING"
|
||||
self._alert_img = self._lead_depart_alert_img
|
||||
|
||||
elif ui_state.standstill_timer and self._is_standstill:
|
||||
self._alert_img = None
|
||||
self._standstill_elapsed_time += 1.0 / gui_app.target_fps
|
||||
minute = int(self._standstill_elapsed_time / 60)
|
||||
second = int(self._standstill_elapsed_time - (minute * 60))
|
||||
self._alert_text = f"{minute:01d}:{second:02d}"
|
||||
self._e2e_alert_frame += 1
|
||||
|
||||
else:
|
||||
self._e2e_alert_frame = 0
|
||||
if not self._is_standstill:
|
||||
self._standstill_elapsed_time = 0.0
|
||||
|
||||
def render(self, rect: rl.Rectangle) -> None:
|
||||
if not self._allow_e2e_alerts or (self._e2e_alert_display_timer <= 0 and not (ui_state.standstill_timer and self._is_standstill)):
|
||||
return
|
||||
|
||||
e2e_alert_size = 250
|
||||
dev_ui_width_adjustment = 180 if ui_state.developer_ui in (DeveloperUiState.RIGHT, DeveloperUiState.BOTH) else 100
|
||||
|
||||
x = rect.x + rect.width - e2e_alert_size - dev_ui_width_adjustment - (UI_BORDER_SIZE * 3)
|
||||
y = rect.y + rect.height / 2 + 20
|
||||
|
||||
alert_rect = rl.Rectangle(x - e2e_alert_size, y - e2e_alert_size, e2e_alert_size * 2, e2e_alert_size * 2)
|
||||
center = rl.Vector2(alert_rect.x + alert_rect.width / 2, alert_rect.y + alert_rect.height / 2)
|
||||
|
||||
# Pulse logic
|
||||
is_pulsing = (self._e2e_alert_frame % gui_app.target_fps) < (gui_app.target_fps / 2.5)
|
||||
|
||||
# Standstill Timer (STOPPED) should be static white
|
||||
if self._e2e_alert_display_timer == 0 and ui_state.standstill_timer and self._is_standstill:
|
||||
frame_color = rl.Color(255, 255, 255, 75)
|
||||
else:
|
||||
frame_color = rl.Color(255, 255, 255, 75) if is_pulsing else rl.Color(0, 255, 0, 75)
|
||||
|
||||
# Draw Circle
|
||||
rl.draw_circle_v(center, e2e_alert_size, rl.Color(0, 0, 0, 190))
|
||||
# Draw Ring (Border)
|
||||
rl.draw_ring(center, e2e_alert_size - 7.5, e2e_alert_size + 7.5, 0, 360, 0, frame_color)
|
||||
|
||||
# Draw Image
|
||||
if self._alert_img and self._e2e_alert_display_timer > 0:
|
||||
img_x = center.x - self._alert_img.width / 2
|
||||
img_y = center.y - self._alert_img.height / 2
|
||||
rl.draw_texture_ex(self._alert_img, rl.Vector2(img_x, img_y), 0.0, 1.0, rl.WHITE)
|
||||
|
||||
# Draw Text
|
||||
txt_color = rl.Color(255, 255, 255, 255) if is_pulsing else rl.Color(0, 255, 0, 190)
|
||||
font = gui_app.font(FontWeight.BOLD)
|
||||
text_size = 48
|
||||
spacing = 0
|
||||
|
||||
lines = self._alert_text.split('\n')
|
||||
|
||||
# Position text at bottom of alert circle
|
||||
bottom_y = (alert_rect.y + alert_rect.height) - (alert_rect.height / 7)
|
||||
|
||||
# Draw lines upwards from bottom
|
||||
current_y = bottom_y - (len(lines) * text_size * FONT_SCALE)
|
||||
|
||||
if self._e2e_alert_display_timer == 0 and ui_state.standstill_timer and self._is_standstill:
|
||||
# Standstill Timer Text
|
||||
alert_alt_text = "STOPPED"
|
||||
top_text_size = 80
|
||||
measure_top = measure_text_cached(font, alert_alt_text, top_text_size, spacing)
|
||||
top_y = alert_rect.y + alert_rect.height / 3.5
|
||||
rl.draw_text_ex(font, alert_alt_text, rl.Vector2(center.x - measure_top.x / 2, top_y), top_text_size, spacing, rl.Color(255, 175, 3, 240))
|
||||
|
||||
# Timer
|
||||
timer_text_size = 100
|
||||
measure_timer = measure_text_cached(font, self._alert_text, timer_text_size, spacing)
|
||||
timer_y = (alert_rect.y + alert_rect.height) - (alert_rect.height / 5) - measure_timer.y
|
||||
rl.draw_text_ex(font, self._alert_text, rl.Vector2(center.x - measure_timer.x / 2, timer_y), timer_text_size, spacing, rl.WHITE)
|
||||
else:
|
||||
for line in lines:
|
||||
measure = measure_text_cached(font, line, text_size, spacing)
|
||||
line_x = center.x - measure.x / 2
|
||||
rl.draw_text_ex(font, line, rl.Vector2(line_x, current_y), text_size, spacing, txt_color)
|
||||
current_y += text_size * FONT_SCALE
|
||||
@@ -0,0 +1,192 @@
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
from enum import IntEnum
|
||||
|
||||
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, DesiredSteeringPIDElement
|
||||
)
|
||||
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
|
||||
|
||||
|
||||
def get_bottom_dev_ui_offset():
|
||||
if ui_state.developer_ui in (DeveloperUiState.BOTTOM, DeveloperUiState.BOTH):
|
||||
return 60
|
||||
return 0
|
||||
|
||||
|
||||
class DeveloperUiState(IntEnum):
|
||||
OFF = 0
|
||||
BOTTOM = 1
|
||||
RIGHT = 2
|
||||
BOTH = 3
|
||||
|
||||
|
||||
class DeveloperUiRenderer(Widget):
|
||||
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 = DeveloperUiState.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.desired_pid_steer_elem = DesiredSteeringPIDElement()
|
||||
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 == DeveloperUiState.OFF:
|
||||
return
|
||||
|
||||
sm = ui_state.sm
|
||||
if sm.recv_frame["carState"] < ui_state.started_frame:
|
||||
return
|
||||
|
||||
if self.dev_ui_mode == DeveloperUiState.BOTTOM:
|
||||
self._draw_bottom_dev_ui(rect)
|
||||
elif self.dev_ui_mode == DeveloperUiState.RIGHT:
|
||||
self._draw_right_dev_ui(rect)
|
||||
elif self.dev_ui_mode == DeveloperUiState.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))
|
||||
elif controls_state.lateralControlState.which() == 'angleState':
|
||||
elements.append(self.desired_steer_elem.update(sm, ui_state.is_metric))
|
||||
elif controls_state.lateralControlState.which() == 'pidState':
|
||||
elements.append(self.desired_pid_steer_elem.update(sm, ui_state.is_metric))
|
||||
|
||||
elements.append(self.actual_lat_accel_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
|
||||
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':
|
||||
override_active = ui_state.enforce_torque_control and ui_state.custom_torque_params and ui_state.torque_override_enabled
|
||||
if sm.valid['liveTorqueParameters'] or override_active:
|
||||
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))
|
||||
|
||||
if not elements:
|
||||
return
|
||||
|
||||
font_size = 38
|
||||
element_widths = []
|
||||
for element in elements:
|
||||
element.measure(self._font_bold, font_size)
|
||||
element_widths.append(element.total_width)
|
||||
|
||||
total_element_width = sum(element_widths)
|
||||
num_gaps = len(elements) + 1
|
||||
available_width = rect.width
|
||||
gap_width = (available_width - total_element_width) / num_gaps
|
||||
|
||||
center_y = y + bar_height // 2
|
||||
current_x = rect.x + gap_width
|
||||
|
||||
for i, element in enumerate(elements):
|
||||
element_center_x = int(current_x + element_widths[i] / 2)
|
||||
self._draw_bottom_dev_ui_element(element_center_x, center_y, element)
|
||||
current_x += element_widths[i] + gap_width
|
||||
|
||||
def _draw_bottom_dev_ui_element(self, center_x: int, y: int, element: UiElement) -> None:
|
||||
font_size = 38
|
||||
start_x = center_x - element.total_width / 2
|
||||
|
||||
rl.draw_text_ex(self._font_bold, element.label_text, rl.Vector2(start_x, y - font_size // 2), font_size, 0, rl.WHITE)
|
||||
rl.draw_text_ex(self._font_bold, element.val_text, rl.Vector2(start_x + element.label_width, y - font_size // 2), font_size, 0, element.color)
|
||||
|
||||
if element.unit:
|
||||
rl.draw_text_ex(self._font_bold, element.unit_text, rl.Vector2(start_x + element.label_width + element.val_width, y - font_size // 2),
|
||||
font_size, 0, rl.WHITE)
|
||||
@@ -0,0 +1,348 @@
|
||||
"""
|
||||
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
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
|
||||
|
||||
@dataclass
|
||||
class UiElement:
|
||||
value: str
|
||||
label: str
|
||||
unit: str
|
||||
color: rl.Color
|
||||
val_text: str = ""
|
||||
label_text: str = ""
|
||||
unit_text: str = ""
|
||||
val_width: float = 0.0
|
||||
label_width: float = 0.0
|
||||
unit_width: float = 0.0
|
||||
total_width: float = 0.0
|
||||
|
||||
def measure(self, font, font_size: int):
|
||||
self.label_text = f"{self.label} "
|
||||
self.val_text = self.value
|
||||
self.unit_text = f" {self.unit}" if self.unit else ""
|
||||
|
||||
self.label_width = measure_text_cached(font, self.label_text, font_size, 0).x
|
||||
self.val_width = measure_text_cached(font, self.val_text, font_size, 0).x
|
||||
self.unit_width = measure_text_cached(font, self.unit_text, font_size, 0).x if self.unit else 0
|
||||
|
||||
self.total_width = self.label_width + self.val_width + self.unit_width
|
||||
|
||||
|
||||
class LeadInfoElement:
|
||||
@staticmethod
|
||||
def get_lead_status(sm):
|
||||
lead_one = sm['radarState'].leadOne
|
||||
return lead_one.present, 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 DesiredSteeringPIDElement(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.pidState.steeringAngleDesiredDeg
|
||||
|
||||
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 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:
|
||||
if ui_state.enforce_torque_control and ui_state.custom_torque_params and ui_state.torque_override_enabled:
|
||||
return UiElement(f"{ui_state.torque_override_friction:.3f}", "FRIC.", self.unit, rl.WHITE)
|
||||
|
||||
ltp = sm['liveTorqueParameters']
|
||||
value = f"{ltp.frictionCoefficientFiltered:.3f}"
|
||||
color = rl.Color(0, 255, 0, 255) if ltp.liveValid 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:
|
||||
if ui_state.enforce_torque_control and ui_state.custom_torque_params and ui_state.torque_override_enabled:
|
||||
return UiElement(f"{ui_state.torque_override_lat_accel_factor:.3f}", "L.A.F.", self.unit, rl.WHITE)
|
||||
|
||||
ltp = sm['liveTorqueParameters']
|
||||
value = f"{ltp.latAccelFactorFiltered:.3f}"
|
||||
color = rl.Color(0, 255, 0, 255) if ltp.liveValid 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)
|
||||
@@ -0,0 +1,48 @@
|
||||
"""
|
||||
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 numpy as np
|
||||
|
||||
from openpilot.selfdrive.ui import UI_BORDER_SIZE
|
||||
from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer, BTN_SIZE, ARC_LENGTH
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.developer_ui import get_bottom_dev_ui_offset
|
||||
|
||||
|
||||
class DriverStateRendererSP(DriverStateRenderer):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
|
||||
def _pre_calculate_drawing_elements(self):
|
||||
"""Pre-calculate all drawing elements based on the current rectangle"""
|
||||
# Calculate icon position (bottom-left or bottom-right)
|
||||
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 - get_bottom_dev_ui_offset()
|
||||
|
||||
# Pre-calculate the face lines positions
|
||||
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]
|
||||
|
||||
# Calculate arc dimensions based on head rotation
|
||||
delta_x = -self.driver_pose_sins[1] * ARC_LENGTH / 2.0 # Horizontal movement
|
||||
delta_y = -self.driver_pose_sins[0] * ARC_LENGTH / 2.0 # Vertical movement
|
||||
|
||||
# Horizontal arc
|
||||
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
|
||||
)
|
||||
|
||||
# Vertical arc
|
||||
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
|
||||
)
|
||||
@@ -0,0 +1,146 @@
|
||||
"""
|
||||
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.common.constants import CV
|
||||
from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.developer_ui import DeveloperUiRenderer, DeveloperUiState, get_bottom_dev_ui_offset
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.road_name import RoadNameRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.rocket_fuel import RocketFuel
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.speed_limit import SpeedLimitRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.smart_cruise_control import SmartCruiseControlRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.turn_signal import TurnSignalController
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.circular_alerts import CircularAlertsRenderer
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.speed_renderer import SpeedRenderer
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import HudRenderer, UI_CONFIG, FONT_SIZES, COLORS, CRUISE_DISABLED_CHAR
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
|
||||
SLA_ACTIVE_COLOR = rl.Color(0x91, 0x9b, 0x95, 0xff)
|
||||
|
||||
|
||||
class HudRendererSP(HudRenderer):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.developer_ui = DeveloperUiRenderer()
|
||||
self.road_name_renderer = RoadNameRenderer()
|
||||
self.rocket_fuel = RocketFuel()
|
||||
self.speed_limit_renderer = SpeedLimitRenderer()
|
||||
self.smart_cruise_control_renderer = SmartCruiseControlRenderer()
|
||||
self.turn_signal_controller = TurnSignalController()
|
||||
self.circular_alerts_renderer = CircularAlertsRenderer()
|
||||
self.speed_renderer = SpeedRenderer()
|
||||
self._torque_bar = TorqueBar(scale=3.0, always=True)
|
||||
|
||||
self.pcm_cruise_speed: bool = True
|
||||
self.show_icbm_status: bool = False
|
||||
self.icbm_active_counter: int = 0
|
||||
self.speed_cluster: float = 0.0
|
||||
self.speed_conv: float = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
|
||||
def _update_state(self) -> None:
|
||||
if ui_state.sm.recv_frame["carState"] < ui_state.started_frame:
|
||||
return
|
||||
|
||||
if ui_state.CP_SP is not None:
|
||||
self.pcm_cruise_speed = ui_state.CP_SP.pcmCruiseSpeed
|
||||
self.speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
self.speed_cluster = ui_state.sm['carState'].cruiseState.speedCluster * self.speed_conv
|
||||
|
||||
super()._update_state()
|
||||
self.road_name_renderer.update()
|
||||
self.speed_limit_renderer.update()
|
||||
self.smart_cruise_control_renderer.update()
|
||||
self.turn_signal_controller.update()
|
||||
self.circular_alerts_renderer.update()
|
||||
self.speed_renderer.update()
|
||||
|
||||
def _get_icbm_status(self):
|
||||
if not self.pcm_cruise_speed and ui_state.sm['carControl'].enabled:
|
||||
if round(self.set_speed) != round(self.speed_cluster):
|
||||
self.icbm_active_counter = 3 * gui_app.target_fps # 3 seconds usually
|
||||
elif self.icbm_active_counter > 0:
|
||||
self.icbm_active_counter -= 1
|
||||
else:
|
||||
self.icbm_active_counter = 0
|
||||
|
||||
self.show_icbm_status = self.icbm_active_counter > 0
|
||||
|
||||
def _draw_set_speed(self, rect: rl.Rectangle) -> None:
|
||||
long_plan_sp = ui_state.sm['longitudinalPlanSP']
|
||||
long_override = ui_state.sm['carControl'].cruiseControl.override
|
||||
self._get_icbm_status()
|
||||
|
||||
set_speed_width = UI_CONFIG.set_speed_width_metric if ui_state.is_metric else UI_CONFIG.set_speed_width_imperial
|
||||
x = rect.x + 60 + (UI_CONFIG.set_speed_width_imperial - set_speed_width) // 2
|
||||
y = rect.y + 45
|
||||
|
||||
set_speed_rect = rl.Rectangle(x, y, set_speed_width, UI_CONFIG.set_speed_height)
|
||||
rl.draw_rectangle_rounded(set_speed_rect, 0.35, 10, COLORS.BLACK_TRANSLUCENT)
|
||||
rl.draw_rectangle_rounded_lines_ex(set_speed_rect, 0.35, 10, 6, COLORS.BORDER_TRANSLUCENT)
|
||||
|
||||
max_color = COLORS.GREY
|
||||
set_speed_color = COLORS.DARK_GREY
|
||||
if self.is_cruise_set:
|
||||
set_speed_color = COLORS.WHITE
|
||||
if long_plan_sp.speedLimit.assist.active:
|
||||
set_speed_color = SLA_ACTIVE_COLOR if long_override else rl.Color(0, 0xff, 0, 0xff)
|
||||
max_color = SLA_ACTIVE_COLOR if long_override else rl.Color(0x80, 0xd8, 0xa6, 0xff)
|
||||
else:
|
||||
if ui_state.status == UIStatus.ENGAGED:
|
||||
max_color = COLORS.ENGAGED
|
||||
elif ui_state.status == UIStatus.DISENGAGED:
|
||||
max_color = COLORS.DISENGAGED
|
||||
elif ui_state.status == UIStatus.OVERRIDE:
|
||||
max_color = COLORS.OVERRIDE
|
||||
|
||||
max_str_size = 60 if self.show_icbm_status else 40
|
||||
max_str_y = 15 if self.show_icbm_status else 27
|
||||
|
||||
max_text = str(round(self.speed_cluster)) if self.show_icbm_status else tr("MAX")
|
||||
max_text_width = measure_text_cached(self._font_semi_bold, max_text, max_str_size).x
|
||||
rl.draw_text_ex(
|
||||
self._font_semi_bold,
|
||||
max_text,
|
||||
rl.Vector2(x + (set_speed_width - max_text_width) / 2, y + max_str_y),
|
||||
max_str_size,
|
||||
0,
|
||||
max_color,
|
||||
)
|
||||
|
||||
set_speed_text = CRUISE_DISABLED_CHAR if not self.is_cruise_set else str(round(self.set_speed))
|
||||
speed_text_width = measure_text_cached(self._font_bold, set_speed_text, FONT_SIZES.set_speed).x
|
||||
rl.draw_text_ex(
|
||||
self._font_bold,
|
||||
set_speed_text,
|
||||
rl.Vector2(x + (set_speed_width - speed_text_width) / 2, y + 77),
|
||||
FONT_SIZES.set_speed,
|
||||
0,
|
||||
set_speed_color,
|
||||
)
|
||||
|
||||
def _draw_current_speed(self, rect: rl.Rectangle) -> None:
|
||||
self.speed_renderer.render(rect)
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
super()._render(rect)
|
||||
|
||||
if ui_state.torque_bar:
|
||||
torque_rect = rect
|
||||
if ui_state.developer_ui in (DeveloperUiState.BOTTOM, DeveloperUiState.BOTH):
|
||||
torque_rect = rl.Rectangle(rect.x, rect.y, rect.width, rect.height - get_bottom_dev_ui_offset())
|
||||
self._torque_bar.render(torque_rect)
|
||||
|
||||
self.developer_ui.render(rect)
|
||||
self.road_name_renderer.render(rect)
|
||||
self.speed_limit_renderer.render(rect)
|
||||
self.smart_cruise_control_renderer.render(rect)
|
||||
self.turn_signal_controller.render(rect)
|
||||
self.circular_alerts_renderer.render(rect)
|
||||
self.rocket_fuel.render(rect, ui_state.sm)
|
||||
@@ -0,0 +1,14 @@
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
|
||||
|
||||
|
||||
class ModelRendererSP:
|
||||
def __init__(self):
|
||||
self.rainbow_path = RainbowPath()
|
||||
self.chevron_metrics = ChevronMetrics()
|
||||
@@ -0,0 +1,79 @@
|
||||
"""
|
||||
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 time
|
||||
import colorsys
|
||||
import pyray as rl
|
||||
from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
|
||||
|
||||
|
||||
class RainbowPath:
|
||||
DEFAULT_NUM_SEGMENTS = 8
|
||||
DEFAULT_SPEED = 50.0 # degrees per second
|
||||
DEFAULT_SATURATION = 0.9
|
||||
DEFAULT_LIGHTNESS = 0.6
|
||||
BASE_ALPHA = 0.8
|
||||
ALPHA_FADE = 0.3 # Alpha reduction from bottom to top
|
||||
|
||||
def __init__(self, num_segments: int = DEFAULT_NUM_SEGMENTS, speed: float = DEFAULT_SPEED,
|
||||
saturation: float = DEFAULT_SATURATION, lightness: float = DEFAULT_LIGHTNESS):
|
||||
self.num_segments = num_segments
|
||||
self.speed = speed
|
||||
self.saturation = saturation
|
||||
self.lightness = lightness
|
||||
|
||||
def set_speed(self, speed: float):
|
||||
self.speed = speed
|
||||
|
||||
def set_num_segments(self, num_segments: int):
|
||||
self.num_segments = num_segments
|
||||
|
||||
def set_saturation(self, saturation: float):
|
||||
self.saturation = max(0.0, min(1.0, saturation))
|
||||
|
||||
def set_lightness(self, lightness: float):
|
||||
self.lightness = max(0.0, min(1.0, lightness))
|
||||
|
||||
def get_gradient(self) -> Gradient:
|
||||
time_offset = time.monotonic()
|
||||
hue_offset = (time_offset * self.speed) % 360.0
|
||||
|
||||
segment_colors = []
|
||||
gradient_stops = []
|
||||
|
||||
for i in range(self.num_segments):
|
||||
position = i / (self.num_segments - 1)
|
||||
hue = (hue_offset + position * 360.0) % 360.0
|
||||
alpha = self.BASE_ALPHA * (1.0 - position * self.ALPHA_FADE)
|
||||
color = self._hsla_to_color(
|
||||
hue / 360.0,
|
||||
self.saturation,
|
||||
self.lightness,
|
||||
alpha
|
||||
)
|
||||
gradient_stops.append(position)
|
||||
segment_colors.append(color)
|
||||
|
||||
return Gradient(
|
||||
start=(0.0, 1.0), # Bottom of path
|
||||
end=(0.0, 0.0), # Top of path
|
||||
colors=segment_colors,
|
||||
stops=gradient_stops,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _hsla_to_color(h: float, s: float, l: float, a: float) -> rl.Color:
|
||||
rgb = colorsys.hls_to_rgb(h, l, s)
|
||||
return rl.Color(
|
||||
int(rgb[0] * 255),
|
||||
int(rgb[1] * 255),
|
||||
int(rgb[2] * 255),
|
||||
int(a * 255)
|
||||
)
|
||||
|
||||
def draw_rainbow_path(self, rect, path):
|
||||
gradient = self.get_gradient()
|
||||
draw_polygon(rect, path.projected_points, gradient=gradient)
|
||||
@@ -0,0 +1,56 @@
|
||||
"""
|
||||
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.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 RoadNameRenderer(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.road_name = ""
|
||||
self.is_metric = False
|
||||
self.font_demi = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
|
||||
def update(self):
|
||||
sm = ui_state.sm
|
||||
if sm.recv_frame["carState"] < ui_state.started_frame:
|
||||
return
|
||||
|
||||
self.is_metric = ui_state.is_metric
|
||||
|
||||
if sm.updated["liveMapDataSP"]:
|
||||
lmd = sm["liveMapDataSP"]
|
||||
self.road_name = lmd.roadName
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
if not self.road_name or not ui_state.road_name_toggle:
|
||||
return
|
||||
|
||||
text = self.road_name
|
||||
text_size = measure_text_cached(self.font_demi, text, 46)
|
||||
|
||||
padding = 40
|
||||
rect_width = max(200, min(text_size.x + padding, rect.width - 40))
|
||||
|
||||
road_rect = rl.Rectangle(rect.x + rect.width / 2 - rect_width / 2, rect.y - 4, rect_width, 60)
|
||||
|
||||
rl.draw_rectangle_rounded(road_rect, 0.2, 10, rl.Color(0, 0, 0, 120))
|
||||
|
||||
max_text_width = road_rect.width - 20
|
||||
if text_size.x > max_text_width:
|
||||
while text_size.x > max_text_width and len(text) > 3:
|
||||
text = text[:-1]
|
||||
text_size = measure_text_cached(self.font_demi, text + "...", 46)
|
||||
text = text + "..."
|
||||
|
||||
sz = measure_text_cached(self.font_demi, text, 46)
|
||||
origin = rl.Vector2(road_rect.x + road_rect.width / 2 - sz.x / 2, road_rect.y + road_rect.height / 2 - sz.y / 2)
|
||||
rl.draw_text_ex(self.font_demi, text, origin, 46, 0, rl.Color(255, 255, 255, 200))
|
||||
@@ -0,0 +1,50 @@
|
||||
"""
|
||||
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
|
||||
|
||||
|
||||
class RocketFuel:
|
||||
def __init__(self):
|
||||
self.vc_accel = 0.0
|
||||
|
||||
def render(self, rect: rl.Rectangle, sm) -> None:
|
||||
if not ui_state.rocket_fuel:
|
||||
return
|
||||
|
||||
vc_accel0 = sm['carState'].aEgo
|
||||
|
||||
# Smooth the acceleration
|
||||
self.vc_accel = self.vc_accel + (vc_accel0 - self.vc_accel) / 5.0
|
||||
|
||||
hha = 0.0
|
||||
color = rl.Color(0, 0, 0, 0) # Transparent by default
|
||||
|
||||
if self.vc_accel > 0:
|
||||
hha = 0.85 - 0.1 / self.vc_accel # only extend up to 85%
|
||||
color = rl.Color(0, 245, 0, 200)
|
||||
elif self.vc_accel < 0:
|
||||
hha = 0.85 + 0.1 / self.vc_accel # only extend up to 85%
|
||||
color = rl.Color(245, 0, 0, 200)
|
||||
|
||||
if hha < 0:
|
||||
hha = 0.0
|
||||
|
||||
hha = hha * rect.height
|
||||
wp = 28.0
|
||||
|
||||
# Draw
|
||||
rect_h = rect.height
|
||||
|
||||
if self.vc_accel > 0:
|
||||
ra_y = rect_h / 2.0 - hha / 2.0
|
||||
else:
|
||||
ra_y = rect_h / 2.0
|
||||
|
||||
if hha > 0:
|
||||
rl.draw_rectangle(int(rect.x), int(rect.y + ra_y), int(wp), int(hha / 2.0), color)
|
||||
@@ -0,0 +1,105 @@
|
||||
"""
|
||||
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 COLORS
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
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.sunnypilot.lib.utils import AlertFadeAnimator
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
|
||||
class SmartCruiseControlRenderer(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.vision_enabled = False
|
||||
self.vision_active = False
|
||||
self.map_enabled = False
|
||||
self.map_active = False
|
||||
self.long_override = False
|
||||
|
||||
self._vision_fade = AlertFadeAnimator(gui_app.target_fps)
|
||||
self._map_fade = AlertFadeAnimator(gui_app.target_fps)
|
||||
|
||||
self.font = gui_app.font(FontWeight.BOLD)
|
||||
|
||||
def update(self):
|
||||
sm = ui_state.sm
|
||||
if sm.updated["longitudinalPlanSP"]:
|
||||
lp_sp = sm["longitudinalPlanSP"]
|
||||
vision = lp_sp.smartCruiseControl.vision
|
||||
map_ = lp_sp.smartCruiseControl.map
|
||||
|
||||
self.vision_enabled = vision.enabled
|
||||
self.vision_active = vision.active
|
||||
self.map_enabled = map_.enabled
|
||||
self.map_active = map_.active
|
||||
|
||||
if sm.updated["carControl"]:
|
||||
self.long_override = sm["carControl"].cruiseControl.override
|
||||
|
||||
self._vision_fade.update(self.vision_active)
|
||||
self._map_fade.update(self.map_active)
|
||||
|
||||
def _draw_icon(self, rect_center_x, rect_height, x_offset, y_offset, name, alpha=1.0):
|
||||
text = name
|
||||
font_size = 36
|
||||
padding_v = 5
|
||||
box_width = 160
|
||||
|
||||
sz = measure_text_cached(self.font, text, font_size)
|
||||
box_height = int(sz.y + padding_v * 2)
|
||||
|
||||
if self.long_override:
|
||||
color = COLORS.OVERRIDE
|
||||
box_color = rl.Color(color.r, color.g, color.b, int(alpha * 255))
|
||||
else:
|
||||
box_color = rl.Color(0, 255, 0, int(alpha * 255))
|
||||
|
||||
text_color = rl.Color(0, 0, 0, int(alpha * 255))
|
||||
|
||||
screen_y = rect_height / 4 + y_offset
|
||||
|
||||
box_x = rect_center_x + x_offset - box_width / 2
|
||||
box_y = screen_y - box_height / 2
|
||||
|
||||
# Draw rounded background box
|
||||
if alpha > 0.01:
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(box_x, box_y, box_width, box_height), 0.2, 10, box_color)
|
||||
|
||||
# Draw text centered in the box (black color for contrast against bright green/grey)
|
||||
text_pos_x = box_x + (box_width - sz.x) / 2
|
||||
text_pos_y = box_y + (box_height - sz.y) / 2
|
||||
|
||||
rl.draw_text_ex(self.font, text, rl.Vector2(text_pos_x, text_pos_y), font_size, 0, text_color)
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
x_offset = -260
|
||||
y1_offset = -40
|
||||
y2_offset = -100
|
||||
|
||||
orders = [y1_offset, y2_offset]
|
||||
y_scc_v = 0
|
||||
y_scc_m = 0
|
||||
idx = 0
|
||||
|
||||
if self.vision_enabled:
|
||||
y_scc_v = orders[idx]
|
||||
idx += 1
|
||||
|
||||
if self.map_enabled:
|
||||
y_scc_m = orders[idx]
|
||||
idx += 1
|
||||
|
||||
if self.vision_enabled:
|
||||
alpha = self._vision_fade.alpha if self.vision_active else 1.0
|
||||
self._draw_icon(rect.x + rect.width / 2, rect.height, x_offset, y_scc_v, "SCC-V", alpha)
|
||||
|
||||
if self.map_enabled:
|
||||
alpha = self._map_fade.alpha if self.map_active else 1.0
|
||||
self._draw_icon(rect.x + rect.width / 2, rect.height, x_offset, y_scc_m, "SCC-M", alpha)
|
||||
@@ -0,0 +1,328 @@
|
||||
"""
|
||||
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.
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import StrEnum
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import UI_CONFIG
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode
|
||||
from openpilot.common.hardware import HARDWARE
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.sunnypilot.lib.utils import AlertFadeAnimator
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
METER_TO_FOOT = 3.28084
|
||||
METER_TO_MILE = 0.000621371
|
||||
AHEAD_THRESHOLD = 5
|
||||
SET_SPEED_NA = 255
|
||||
KM_TO_MILE = 0.621371
|
||||
|
||||
AssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Colors:
|
||||
WHITE = rl.WHITE
|
||||
BLACK = rl.BLACK
|
||||
RED = rl.Color(235, 32, 32, 255)
|
||||
GREY = rl.Color(145, 155, 149, 255)
|
||||
DARK_GREY = rl.Color(77, 77, 77, 255)
|
||||
SUB_BG = rl.Color(0, 0, 0, 180)
|
||||
MUTCD_LINES = rl.Color(255, 255, 255, 100)
|
||||
|
||||
|
||||
class IconSide(StrEnum):
|
||||
left = 'left'
|
||||
right = 'right'
|
||||
|
||||
|
||||
class SpeedLimitAlertRenderer:
|
||||
ARROW_SIZE = 90 if HARDWARE.get_device_type() == 'mici' else 200
|
||||
|
||||
def __init__(self):
|
||||
self.arrow_up = gui_app.texture("../../sunnypilot/selfdrive/assets/img_plus_arrow_up.png", self.ARROW_SIZE, self.ARROW_SIZE)
|
||||
self.arrow_down = gui_app.texture("../../sunnypilot/selfdrive/assets/img_minus_arrow_down.png", self.ARROW_SIZE, self.ARROW_SIZE)
|
||||
|
||||
blank_image = rl.gen_image_color(self.ARROW_SIZE, self.ARROW_SIZE, rl.Color(0, 0, 0, 0))
|
||||
self.arrow_blank = rl.load_texture_from_image(blank_image)
|
||||
rl.unload_image(blank_image)
|
||||
|
||||
self._pre_active_fade = AlertFadeAnimator(gui_app.target_fps, duration_on=0.75, rc=0.05)
|
||||
|
||||
def update(self):
|
||||
assist_state = ui_state.sm['longitudinalPlanSP'].speedLimit.assist.state
|
||||
self._pre_active_fade.update(assist_state == AssistState.preActive)
|
||||
|
||||
def speed_limit_pre_active_icon_helper(self):
|
||||
icon_alpha = max(0.0, min(self._pre_active_fade.alpha * 255.0, 255.0))
|
||||
txt_icon = self.arrow_blank
|
||||
icon_margin_x = 10
|
||||
icon_margin_y = 18
|
||||
|
||||
if icon_alpha > 0:
|
||||
speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
speed_limit_final_last = ui_state.sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
|
||||
|
||||
v_cruise_cluster = ui_state.sm['carState'].vCruiseCluster
|
||||
set_speed = ui_state.sm['controlsState'].deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster
|
||||
if not ui_state.is_metric:
|
||||
set_speed *= KM_TO_MILE
|
||||
|
||||
set_speed_round = round(set_speed)
|
||||
speed_limit_round = round(speed_limit_final_last * speed_conv)
|
||||
|
||||
if set_speed_round < speed_limit_round:
|
||||
txt_icon = self.arrow_up
|
||||
elif set_speed_round > speed_limit_round:
|
||||
txt_icon = self.arrow_down
|
||||
|
||||
return IconSide.right, txt_icon, icon_alpha, icon_margin_x, icon_margin_y
|
||||
|
||||
|
||||
class SpeedLimitRenderer(Widget, SpeedLimitAlertRenderer):
|
||||
def __init__(self):
|
||||
Widget.__init__(self)
|
||||
SpeedLimitAlertRenderer.__init__(self)
|
||||
|
||||
self.speed_limit = 0.0
|
||||
self.speed_limit_last = 0.0
|
||||
self.speed_limit_offset = 0.0
|
||||
self.speed_limit_valid = False
|
||||
self.speed_limit_last_valid = False
|
||||
self.speed_limit_final_last = 0.0
|
||||
self.speed_limit_source = SpeedLimitSource.none
|
||||
self.speed_limit_assist_state = AssistState.disabled
|
||||
|
||||
self.speed_limit_ahead = 0.0
|
||||
self.speed_limit_ahead_dist = 0.0
|
||||
self.speed_limit_ahead_dist_prev = 0.0
|
||||
self.speed_limit_ahead_valid = False
|
||||
self.speed_limit_ahead_frame = 0
|
||||
|
||||
self.is_cruise_set: bool = False
|
||||
self.is_cruise_available: bool = True
|
||||
self.set_speed: float = SET_SPEED_NA
|
||||
self.speed: float = 0.0
|
||||
self.v_ego_cluster_seen: bool = False
|
||||
|
||||
self.font_bold = gui_app.font(FontWeight.BOLD)
|
||||
self.font_demi = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
self.font_norm = gui_app.font(FontWeight.NORMAL)
|
||||
|
||||
@property
|
||||
def speed_conv(self):
|
||||
return CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
|
||||
def update(self):
|
||||
SpeedLimitAlertRenderer.update(self)
|
||||
sm = ui_state.sm
|
||||
if sm.recv_frame["carState"] < ui_state.started_frame:
|
||||
self.set_speed = SET_SPEED_NA
|
||||
self.speed = 0.0
|
||||
return
|
||||
|
||||
if sm.updated["longitudinalPlanSP"]:
|
||||
lp_sp = sm["longitudinalPlanSP"]
|
||||
resolver = lp_sp.speedLimit.resolver
|
||||
assist = lp_sp.speedLimit.assist
|
||||
|
||||
self.speed_limit = resolver.speedLimit * self.speed_conv
|
||||
self.speed_limit_last = resolver.speedLimitLast * self.speed_conv
|
||||
self.speed_limit_offset = resolver.speedLimitOffset * self.speed_conv
|
||||
self.speed_limit_valid = resolver.speedLimitValid
|
||||
self.speed_limit_last_valid = resolver.speedLimitLastValid
|
||||
self.speed_limit_final_last = resolver.speedLimitFinalLast * self.speed_conv
|
||||
self.speed_limit_source = resolver.source
|
||||
self.speed_limit_assist_state = assist.state
|
||||
|
||||
if sm.updated["liveMapDataSP"]:
|
||||
lmd = sm["liveMapDataSP"]
|
||||
self.speed_limit_ahead_valid = lmd.speedLimitAheadValid
|
||||
self.speed_limit_ahead = lmd.speedLimitAhead * self.speed_conv
|
||||
self.speed_limit_ahead_dist = lmd.speedLimitAheadDistance
|
||||
|
||||
if self.speed_limit_ahead_dist < self.speed_limit_ahead_dist_prev and self.speed_limit_ahead_frame < AHEAD_THRESHOLD:
|
||||
self.speed_limit_ahead_frame += 1
|
||||
elif self.speed_limit_ahead_dist > self.speed_limit_ahead_dist_prev and self.speed_limit_ahead_frame > 0:
|
||||
self.speed_limit_ahead_frame -= 1
|
||||
|
||||
self.speed_limit_ahead_dist_prev = self.speed_limit_ahead_dist
|
||||
|
||||
controls_state = sm['controlsState']
|
||||
car_state = sm["carState"]
|
||||
|
||||
v_cruise_cluster = car_state.vCruiseCluster
|
||||
self.set_speed = (
|
||||
controls_state.deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster
|
||||
)
|
||||
self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA
|
||||
self.is_cruise_available = self.set_speed != -1
|
||||
|
||||
if self.is_cruise_set and not ui_state.is_metric:
|
||||
self.set_speed *= KM_TO_MILE
|
||||
|
||||
self.v_ego_cluster_seen = self.v_ego_cluster_seen or car_state.vEgoCluster != 0.0
|
||||
v_ego = car_state.vEgoCluster if self.v_ego_cluster_seen else car_state.vEgo
|
||||
self.speed = max(0.0, v_ego * self.speed_conv)
|
||||
|
||||
@staticmethod
|
||||
def _draw_text_centered(font, text, size, pos_center, color):
|
||||
sz = measure_text_cached(font, text, size)
|
||||
rl.draw_text_ex(font, text, rl.Vector2(pos_center.x - sz.x / 2, pos_center.y - sz.y / 2), size, 0, color)
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
width = UI_CONFIG.set_speed_width_metric if ui_state.is_metric else UI_CONFIG.set_speed_width_imperial
|
||||
x = rect.x + 60 + width + 30 - 6
|
||||
y = rect.y + 45 - 6
|
||||
|
||||
sign_rect = rl.Rectangle(x, y, width, UI_CONFIG.set_speed_height + 6 * 2)
|
||||
|
||||
alpha = self._pre_active_fade.alpha
|
||||
|
||||
if ui_state.speed_limit_mode != SpeedLimitMode.off:
|
||||
self._draw_sign_main(sign_rect, alpha)
|
||||
if self.speed_limit_assist_state == AssistState.preActive:
|
||||
self._draw_pre_active_arrow(sign_rect)
|
||||
else:
|
||||
self._draw_ahead_info(sign_rect)
|
||||
|
||||
def _draw_sign_main(self, rect, alpha=1.0):
|
||||
speed_limit_warning_enabled = ui_state.speed_limit_mode >= SpeedLimitMode.warning
|
||||
has_limit = self.speed_limit_valid or self.speed_limit_last_valid
|
||||
is_overspeed = has_limit and round(self.speed_limit_final_last) < round(self.speed)
|
||||
|
||||
limit_str = str(round(self.speed_limit_last)) if has_limit else "---"
|
||||
sub_text = ""
|
||||
if self.speed_limit_offset != 0:
|
||||
sign = "" if self.speed_limit_offset > 0 else "-"
|
||||
sub_text = f"{sign}{round(abs(self.speed_limit_offset))}"
|
||||
|
||||
txt_color = Colors.BLACK
|
||||
if speed_limit_warning_enabled and is_overspeed:
|
||||
txt_color = Colors.RED
|
||||
elif not self.speed_limit_valid:
|
||||
txt_color = Colors.GREY
|
||||
|
||||
if ui_state.is_metric:
|
||||
self._render_vienna(rect, limit_str, sub_text, txt_color, has_limit, alpha)
|
||||
else:
|
||||
self._render_mutcd(rect, limit_str, sub_text, txt_color, has_limit, alpha)
|
||||
|
||||
def _draw_pre_active_arrow(self, sign_rect):
|
||||
_, txt_icon, icon_alpha, _, _ = SpeedLimitAlertRenderer.speed_limit_pre_active_icon_helper(self)
|
||||
if icon_alpha > 0 and txt_icon != self.arrow_blank:
|
||||
sign_margin = 12
|
||||
arrow_spacing = int(sign_margin * 1.4)
|
||||
arrow_x = sign_rect.x + sign_rect.width + arrow_spacing
|
||||
arrow_y = sign_rect.y + (sign_rect.height - txt_icon.height) / 2
|
||||
color = rl.Color(255, 255, 255, int(icon_alpha))
|
||||
rl.draw_texture_ex(txt_icon, rl.Vector2(arrow_x, arrow_y), 0.0, 1.0, color)
|
||||
|
||||
def _render_vienna(self, rect, val, sub, color, has_limit, alpha=1.0):
|
||||
center = rl.Vector2(rect.x + rect.width / 2, rect.y + rect.height / 2)
|
||||
radius = (rect.width + 18) / 2
|
||||
|
||||
white = rl.color_alpha(Colors.WHITE, alpha)
|
||||
red = rl.color_alpha(Colors.RED, alpha)
|
||||
black = rl.color_alpha(Colors.BLACK, alpha)
|
||||
dark_grey = rl.color_alpha(Colors.DARK_GREY, alpha)
|
||||
text_color = rl.color_alpha(color, alpha)
|
||||
|
||||
rl.draw_circle_v(center, radius, white)
|
||||
rl.draw_ring(center, radius * 0.75, radius, 0, 360, 36, red)
|
||||
|
||||
font_size = 70 if len(val) >= 3 else 85
|
||||
self._draw_text_centered(self.font_bold, val, font_size, center, text_color)
|
||||
|
||||
if sub and has_limit:
|
||||
s_radius = radius * 0.4
|
||||
s_center = rl.Vector2(rect.x + rect.width - s_radius / 2, rect.y + s_radius / 2)
|
||||
|
||||
rl.draw_circle_v(s_center, s_radius, black)
|
||||
rl.draw_ring(s_center, s_radius - 3, s_radius, 0, 360, 36, dark_grey)
|
||||
|
||||
font_scale = 0.5 if len(sub) < 3 else 0.45
|
||||
self._draw_text_centered(self.font_bold, sub, int(s_radius * 2 * font_scale), s_center, white)
|
||||
|
||||
def _render_mutcd(self, rect, val, sub, color, has_limit, alpha=1.0):
|
||||
white = rl.color_alpha(Colors.WHITE, alpha)
|
||||
black = rl.color_alpha(Colors.BLACK, alpha)
|
||||
dark_grey = rl.color_alpha(Colors.DARK_GREY, alpha)
|
||||
text_color = rl.color_alpha(color, alpha)
|
||||
|
||||
rl.draw_rectangle_rounded(rect, 0.35, 10, white)
|
||||
|
||||
inner = rl.Rectangle(rect.x + 10, rect.y + 10, rect.width - 20, rect.height - 20)
|
||||
outer_radius = 0.35 * rect.width / 2.0
|
||||
inner_radius = outer_radius - 10.0
|
||||
inner_roundness = inner_radius / (inner.width / 2.0)
|
||||
|
||||
rl.draw_rectangle_rounded_lines_ex(inner, inner_roundness, 10, 4, black)
|
||||
|
||||
self._draw_text_centered(self.font_demi, "SPEED", 40, rl.Vector2(rect.x + rect.width / 2, rect.y + 40), black)
|
||||
self._draw_text_centered(self.font_demi, "LIMIT", 40, rl.Vector2(rect.x + rect.width / 2, rect.y + 80), black)
|
||||
self._draw_text_centered(self.font_bold, val, 90, rl.Vector2(rect.x + rect.width / 2, rect.y + 150), text_color)
|
||||
|
||||
if sub and has_limit:
|
||||
box_sz = rect.width * 0.3
|
||||
overlap = box_sz * 0.2
|
||||
s_rect = rl.Rectangle(rect.x + rect.width - box_sz / 1.5 + overlap, rect.y - box_sz / 1.25 + overlap, box_sz, box_sz)
|
||||
|
||||
rl.draw_rectangle_rounded(s_rect, 0.35, 10, black)
|
||||
rl.draw_rectangle_rounded_lines_ex(s_rect, 0.35, 10, 6, dark_grey)
|
||||
|
||||
f_scale = 0.6 if len(sub) < 3 else 0.475
|
||||
self._draw_text_centered(self.font_bold, sub, int(box_sz * f_scale), rl.Vector2(s_rect.x + box_sz / 2, s_rect.y + box_sz / 2), white)
|
||||
|
||||
def _draw_ahead_info(self, sign_rect):
|
||||
source_is_map = self.speed_limit_source == SpeedLimitSource.map
|
||||
valid = self.speed_limit_ahead_valid and self.speed_limit_ahead > 0 and self.speed_limit_ahead != self.speed_limit
|
||||
|
||||
if not (valid and source_is_map):
|
||||
return
|
||||
|
||||
rect = rl.Rectangle(sign_rect.x + (sign_rect.width - 170) / 2, sign_rect.y + sign_rect.height + 10, 170, 160)
|
||||
rl.draw_rectangle_rounded(rect, 0.35, 10, Colors.SUB_BG)
|
||||
rl.draw_rectangle_rounded_lines_ex(rect, 0.35, 10, 3, Colors.MUTCD_LINES)
|
||||
|
||||
mid_x = rect.x + rect.width / 2
|
||||
self._draw_text_centered(self.font_demi, "AHEAD", 40, rl.Vector2(mid_x, rect.y + 28), Colors.GREY)
|
||||
self._draw_text_centered(self.font_bold, str(round(self.speed_limit_ahead)), 70, rl.Vector2(mid_x, rect.y + 82), Colors.WHITE)
|
||||
self._draw_text_centered(self.font_norm, self._format_dist(self.speed_limit_ahead_dist), 36, rl.Vector2(mid_x, rect.y + 134), Colors.GREY)
|
||||
|
||||
@staticmethod
|
||||
def _format_dist(d):
|
||||
# metric
|
||||
if ui_state.is_metric:
|
||||
if d < 50:
|
||||
return tr("Near")
|
||||
|
||||
if d >= 1000:
|
||||
return f"{d / 1000:.1f} km"
|
||||
|
||||
d_rounded = round(d, -1) if d < 200 else round(d, -2)
|
||||
return f"{int(d_rounded)} m"
|
||||
|
||||
# imperial
|
||||
d_ft = d * METER_TO_FOOT
|
||||
if d_ft < 100:
|
||||
return tr("Near")
|
||||
|
||||
if d_ft >= 900:
|
||||
return f"{d * METER_TO_MILE:.1f} mi"
|
||||
|
||||
if d_ft < 500:
|
||||
return f"{int(round(d_ft / 50) * 50)} ft"
|
||||
|
||||
return f"{int(round(d_ft / 100) * 100)} ft"
|
||||
@@ -0,0 +1,46 @@
|
||||
"""
|
||||
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.common.constants import CV
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.selfdrive.ui.onroad.hud_renderer import FONT_SIZES, COLORS
|
||||
|
||||
|
||||
class SpeedRenderer:
|
||||
def __init__(self):
|
||||
self.speed: float = 0.0
|
||||
self.v_ego_cluster_seen: bool = False
|
||||
|
||||
self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
|
||||
self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
|
||||
|
||||
def update(self) -> None:
|
||||
car_state = ui_state.sm['carState']
|
||||
v_ego_cluster = car_state.vEgoCluster
|
||||
self.v_ego_cluster_seen = self.v_ego_cluster_seen or v_ego_cluster != 0.0
|
||||
v_ego = v_ego_cluster if self.v_ego_cluster_seen and not ui_state.true_v_ego_ui else car_state.vEgo
|
||||
speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
self.speed = max(0.0, v_ego * speed_conversion)
|
||||
|
||||
def render(self, rect: rl.Rectangle) -> None:
|
||||
if ui_state.hide_v_ego_ui:
|
||||
return
|
||||
|
||||
# Draw current speed and unit
|
||||
speed_text = str(round(self.speed))
|
||||
speed_text_size = measure_text_cached(self._font_bold, speed_text, FONT_SIZES.current_speed)
|
||||
speed_pos = rl.Vector2(rect.x + rect.width / 2 - speed_text_size.x / 2, 180 - speed_text_size.y / 2)
|
||||
rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.WHITE)
|
||||
|
||||
unit_text = tr("km/h") if ui_state.is_metric else tr("mph")
|
||||
unit_text_size = measure_text_cached(self._font_medium, unit_text, FONT_SIZES.speed_unit)
|
||||
unit_pos = rl.Vector2(rect.x + rect.width / 2 - unit_text_size.x / 2, 290 - unit_text_size.y / 2)
|
||||
rl.draw_text_ex(self._font_medium, unit_text, unit_pos, FONT_SIZES.speed_unit, 0, COLORS.WHITE_TRANSLUCENT)
|
||||
@@ -0,0 +1,119 @@
|
||||
"""
|
||||
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
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.selfdrive.ui.mici.onroad.alert_renderer import IconSide, TURN_SIGNAL_BLINK_PERIOD
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TurnSignalConfig:
|
||||
left_x: int = 80
|
||||
left_y: int = 190
|
||||
right_x: int = 80
|
||||
right_y: int = 190
|
||||
size: int = 150
|
||||
|
||||
|
||||
class TurnSignalWidget(Widget):
|
||||
def __init__(self, direction: IconSide):
|
||||
super().__init__()
|
||||
self._direction = direction
|
||||
self._active = False
|
||||
self._type = 'signal'
|
||||
|
||||
self._turn_signal_timer = 0.0
|
||||
self._turn_signal_alpha_filter = FirstOrderFilter(0.0, 0.3, 1 / gui_app.target_fps)
|
||||
|
||||
self._signal_texture = gui_app.texture('icons_mici/onroad/turn_signal_left.png', 120, 109, flip_x=(direction == IconSide.right))
|
||||
self._blind_spot_texture = gui_app.texture('icons_mici/onroad/blind_spot_left.png', 120, 109, flip_x=(direction == IconSide.right))
|
||||
self._texture = self._signal_texture
|
||||
|
||||
def _render(self, _):
|
||||
if not self._active:
|
||||
return
|
||||
|
||||
if self._type == 'signal':
|
||||
if time.monotonic() - self._turn_signal_timer > TURN_SIGNAL_BLINK_PERIOD:
|
||||
self._turn_signal_timer = time.monotonic()
|
||||
self._turn_signal_alpha_filter.x = 255 * 2
|
||||
else:
|
||||
self._turn_signal_alpha_filter.update(255 * 0.2)
|
||||
icon_alpha = int(min(self._turn_signal_alpha_filter.x, 255))
|
||||
else:
|
||||
icon_alpha = 255
|
||||
|
||||
self._texture = self._blind_spot_texture if self._type == 'blind_spot' else self._signal_texture
|
||||
|
||||
if self._texture:
|
||||
pos_x = self._rect.x + (self._rect.width - self._texture.width) / 2
|
||||
pos_y = self._rect.y + (self._rect.height - self._texture.height) / 2
|
||||
color = rl.Color(255, 255, 255, icon_alpha)
|
||||
rl.draw_texture_ex(self._texture, rl.Vector2(pos_x, pos_y), 0.0, 1.0, color)
|
||||
|
||||
def activate(self, _type: str = 'signal'):
|
||||
if not self._active or self._type != _type:
|
||||
self._turn_signal_timer = 0.0
|
||||
self._active = True
|
||||
self._type = _type
|
||||
|
||||
def deactivate(self):
|
||||
self._active = False
|
||||
self._turn_signal_timer = 0.0
|
||||
|
||||
|
||||
class TurnSignalController:
|
||||
def __init__(self):
|
||||
self._config = TurnSignalConfig()
|
||||
self._left_signal = TurnSignalWidget(direction=IconSide.left)
|
||||
self._right_signal = TurnSignalWidget(direction=IconSide.right)
|
||||
|
||||
@staticmethod
|
||||
def _update_signal(signal, blindspot, blinker):
|
||||
if ui_state.blindspot and blindspot:
|
||||
signal.activate('blind_spot')
|
||||
elif ui_state.turn_signals and blinker:
|
||||
signal.activate('signal')
|
||||
else:
|
||||
signal.deactivate()
|
||||
|
||||
def update(self):
|
||||
CS = ui_state.sm['carState']
|
||||
|
||||
self._update_signal(self._left_signal, CS.leftBlindspot, CS.leftBlinker)
|
||||
self._update_signal(self._right_signal, CS.rightBlindspot, CS.rightBlinker)
|
||||
|
||||
def render(self, rect: rl.Rectangle):
|
||||
if not ui_state.turn_signals and not ui_state.blindspot:
|
||||
return
|
||||
|
||||
x = rect.x + rect.width / 2
|
||||
|
||||
left_x = x - self._config.left_x - self._config.size
|
||||
left_y = rect.y + self._config.left_y
|
||||
|
||||
right_x = x + self._config.right_x
|
||||
right_y = rect.y + self._config.right_y
|
||||
|
||||
if self._left_signal._active:
|
||||
self._left_signal.render(rl.Rectangle(left_x, left_y, self._config.size, self._config.size))
|
||||
|
||||
if self._right_signal._active:
|
||||
self._right_signal.render(rl.Rectangle(right_x, right_y, self._config.size, self._config.size))
|
||||
|
||||
@property
|
||||
def config(self) -> TurnSignalConfig:
|
||||
return self._config
|
||||
|
||||
@config.setter
|
||||
def config(self, new_config: TurnSignalConfig):
|
||||
self._config = new_config
|
||||
Reference in new issue
Block a user