87 lines
3.3 KiB
Python
87 lines
3.3 KiB
Python
import math
|
|
import pyray as rl
|
|
from iqpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH
|
|
from iqpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
|
from iqpilot.system.ui.widgets import Widget
|
|
from iqpilot.system.ui.lib.application import gui_app
|
|
from iqpilot.common.filter_simple import FirstOrderFilter
|
|
|
|
from iqpilot.ui.mici.onroad.confidence_ball import IQConfidenceBall
|
|
|
|
|
|
def draw_circle_gradient(center_x: float, center_y: float, radius: int,
|
|
top: rl.Color, bottom: rl.Color) -> None:
|
|
# Draw a square with the gradient
|
|
rl.draw_rectangle_gradient_v(int(center_x - radius), int(center_y - radius),
|
|
radius * 2, radius * 2,
|
|
top, bottom)
|
|
|
|
# Paint over square with a ring
|
|
outer_radius = math.ceil(radius * math.sqrt(2)) + 1
|
|
rl.draw_ring(rl.Vector2(int(center_x), int(center_y)), radius, outer_radius,
|
|
0.0, 360.0,
|
|
20, rl.BLACK)
|
|
|
|
|
|
class ConfidenceBall(Widget, IQConfidenceBall):
|
|
def __init__(self, demo: bool = False):
|
|
Widget.__init__(self)
|
|
IQConfidenceBall.__init__(self)
|
|
self._demo = demo
|
|
self._confidence_filter = FirstOrderFilter(-0.5, 0.5, 1 / gui_app.target_fps)
|
|
|
|
def update_filter(self, value: float):
|
|
self._confidence_filter.update(value)
|
|
|
|
def _update_state(self):
|
|
if self._demo:
|
|
return
|
|
|
|
# animate status dot in from bottom
|
|
if ui_state.status == UIStatus.DISENGAGED:
|
|
self._confidence_filter.update(-0.5)
|
|
elif ui_state.status in (UIStatus.LAT_ONLY, UIStatus.LONG_ONLY):
|
|
self._confidence_filter.update(1 - max(self.get_animate_status_probs() or [1]))
|
|
else:
|
|
self._confidence_filter.update((1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs or [1])) *
|
|
(1 - max(ui_state.sm['modelV2'].meta.disengagePredictions.steerOverrideProbs or [1])))
|
|
|
|
def _render(self, _):
|
|
content_rect = rl.Rectangle(
|
|
self.rect.x + self.rect.width - SIDE_PANEL_WIDTH,
|
|
self.rect.y,
|
|
SIDE_PANEL_WIDTH,
|
|
self.rect.height,
|
|
)
|
|
|
|
status_dot_radius = 24
|
|
dot_height = (1 - self._confidence_filter.x) * (content_rect.height - 2 * status_dot_radius) + status_dot_radius
|
|
dot_height = self._rect.y + dot_height
|
|
|
|
# confidence zones
|
|
if ui_state.status == UIStatus.ENGAGED or self._demo:
|
|
if self._confidence_filter.x > 0.5:
|
|
top_dot_color = rl.Color(0, 255, 204, 255)
|
|
bottom_dot_color = rl.Color(0, 255, 38, 255)
|
|
elif self._confidence_filter.x > 0.2:
|
|
top_dot_color = rl.Color(255, 200, 0, 255)
|
|
bottom_dot_color = rl.Color(255, 115, 0, 255)
|
|
else:
|
|
top_dot_color = rl.Color(255, 0, 21, 255)
|
|
bottom_dot_color = rl.Color(255, 0, 89, 255)
|
|
|
|
elif ui_state.status in (UIStatus.LAT_ONLY, UIStatus.LONG_ONLY):
|
|
top_dot_color, bottom_dot_color = self.get_lat_long_dot_colors(self._confidence_filter.x)
|
|
|
|
elif ui_state.status == UIStatus.OVERRIDE:
|
|
top_dot_color = rl.Color(255, 255, 255, 255)
|
|
bottom_dot_color = rl.Color(82, 82, 82, 255)
|
|
|
|
else:
|
|
top_dot_color = rl.Color(50, 50, 50, 255)
|
|
bottom_dot_color = rl.Color(13, 13, 13, 255)
|
|
|
|
draw_circle_gradient(content_rect.x + content_rect.width - status_dot_radius,
|
|
dot_height, status_dot_radius,
|
|
top_dot_color, bottom_dot_color)
|