From 7bebf0ef3c20817cff103c2932c300785a8d84a9 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Sun, 24 May 2026 01:01:31 -0400 Subject: [PATCH] BigUI WIP: Force Stop -> Aethergauge --- selfdrive/ui/onroad/starpilot/aethergauge.py | 103 +++++++++++++- .../ui/onroad/starpilot/csc_force_stop.py | 126 ------------------ .../onroad/starpilot/starpilot_onroad_view.py | 2 - 3 files changed, 97 insertions(+), 134 deletions(-) delete mode 100644 selfdrive/ui/onroad/starpilot/csc_force_stop.py diff --git a/selfdrive/ui/onroad/starpilot/aethergauge.py b/selfdrive/ui/onroad/starpilot/aethergauge.py index 17bf983f0..9c09864fe 100644 --- a/selfdrive/ui/onroad/starpilot/aethergauge.py +++ b/selfdrive/ui/onroad/starpilot/aethergauge.py @@ -15,6 +15,7 @@ class AetherGaugeData: color: rl.Color = rl.WHITE indicator_type: str = "none" # e.g. "road_curve" indicator_value: float = 0.0 # value used by indicator (e.g. curvature) + stop_sign_confirmed: bool = False class AetherGaugeSource: def is_active(self) -> bool: @@ -23,6 +24,36 @@ class AetherGaugeSource: def get_gauge_data(self) -> AetherGaugeData: raise NotImplementedError +class ForceStopSource(AetherGaugeSource): + def is_active(self) -> bool: + if "starpilotPlan" not in ui_state.sm.valid or not ui_state.sm.valid["starpilotPlan"]: + return False + plan = ui_state.sm["starpilotPlan"] + return getattr(plan, "forcingStop", False) + + def get_gauge_data(self) -> AetherGaugeData: + plan = ui_state.sm["starpilotPlan"] + + distance_conversion = 1.0 if ui_state.is_metric else 3.28084 + dist_unit = "m" if ui_state.is_metric else "ft" + + forcing_stop_length = getattr(plan, "forcingStopLength", 0.0) + dist_val = int(round(forcing_stop_length * distance_conversion)) + stop_sign_confirmed = getattr(plan, "stopSignConfirmed", False) + + color = rl.Color(255, 30, 60, 255) # Sleek high-contrast red + + return AetherGaugeData( + text=str(dist_val), + unit=dist_unit, + label="STOP", + animation="force_stop", + color=color, + indicator_type="force_stop", + indicator_value=forcing_stop_length, + stop_sign_confirmed=stop_sign_confirmed + ) + class CurveSpeedSource(AetherGaugeSource): def is_active(self) -> bool: state = _csc_state() @@ -59,7 +90,7 @@ class CurveSpeedSource(AetherGaugeSource): class AetherGauge: def __init__(self): - self.sources: list[AetherGaugeSource] = [CurveSpeedSource()] + self.sources: list[AetherGaugeSource] = [ForceStopSource(), CurveSpeedSource()] def get_active_data(self) -> AetherGaugeData | None: for source in self.sources: @@ -82,7 +113,7 @@ class AetherGauge: # Position the curving road widget to the left of the speed text, center-aligned vertically icon_cx = cx - speed_text_size.x / 2 - 70.0 - if data.indicator_type == "road_curve": + if data.indicator_type in ("road_curve", "force_stop"): self._render_unified_road(icon_cx, cy_speed - 39.5, data, font_bold, font_medium) def _render_unified_road(self, icx: float, icy: float, data: AetherGaugeData, font_bold: rl.Font, font_medium: rl.Font): @@ -92,9 +123,12 @@ class AetherGauge: top = icy - half_size # Non-linear curvature offset - abs_curv = abs(data.indicator_value) - sign = math.copysign(1.0, data.indicator_value) if data.indicator_value != 0.0 else 1.0 - curve_offset = max(-half_size, min(half_size, sign * (abs_curv ** 0.5) * 300.0)) + if data.indicator_type == "road_curve": + abs_curv = abs(data.indicator_value) + sign = math.copysign(1.0, data.indicator_value) if data.indicator_value != 0.0 else 1.0 + curve_offset = max(-half_size, min(half_size, sign * (abs_curv ** 0.5) * 300.0)) + else: + curve_offset = 0.0 # 3D perspective widths w_bottom = 28.0 @@ -137,7 +171,13 @@ class AetherGauge: rl.draw_line_ex(points_right[i], points_right[i+1], thickness, data.color) # C. Draw animating chevrons flowing down the road center path - progress = (rl.get_time() * 1.2) % 1.0 + if data.indicator_type == "force_stop": + # Slow down chevrons as we approach the stop + speed_factor = max(0.1, min(1.0, data.indicator_value / 30.0)) + else: + speed_factor = 1.0 + + progress = (rl.get_time() * 1.2 * speed_factor) % 1.0 for i in range(3): # Chevron fraction t (flowing down from 1.0 to 0.0) t = 1.0 - ((i + progress) / 3.0) @@ -187,6 +227,10 @@ class AetherGauge: rl.draw_line_ex(rl.Vector2(int(lx), int(ly)), rl.Vector2(int(cx_t), int(cy_t)), chevron_thick, chev_color) rl.draw_line_ex(rl.Vector2(int(rx), int(ry)), rl.Vector2(int(cx_t), int(cy_t)), chevron_thick, chev_color) + # E. Draw approaching stop sign if force_stop + if data.indicator_type == "force_stop": + self._draw_approaching_stop_sign(icx, icy, bottom, data.indicator_value, data.stop_sign_confirmed) + # D. Draw clean floating target speed directly below the road base self._draw_mini_cradle(icx, bottom, data.text, data.unit, data.color, font_bold, font_medium) @@ -210,3 +254,50 @@ class AetherGauge: rl.draw_text_ex(font_medium, unit, rl.Vector2(unit_pos.x - 1, unit_pos.y + 1), 20, 0, rl.BLACK) rl.draw_text_ex(font_medium, unit, rl.Vector2(unit_pos.x + 1, unit_pos.y + 1), 20, 0, rl.BLACK) rl.draw_text_ex(font_medium, unit, unit_pos, 20, 0, rl.Color(255, 255, 255, 200)) + + def _draw_approaching_stop_sign(self, cx: float, icy: float, bottom: float, distance: float, confirmed: bool): + # Normalized progress: d_max = 60.0 meters + d_max = 60.0 + s = max(0.0, min(1.0, (d_max - distance) / d_max)) + + # 3D perspective translation (stays within the road boundary) + y_sign = (icy - 30.0) + (s ** 1.5) * 50.0 + + # Dynamic scale based on distance + r_min = 6.0 + r_max = 20.0 + r_sign = r_min + (s ** 2.0) * (r_max - r_min) + + # Draw drop shadow + rl.draw_poly(rl.Vector2(int(cx), int(y_sign + 2)), 8, r_sign, 22.5, rl.Color(0, 0, 0, 120)) + + # Draw outer white border + rl.draw_poly(rl.Vector2(int(cx), int(y_sign)), 8, r_sign, 22.5, rl.WHITE) + + # Draw inner red octagon + red_color = rl.Color(196, 30, 58, 255) + rl.draw_poly(rl.Vector2(int(cx), int(y_sign)), 8, r_sign - 3, 22.5, red_color) + + # Draw checkmark or exclamation point + if confirmed: + # White checkmark + lx = cx - r_sign * 0.3 + ly = y_sign + bx = cx - r_sign * 0.07 + by = y_sign + r_sign * 0.22 + rx = cx + r_sign * 0.35 + ry = y_sign - r_sign * 0.22 + + thick = max(2.0, r_sign * 0.12) + rl.draw_line_ex(rl.Vector2(int(lx), int(ly)), rl.Vector2(int(bx), int(by)), thick, rl.WHITE) + rl.draw_line_ex(rl.Vector2(int(bx), int(by)), rl.Vector2(int(rx), int(ry)), thick, rl.WHITE) + else: + # White exclamation mark + thick = max(1.5, r_sign * 0.12) + bar_h = r_sign * 0.4 + dot_r = max(1.0, r_sign * 0.08) + + # Top bar + rl.draw_rectangle(int(cx - thick / 2), int(y_sign - r_sign * 0.35), int(thick), int(bar_h), rl.WHITE) + # Dot + rl.draw_circle(int(cx), int(y_sign + r_sign * 0.35), int(dot_r), rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/csc_force_stop.py b/selfdrive/ui/onroad/starpilot/csc_force_stop.py deleted file mode 100644 index c5845200a..000000000 --- a/selfdrive/ui/onroad/starpilot/csc_force_stop.py +++ /dev/null @@ -1,126 +0,0 @@ -import pyray as rl -import math -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 - -def render_csc_force_stop(content_rect: rl.Rectangle, font_bold): - plan = ui_state.sm["starpilotPlan"] if ui_state.sm.valid.get("starpilotPlan", False) else None - if not plan: - return - - forcing_stop = getattr(plan, "forcingStop", False) - csc_enabled = getattr(plan, "curveSpeedControlEnabled", False) - cond_status = ui_state.conditional_status - - if not forcing_stop and not (cond_status == 3 and csc_enabled): - return - - # Calculate layout coordinates matching C++ - # setSpeedRect calculations: - ss_width = 200 if ui_state.is_metric else 172 - ss_x = content_rect.x + 60 + (172 - ss_width) // 2 - ss_y = content_rect.y + 45 - ss_height = 204 - - csc_x = ss_x + ss_width + 30 - csc_y = ss_y - w = 215 - h = 215 - - csc_rect = rl.Rectangle(csc_x, csc_y, w, h) - badge_rect = rl.Rectangle(csc_x, csc_y + h + 10, w, 100) - - distance_conversion = 1.0 if ui_state.is_metric else 3.28084 - speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH - dist_unit = "m" if ui_state.is_metric else "ft" - speed_unit = "km/h" if ui_state.is_metric else "mph" - - car_state = ui_state.sm["carState"] if ui_state.sm.valid.get("carState", False) else None - v_ego = car_state.vEgo if car_state else 0.0 - - if forcing_stop: - # ── FORCE STOP MODE ── - forcing_stop_length = getattr(plan, "forcingStopLength", 0.0) - stop_sign_confirmed = getattr(plan, "stopSignConfirmed", False) - - # Draw Octagon Stop sign in csc_rect - cx = csc_x + w / 2 - cy = csc_y + h / 2 - radius = 65.0 - rl.draw_poly(rl.Vector2(int(cx), int(cy)), 8, radius, 22.5, rl.WHITE) - rl.draw_poly(rl.Vector2(int(cx), int(cy)), 8, radius - 5, 22.5, rl.Color(196, 30, 58, 255)) - - if stop_sign_confirmed: - # Draw white checkmark - rl.draw_line_ex(rl.Vector2(int(cx - 20), int(cy)), rl.Vector2(int(cx - 5), int(cy + 15)), 6, rl.WHITE) - rl.draw_line_ex(rl.Vector2(int(cx - 5), int(cy + 15)), rl.Vector2(int(cx + 25), int(cy - 15)), 6, rl.WHITE) - else: - # Draw white exclamation mark - rl.draw_rectangle(int(cx - 4), int(cy - 25), 8, 30, rl.WHITE) - rl.draw_circle(int(cx), int(cy + 18), 5, rl.WHITE) - - # Draw red badge with stopping distance - rl.draw_rectangle_rounded(badge_rect, 0.24, 16, rl.Color(196, 30, 58, 166)) - rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.24, 16, 4, rl.Color(255, 150, 150, 255)) - - dist_val = int(round(forcing_stop_length * distance_conversion)) - text = f"{dist_val} {dist_unit}" - text_sz = measure_text_cached(font_bold, text, 40) - rl.draw_text_ex(font_bold, text, rl.Vector2(int(csc_x + 20), int(badge_rect.y + (100 - text_sz.y) / 2)), 40, 0, rl.WHITE) - - else: - # ── CURVE SPEED CONTROL MODE ── - csc_speed = getattr(plan, "cscSpeed", 0.0) - road_curvature = getattr(plan, "roadCurvature", 0.0) - - # Pulsing glowing border for CSC icon - phase = (rl.get_time() % 2.0) / 2.0 * 2.0 * math.pi - alpha_factor = 0.5 + 0.5 * math.sin(phase) - glow_color = rl.Color(0, 140, 255, int(255 * (0.3 + 0.7 * alpha_factor))) - glow_width = int(8 + 4 * alpha_factor) - - rl.draw_rectangle_rounded(csc_rect, 0.24, 16, rl.Color(0, 0, 0, 166)) - rl.draw_rectangle_rounded_lines_ex(csc_rect, 0.24, 16, glow_width, glow_color) - - # Draw curvy path line inside csc_rect - cx = csc_x + w / 2 - cy = csc_y + h / 2 - path_color = rl.Color(0, 140, 255, 255) - - # Draw curve left or right - if road_curvature < 0: - # Left curve spline - p1 = rl.Vector2(int(cx), int(cy + 60)) - p2 = rl.Vector2(int(cx), int(cy - 20)) - p3 = rl.Vector2(int(cx - 50), int(cy - 50)) - rl.draw_spline_bezier_quadratic([p1, p2, p3], 3, 8, path_color) - # Left arrowhead - rl.draw_triangle( - rl.Vector2(int(cx - 55), int(cy - 60)), - rl.Vector2(int(cx - 35), int(cy - 40)), - rl.Vector2(int(cx - 45), int(cy - 35)), - path_color - ) - else: - # Right curve spline - p1 = rl.Vector2(int(cx), int(cy + 60)) - p2 = rl.Vector2(int(cx), int(cy - 20)) - p3 = rl.Vector2(int(cx + 50), int(cy - 50)) - rl.draw_spline_bezier_quadratic([p1, p2, p3], 3, 8, path_color) - # Right arrowhead - rl.draw_triangle( - rl.Vector2(int(cx + 55), int(cy - 60)), - rl.Vector2(int(cx + 45), int(cy - 35)), - rl.Vector2(int(cx + 35), int(cy - 40)), - path_color - ) - - # Draw blue badge with CSC speed target - rl.draw_rectangle_rounded(badge_rect, 0.24, 16, rl.Color(0, 140, 255, 166)) - rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.24, 16, 4, rl.Color(100, 200, 255, 255)) - - csc_speed_val = int(round(min(v_ego, csc_speed) * speed_conversion)) - text = f"{csc_speed_val} {speed_unit}" - text_sz = measure_text_cached(font_bold, text, 40) - rl.draw_text_ex(font_bold, text, rl.Vector2(int(csc_x + 20), int(badge_rect.y + (100 - text_sz.y) / 2)), 40, 0, rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 250bda2f0..f99e85e22 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -52,8 +52,6 @@ class StarPilotOnroadView(AugmentedRoadView): int(self._content_rect.width), int(self._content_rect.height), ) render_speed_limit(self._content_rect) - from openpilot.selfdrive.ui.onroad.starpilot.csc_force_stop import render_csc_force_stop - render_csc_force_stop(self._content_rect, self._font_bold) rl.end_scissor_mode() def _render_overlays(self):