IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/ui/mici/onroad/confidence_ball.py

@@ -0,0 +1,86 @@
+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)