From fd7f75578dc3a62e5d9f261762a62ca842308b30 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Sun, 24 May 2026 03:58:25 -0400 Subject: [PATCH] BigUI WIP: Remove white border on sign + less steppy movement --- selfdrive/ui/onroad/starpilot/aethergauge.py | 594 ++++++++++++++---- .../ui/onroad/starpilot/stopping_point.py | 19 +- 2 files changed, 500 insertions(+), 113 deletions(-) diff --git a/selfdrive/ui/onroad/starpilot/aethergauge.py b/selfdrive/ui/onroad/starpilot/aethergauge.py index b4e293574..f51bb4c88 100644 --- a/selfdrive/ui/onroad/starpilot/aethergauge.py +++ b/selfdrive/ui/onroad/starpilot/aethergauge.py @@ -16,6 +16,7 @@ class AetherGaugeData: 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 + indicator_extra: str = "" class AetherGaugeSource: def is_active(self) -> bool: @@ -54,6 +55,15 @@ class ForceStopSource(AetherGaugeSource): stop_sign_confirmed=stop_sign_confirmed ) +class CEStatus: + USER_OVERRIDDEN = 2 + CURVATURE = 3 + LEAD = 4 + SIGNAL = 5 + SPEED = 6 + SPEED_LIMIT = 7 + STOP_LIGHT = 8 + class CurveSpeedSource(AetherGaugeSource): def is_active(self) -> bool: state = _csc_state() @@ -88,9 +98,121 @@ class CurveSpeedSource(AetherGaugeSource): indicator_value=state['curvature'] ) +class CEMSource(AetherGaugeSource): + # Configuration mapping for each conditional status code. + # Format: (label, animation, indicator_type, static_color) + CONFIGS = { + CEStatus.USER_OVERRIDDEN: ("EXP", "down_arrows", "road_curve", rl.Color(255, 140, 0, 255)), + CEStatus.CURVATURE: ("CURVE", "down_arrows", "road_curve", None), + CEStatus.LEAD: ("LEAD", "lead", "lead", rl.Color(255, 191, 0, 255)), + CEStatus.STOP_LIGHT: ("STOP", "stop_light", "stop_light", rl.Color(255, 30, 60, 255)), + } + + def __init__(self, status: int): + self.status = status + self.label, self.animation, self.indicator_type, self.static_color = self.CONFIGS[status] + + def is_active(self) -> bool: + if ui_state.conditional_status != self.status: + return False + if self.status in (CEStatus.CURVATURE, CEStatus.STOP_LIGHT): + return "starpilotPlan" in ui_state.sm.valid and ui_state.sm.valid["starpilotPlan"] + if self.status == CEStatus.LEAD: + if "radarState" not in ui_state.sm.valid or not ui_state.sm.valid["radarState"]: + return False + return ui_state.sm["radarState"].leadOne.status + return True + + def get_gauge_data(self) -> AetherGaugeData: + plan = ui_state.sm["starpilotPlan"] if "starpilotPlan" in ui_state.sm.valid and ui_state.sm.valid["starpilotPlan"] else None + car_state = ui_state.sm["carState"] if "carState" in ui_state.sm.valid and ui_state.sm.valid["carState"] else None + starpilot_car_state = ui_state.sm["starpilotCarState"] if "starpilotCarState" in ui_state.sm.valid and ui_state.sm.valid["starpilotCarState"] else None + model = ui_state.sm["modelV2"] if "modelV2" in ui_state.sm.valid and ui_state.sm.valid["modelV2"] else None + radar_state = ui_state.sm["radarState"] if "radarState" in ui_state.sm.valid and ui_state.sm.valid["radarState"] else None + + # Determine conversion and unit (STOP_LIGHT displays stopping distance; others display speeds) + is_distance = (self.status == CEStatus.STOP_LIGHT) + if is_distance: + conversion = 1.0 if ui_state.is_metric else 3.28084 + unit = "m" if ui_state.is_metric else "ft" + else: + conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH + unit = "km/h" if ui_state.is_metric else "mph" + + # Default output parameters + val = 0 + indicator_value = 0.0 + stop_sign_confirmed = False + indicator_extra = "" + + # Shared cruise speed calculation fallback + v_cruise = getattr(plan, "vCruise", car_state.vEgo if car_state else 0.0) if plan else (car_state.vEgo if car_state else 0.0) + + if self.status == CEStatus.STOP_LIGHT: + stopping_distance = 0.0 + if model and len(model.position.x) > 0: + stopping_distance = model.position.x[min(32, len(model.position.x) - 1)] + else: + stopping_distance = getattr(plan, "forcingStopLength", 0.0) if plan else 0.0 + val = int(round(stopping_distance * conversion)) + indicator_value = stopping_distance + stop_sign_confirmed = getattr(plan, "stopSignConfirmed", False) or (getattr(starpilot_car_state, "dashboardStopSign", 0) > 0 if starpilot_car_state else False) + indicator_extra = "red" + + elif self.status == CEStatus.CURVATURE: + csc_speed = getattr(plan, "cscSpeed", 0.0) if plan else 0.0 + target_speed = csc_speed if csc_speed > 0.1 else v_cruise + val = int(round(target_speed * conversion)) + indicator_value = getattr(plan, "roadCurvature", 0.0) if plan else 0.0 + + elif self.status == CEStatus.LEAD: + lead = radar_state.leadOne if radar_state else None + lead_speed = lead.vLead if (lead and lead.status) else 0.0 + val = int(round(lead_speed * conversion)) + indicator_value = lead.dRel if (lead and lead.status) else 0.0 + if lead and lead.status and lead_speed < 1.0: + indicator_extra = "stopped" + else: + indicator_extra = "slower" + + + + + elif self.status == CEStatus.USER_OVERRIDDEN: + val = int(round(v_cruise * conversion)) + + # Resolve color (CURVATURE dynamically computes its glow color) + if self.status == CEStatus.CURVATURE: + road_curvature = getattr(plan, "roadCurvature", 0.0) if plan else 0.0 + intensity = max(0.0, min(1.0, abs(road_curvature) * 200.0)) + color = _glow_color(intensity) + else: + color = self.static_color + + return AetherGaugeData( + text=str(val), + unit=unit, + label=self.label, + animation=self.animation, + color=color, + indicator_type=self.indicator_type, + indicator_value=indicator_value, + stop_sign_confirmed=stop_sign_confirmed, + indicator_extra=indicator_extra + ) + class AetherGauge: def __init__(self): - self.sources: list[AetherGaugeSource] = [ForceStopSource(), CurveSpeedSource()] + self.sources: list[AetherGaugeSource] = [ + ForceStopSource(), + CEMSource(CEStatus.STOP_LIGHT), + CurveSpeedSource(), + CEMSource(CEStatus.CURVATURE), + CEMSource(CEStatus.LEAD), + CEMSource(CEStatus.USER_OVERRIDDEN), + ] + # Smoothed approach progress for stop sign (smooth motion between data updates) + self._stop_smooth_s = 0.0 def get_active_data(self) -> AetherGaugeData | None: for source in self.sources: @@ -125,6 +247,46 @@ class AetherGauge: def render(self, rect: rl.Rectangle, font_bold: rl.Font, font_medium: rl.Font, current_speed: float): data = self.get_active_data() + + # === TEST CYCLE OVERRIDE === + # Set to True to cycle all Aethergauge visual effects for testing. + # Set to False to run off live data. + TEST_CYCLE = True + if TEST_CYCLE: + now_time = rl.get_time() + if now_time > 3.0: + cycle_sec = 4.0 + states = [ + ("road_curve", "45", "CURVE", rl.Color(0, 255, 100, 255), 0.005, "", False), + ("stop_light", "25", "STOP", rl.Color(255, 30, 60, 255), 35.0, "red", False), + ("lead", "32", "LEAD", rl.Color(255, 191, 0, 255), 25.0, "slower", False), + ("lead", "0", "LEAD", rl.Color(255, 191, 0, 255), 15.0, "stopped", False), + ("force_stop", "12", "STOP", rl.Color(255, 30, 60, 255), 12.0, "", True), + ] + cycle_idx = int((now_time - 3.0) / cycle_sec) % len(states) + s = states[cycle_idx] + + # Determine dynamic indicator_value (approaching distance/speed) for animations + ind_val = s[4] + elapsed = (now_time - 3.0) % cycle_sec + if s[0] in ("stop_light", "force_stop"): + ind_val = max(2.0, 60.0 - (elapsed / (cycle_sec - 0.5)) * 58.0) + elif s[0] == "lead": + ind_val = max(10.0, 80.0 - (elapsed / (cycle_sec - 0.5)) * 70.0) + + extra = s[5] + + data = AetherGaugeData( + text=s[1] if s[0] not in ("stop_light", "force_stop") else str(int(round(ind_val * (1.0 if s[5] != "metric" else 3.28084)))), + unit="mph" if s[5] != "metric" else "km/h", + label=s[2], + color=s[3], + indicator_type=s[0], + indicator_value=ind_val, + indicator_extra=extra, + stop_sign_confirmed=s[6] + ) + if not data: return @@ -138,7 +300,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 in ("road_curve", "force_stop"): + if data.indicator_type in ("road_curve", "force_stop", "lead", "stop_light"): 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): @@ -162,10 +324,11 @@ class AetherGauge: for i in range(num_segments + 1): t = i / num_segments - if data.indicator_type == "road_curve" and model: + if data.indicator_type in ("road_curve", "lead", "stop_light", "force_stop") and model: path_y = self._get_model_offset(t, model) abs_y = abs(path_y) - sign = math.copysign(1.0, path_y) if path_y != 0.0 else 1.0 + # Invert sign because in openpilot y is positive-left, but on screen x is positive-right. + sign = -math.copysign(1.0, path_y) if path_y != 0.0 else 1.0 # Apply perspective-based shaping factor (t * sqrt(t)) to start straight and bend progressively offset = sign * (abs_y ** 0.6) * 10.0 * t * math.sqrt(t) offset = max(-half_size, min(half_size, offset)) @@ -188,6 +351,9 @@ class AetherGauge: rl.draw_triangle(points_right[i], points_right[i+1], points_left[i+1], fill_color) # B. Draw left and right road boundaries + left_color = data.color + right_color = data.color + shadow_color = rl.Color(0, 0, 0, 100) for i in range(num_segments): # Shadow lines @@ -195,84 +361,281 @@ class AetherGauge: rl.draw_line_ex(rl.Vector2(points_right[i].x, points_right[i].y + 2), rl.Vector2(points_right[i+1].x, points_right[i+1].y + 2), thickness, shadow_color) # Main boundaries - rl.draw_line_ex(points_left[i], points_left[i+1], thickness, data.color) - rl.draw_line_ex(points_right[i], points_right[i+1], thickness, data.color) + rl.draw_line_ex(points_left[i], points_left[i+1], thickness, left_color) + rl.draw_line_ex(points_right[i], points_right[i+1], thickness, right_color) + + # B2. Draw red stop line/bar on the road if approaching a stop light or stop sign + if data.indicator_type in ("stop_light", "force_stop"): + t_line = max(0.0, min(1.0, data.indicator_value / 60.0)) + w_line = w_bottom - t_line * (w_bottom - w_top) + + if model: + path_y = self._get_model_offset(t_line, model) + abs_y = abs(path_y) + sign = -math.copysign(1.0, path_y) if path_y != 0.0 else 1.0 + offset_line = sign * (abs_y ** 0.6) * 10.0 * t_line * math.sqrt(t_line) + offset_line = max(-half_size, min(half_size, offset_line)) + else: + offset_line = 0.0 + + cx_line = icx + offset_line + cy_line = bottom - t_line * 80.0 + + p_left = rl.Vector2(int(cx_line - w_line + 2.0), int(cy_line)) + p_right = rl.Vector2(int(cx_line + w_line - 2.0), int(cy_line)) + + line_color_glow = rl.Color(255, 30, 60, int(150 * (1.0 - t_line * 0.5))) + line_color_core = rl.Color(255, 200, 200, 255) + + rl.draw_line_ex(p_left, p_right, max(3.0, 7.0 * (1.0 - t_line * 0.5)), line_color_glow) + rl.draw_line_ex(p_left, p_right, max(1.5, 3.5 * (1.0 - t_line * 0.5)), line_color_core) + + # C. Draw animating overlays based on active indicator mode + if data.indicator_type == "lead": + # Render lead car outline in perspective + lead_dist = data.indicator_value + # Fix mathematically inverted depth representation + t_lead = max(0.15, min(0.85, lead_dist / 80.0)) + + # Model curvature offset for lead car center + if model: + path_y = self._get_model_offset(t_lead, model) + abs_y = abs(path_y) + sign = -math.copysign(1.0, path_y) if path_y != 0.0 else 1.0 + offset_lead = sign * (abs_y ** 0.6) * 10.0 * t_lead * math.sqrt(t_lead) + offset_lead = max(-half_size, min(half_size, offset_lead)) + else: + offset_lead = 0.0 + + cx_lead = icx + offset_lead + cy_lead = bottom - t_lead * 80.0 + + # Scale size by depth (closer car is larger) + scale = 0.45 + (1.0 - t_lead) * 0.55 + W_car = 30.0 * scale + H_car = 18.0 * scale + + is_stopped = (data.indicator_extra == "stopped") + is_slower = (data.indicator_extra == "slower") + + if is_stopped: + border_color = rl.Color(255, 60, 60, 255) + elif is_slower: + # Gentle amber pulse on the outline to show active braking + pulse = 0.5 + 0.5 * math.sin(rl.get_time() * 3.0) + border_color = rl.Color(data.color.r, data.color.g, data.color.b, int(160 + 95 * pulse)) + else: + border_color = data.color + + # Draw car cabin + rect_cabin = rl.Rectangle(cx_lead - W_car * 0.3, cy_lead - H_car, W_car * 0.6, H_car * 0.45) + rl.draw_rectangle_rounded(rect_cabin, 0.5, 4, rl.Color(15, 15, 15, 240)) + rl.draw_rectangle_rounded_lines_ex(rect_cabin, 0.5, 4, 1.5, border_color) + + # Draw car body + rect_body = rl.Rectangle(cx_lead - W_car / 2, cy_lead - H_car * 0.65, W_car, H_car * 0.55) + rl.draw_rectangle_rounded(rect_body, 0.3, 4, rl.Color(20, 20, 20, 240)) + rl.draw_rectangle_rounded_lines_ex(rect_body, 0.3, 4, 1.5, border_color) + + # Draw taillights with state-dependent intensity + tl_w = W_car * 0.15 + tl_h = H_car * 0.12 + if is_stopped: + pulse = 0.5 + 0.5 * math.sin(rl.get_time() * 8.0) # Fast warning pulse + # Outer glowing red halo + rl.draw_circle(int(cx_lead - W_car * 0.38), int(cy_lead - H_car * 0.5), int(W_car * 0.08 * (1.0 + 0.4 * pulse)), rl.Color(255, 30, 60, int(150 + 105 * pulse))) + rl.draw_circle(int(cx_lead + W_car * 0.38), int(cy_lead - H_car * 0.5), int(W_car * 0.08 * (1.0 + 0.4 * pulse)), rl.Color(255, 30, 60, int(150 + 105 * pulse))) + # Inner bright core + rl.draw_rectangle(int(cx_lead - W_car * 0.45), int(cy_lead - H_car * 0.55), int(tl_w), int(tl_h), rl.Color(255, 220, 220, 255)) + rl.draw_rectangle(int(cx_lead + W_car * 0.3), int(cy_lead - H_car * 0.55), int(tl_w), int(tl_h), rl.Color(255, 220, 220, 255)) + elif is_slower: + pulse = 0.5 + 0.5 * math.sin(rl.get_time() * 3.0) # Gentle brake throb + # Warm amber glow halo + rl.draw_circle(int(cx_lead - W_car * 0.38), int(cy_lead - H_car * 0.5), int(W_car * 0.06 * (1.0 + 0.2 * pulse)), rl.Color(255, 140, 30, int(100 + 80 * pulse))) + rl.draw_circle(int(cx_lead + W_car * 0.38), int(cy_lead - H_car * 0.5), int(W_car * 0.06 * (1.0 + 0.2 * pulse)), rl.Color(255, 140, 30, int(100 + 80 * pulse))) + # Amber taillight core + rl.draw_rectangle(int(cx_lead - W_car * 0.45), int(cy_lead - H_car * 0.55), int(tl_w), int(tl_h), rl.Color(255, 160, 60, int(180 + 75 * pulse))) + rl.draw_rectangle(int(cx_lead + W_car * 0.3), int(cy_lead - H_car * 0.55), int(tl_w), int(tl_h), rl.Color(255, 160, 60, int(180 + 75 * pulse))) + else: + rl.draw_rectangle(int(cx_lead - W_car * 0.45), int(cy_lead - H_car * 0.55), int(tl_w), int(tl_h), rl.Color(255, 30, 60, 255)) + rl.draw_rectangle(int(cx_lead + W_car * 0.3), int(cy_lead - H_car * 0.55), int(tl_w), int(tl_h), rl.Color(255, 30, 60, 255)) + + # Draw wheels + rl.draw_rectangle(int(cx_lead - W_car * 0.4), int(cy_lead - H_car * 0.1), int(W_car * 0.12), int(H_car * 0.1), rl.Color(10, 10, 10, 255)) + rl.draw_rectangle(int(cx_lead + W_car * 0.28), int(cy_lead - H_car * 0.1), int(W_car * 0.12), int(H_car * 0.1), rl.Color(10, 10, 10, 255)) + + # Draw flow-limited chevrons behind the lead vehicle (flowing from car to driver) + progress = (rl.get_time() * 1.5) % 1.0 + num_chevs = 2 + for i in range(num_chevs): + t = t_lead * (1.0 - ((i + progress) / num_chevs) % 1.0) + + if model: + path_y = self._get_model_offset(t, model) + abs_y = abs(path_y) + sign = -math.copysign(1.0, path_y) if path_y != 0.0 else 1.0 + offset_t = sign * (abs_y ** 0.6) * 10.0 * t * math.sqrt(t) + offset_t = max(-half_size, min(half_size, offset_t)) + else: + offset_t = 0.0 + + cx_t = icx + offset_t + cy_t = bottom - t * 80.0 + + chevron_w = 12.0 - t * 5.0 + chevron_thick = max(1.5, 3.5 - t * 1.5) + lx = cx_t - chevron_w + rx = cx_t + chevron_w + + alpha = int(data.color.a * (1.0 - t / t_lead) * math.sin(t / t_lead * math.pi)) + c_color = rl.Color(data.color.r, data.color.g, data.color.b, max(0, min(255, alpha))) + + rl.draw_line_ex(rl.Vector2(int(lx), int(cy_t + chevron_w * 0.5)), rl.Vector2(int(cx_t), int(cy_t)), chevron_thick, c_color) + rl.draw_line_ex(rl.Vector2(int(rx), int(cy_t + chevron_w * 0.5)), rl.Vector2(int(cx_t), int(cy_t)), chevron_thick, c_color) - # C. Draw animating chevrons flowing down the road center path - 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) - - # Calculate local tangent vector for rotation using model path - t_next = min(1.0, t + 0.05) - if data.indicator_type == "road_curve" and model: - y_t = self._get_model_offset(t, model) - abs_yt = abs(y_t) - sign_t = math.copysign(1.0, y_t) if y_t != 0.0 else 1.0 - cx_t = icx + sign_t * (abs_yt ** 0.6) * 10.0 * t * math.sqrt(t) - - y_next = self._get_model_offset(t_next, model) - abs_ynext = abs(y_next) - sign_next = math.copysign(1.0, y_next) if y_next != 0.0 else 1.0 - cx_next = icx + sign_next * (abs_ynext ** 0.6) * 10.0 * t_next * math.sqrt(t_next) - - # Clamp both - cx_t = max(icx - half_size, min(icx + half_size, cx_t)) - cx_next = max(icx - half_size, min(icx + half_size, cx_next)) + # Standard chevrons for road_curve, force_stop, and stop_light + is_stop = data.indicator_type in ("force_stop", "stop_light") + if data.indicator_type == "force_stop": + speed_factor = max(0.1, min(1.0, data.indicator_value / 30.0)) + elif data.indicator_type == "stop_light": + speed_factor = max(0.1, min(1.0, data.indicator_value / 30.0)) else: - cx_t = icx - cx_next = icx - - cy_t = bottom - t * 80.0 - cy_next = bottom - t_next * 80.0 - - dx = cx_next - cx_t - dy = cy_next - cy_t - len_v = math.sqrt(dx**2 + dy**2) - if len_v > 0.001: - dir_up_x = dx / len_v - dir_up_y = dy / len_v - else: - dir_up_x = 0.0 - dir_up_y = -1.0 - - dir_right_x = -dir_up_y - dir_right_y = dir_up_x - - # Perspective scaling of chevrons - chevron_w = 14.0 - t * 6.0 - chevron_h = chevron_w * 0.6 - chevron_thick = max(2.0, 4.0 - t * 2.0) - - # Vertices - lx = cx_t - dir_right_x * chevron_w + dir_up_x * chevron_h - ly = cy_t - dir_right_y * chevron_w + dir_up_y * chevron_h - rx = cx_t + dir_right_x * chevron_w + dir_up_x * chevron_h - ry = cy_t + dir_right_y * chevron_w + dir_up_y * chevron_h - - # Alpha envelope fading in at horizon, bright in middle, fading out at bottom - alpha_factor = math.sin(t * math.pi) - alpha = int(data.color.a * alpha_factor) - chev_color = rl.Color(data.color.r, data.color.g, data.color.b, alpha) - chev_shadow = rl.Color(0, 0, 0, int(alpha * 0.5)) - - # Draw chevron with shadow - rl.draw_line_ex(rl.Vector2(int(lx), int(ly + 1.5)), rl.Vector2(int(cx_t), int(cy_t + 1.5)), chevron_thick, chev_shadow) - rl.draw_line_ex(rl.Vector2(int(rx), int(ry + 1.5)), rl.Vector2(int(cx_t), int(cy_t + 1.5)), chevron_thick, chev_shadow) - - 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) + speed_factor = 1.0 - # E. Draw approaching stop sign if force_stop + progress = (rl.get_time() * 1.2 * speed_factor) % 1.0 + t_stop = max(0.05, min(1.0, data.indicator_value / 60.0)) if is_stop else 1.0 + + for i in range(3): + if is_stop: + # Flow UPWARD towards the stopping point + t = ((i + progress) / 3.0) * t_stop + if t > t_stop: + continue + else: + # Chevron fraction t (flowing down from 1.0 to 0.0) + t = 1.0 - ((i + progress) / 3.0) + + # Calculate local tangent vector for rotation using model path + t_next = min(1.0, t + 0.05) + if model: + y_t = self._get_model_offset(t, model) + abs_yt = abs(y_t) + sign_t = -math.copysign(1.0, y_t) if y_t != 0.0 else 1.0 + cx_t = icx + sign_t * (abs_yt ** 0.6) * 10.0 * t * math.sqrt(t) + + y_next = self._get_model_offset(t_next, model) + abs_ynext = abs(y_next) + sign_next = -math.copysign(1.0, y_next) if y_next != 0.0 else 1.0 + cx_next = icx + sign_next * (abs_ynext ** 0.6) * 10.0 * t_next * math.sqrt(t_next) + + # Clamp both + cx_t = max(icx - half_size, min(icx + half_size, cx_t)) + cx_next = max(icx - half_size, min(icx + half_size, cx_next)) + else: + cx_t = icx + cx_next = icx + + cy_t = bottom - t * 80.0 + cy_next = bottom - t_next * 80.0 + + dx = cx_next - cx_t + dy = cy_next - cy_t + len_v = math.sqrt(dx**2 + dy**2) + if len_v > 0.001: + dir_up_x = dx / len_v + dir_up_y = dy / len_v + else: + dir_up_x = 0.0 + dir_up_y = -1.0 + + dir_right_x = -dir_up_y + dir_right_y = dir_up_x + + # Perspective scaling of chevrons + chevron_w = 14.0 - t * 6.0 + chevron_h = chevron_w * 0.6 + chevron_thick = max(2.0, 4.0 - t * 2.0) + + # Vertices - if is_stop, chevron tip points upward (wings extend backward) + dir_mult = -1.0 if is_stop else 1.0 + lx = cx_t - dir_right_x * chevron_w + dir_mult * dir_up_x * chevron_h + ly = cy_t - dir_right_y * chevron_w + dir_mult * dir_up_y * chevron_h + rx = cx_t + dir_right_x * chevron_w + dir_mult * dir_up_x * chevron_h + ry = cy_t + dir_right_y * chevron_w + dir_mult * dir_up_y * chevron_h + + # Alpha envelope: fade out near limits + if is_stop: + alpha_factor = math.sin(t / t_stop * math.pi) + else: + alpha_factor = math.sin(t * math.pi) + alpha = int(data.color.a * alpha_factor) + chev_color = rl.Color(data.color.r, data.color.g, data.color.b, alpha) + chev_shadow = rl.Color(0, 0, 0, int(alpha * 0.5)) + + # Draw chevron with shadow + rl.draw_line_ex(rl.Vector2(int(lx), int(ly + 1.5)), rl.Vector2(int(cx_t), int(cy_t + 1.5)), chevron_thick, chev_shadow) + rl.draw_line_ex(rl.Vector2(int(rx), int(ry + 1.5)), rl.Vector2(int(cx_t), int(cy_t + 1.5)), chevron_thick, chev_shadow) + + 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 overhead traffic light if stop_light + if data.indicator_type == "stop_light": + t_light = max(0.0, min(1.0, data.indicator_value / 60.0)) + s_light = 1.0 - t_light + + if model: + path_y = self._get_model_offset(t_light, model) + abs_y = abs(path_y) + # Invert path_y sign because in openpilot y is positive-left, but on screen x is positive-right. + sign = -math.copysign(1.0, path_y) if path_y != 0.0 else 1.0 + offset_light = sign * (abs_y ** 0.6) * 10.0 * t_light * math.sqrt(t_light) + offset_light = max(-half_size, min(half_size, offset_light)) + else: + offset_light = 0.0 + + cx_light = icx + offset_light + + # 3D perspective translation matching the stop sign vertical translation + y_sign_light = (icy - 30.0) + (s_light ** 1.5) * 50.0 + scale_light = 0.6 + (s_light ** 2.0) * 1.4 + + cy_light = y_sign_light - 15.0 * scale_light + width = 11.0 * scale_light + height = 27.0 * scale_light + + # Shadow + rl.draw_rectangle_rounded(rl.Rectangle(cx_light - width/2, cy_light - height/2 + 1.5, width, height), 0.2, 4, rl.Color(0, 0, 0, 120)) + # Housing + rect_housing = rl.Rectangle(cx_light - width/2, cy_light - height/2, width, height) + rl.draw_rectangle_rounded(rect_housing, 0.2, 4, rl.Color(22, 22, 22, 255)) + rl.draw_rectangle_rounded_lines_ex(rect_housing, 0.2, 4, 1.0, rl.Color(80, 80, 80, 255)) + + r_bulb = 2.2 * scale_light + y_red = cy_light - 7.5 * scale_light + y_yellow = cy_light + y_green = cy_light + 7.5 * scale_light + + active_light = data.indicator_extra if data.indicator_extra in ("red", "yellow", "green") else "red" + + # Pulse halo glow for active light + if active_light == "red": + glow_pulse = 0.5 + 0.5 * math.sin(rl.get_time() * 5.0) + rl.draw_circle(int(cx_light), int(y_red), int(r_bulb + 3.0 * glow_pulse), rl.Color(255, 30, 60, 45)) + rl.draw_circle(int(cx_light), int(y_red), int(r_bulb + 6.0 * glow_pulse), rl.Color(255, 30, 60, 15)) + + c_red = rl.Color(255, 30, 60, 255) if active_light == "red" else rl.Color(50, 10, 15, 255) + c_yellow = rl.Color(255, 200, 0, 255) if active_light == "yellow" else rl.Color(50, 40, 0, 255) + c_green = rl.Color(0, 255, 100, 255) if active_light == "green" else rl.Color(0, 40, 15, 255) + + rl.draw_circle(int(cx_light), int(y_red), int(r_bulb), c_red) + rl.draw_circle(int(cx_light), int(y_yellow), int(r_bulb), c_yellow) + rl.draw_circle(int(cx_light), int(y_green), int(r_bulb), c_green) + + # G. 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) + self._draw_approaching_stop_sign(icx, icy, bottom, data.indicator_value, model, font_bold) # 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) @@ -298,10 +661,30 @@ 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, 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): + def _draw_approaching_stop_sign(self, cx: float, icy: float, bottom: float, distance: float, model, font_bold: rl.Font): # Normalized progress: d_max = 60.0 meters d_max = 60.0 - s = max(0.0, min(1.0, (d_max - distance) / d_max)) + raw_s = max(0.0, min(1.0, (d_max - distance) / d_max)) + # EMA smoother: interpolates between data updates (20Hz) for fluid sub-pixel motion. + # Large discontinuities (new stop sign) reset instantly. + if abs(raw_s - self._stop_smooth_s) > 0.5: + self._stop_smooth_s = raw_s + self._stop_smooth_s += (raw_s - self._stop_smooth_s) * 0.25 + s = self._stop_smooth_s + + # 3D perspective depth coordinate, derived from smoothed s for consistency + t_stop = 1.0 - s + + if model: + path_y = self._get_model_offset(t_stop, model) + abs_y = abs(path_y) + sign = -math.copysign(1.0, path_y) if path_y != 0.0 else 1.0 + offset_stop = sign * (abs_y ** 0.6) * 10.0 * t_stop * math.sqrt(t_stop) + offset_stop = max(-40.0, min(40.0, offset_stop)) + else: + offset_stop = 0.0 + + cx_stop = cx + offset_stop # 3D perspective translation (stays within the road boundary) y_sign = (icy - 30.0) + (s ** 1.5) * 50.0 @@ -311,36 +694,29 @@ class AetherGauge: 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)) + # Shadow alpha scales with sign size (fainter at distance), position uses raw floats + shadow_alpha = int(min(120, r_sign * 12)) + rl.draw_poly(rl.Vector2(cx_stop, y_sign + 2.0), 8, r_sign, 22.5, rl.Color(0, 0, 0, shadow_alpha)) - # 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 + # Draw red octagon body (always visible at full opacity) 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) + rl.draw_poly(rl.Vector2(cx_stop, y_sign), 8, r_sign, 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) + # Proportional white outline — thickness and alpha both scale with sign size. + # At distance (r=6): barely visible hint of an edge. + # Up close (r=20): crisp outline at full alpha. + outline_t = max(0.8, min(1.5, r_sign * 0.07)) + outline_a = int(max(0, min(200, (r_sign - 5.0) * 20))) + if outline_a > 20: + for i in range(8): + a1 = math.radians(22.5 + i * 45.0) + a2 = math.radians(22.5 + ((i + 1) % 8) * 45.0) + p1 = rl.Vector2(cx_stop + r_sign * math.cos(a1), y_sign + r_sign * math.sin(a1)) + p2 = rl.Vector2(cx_stop + r_sign * math.cos(a2), y_sign + r_sign * math.sin(a2)) + rl.draw_line_ex(p1, p2, outline_t, rl.Color(255, 255, 255, outline_a)) + + # Draw "STOP" text centered on the sign face (always visible, sized to fit within octagon) + stop_font_size = max(4, int(r_sign * 0.7)) + stop_txt_size = measure_text_cached(font_bold, "STOP", stop_font_size) + stop_pos = rl.Vector2(cx_stop - stop_txt_size.x / 2, y_sign - stop_txt_size.y / 2) + rl.draw_text_ex(font_bold, "STOP", stop_pos, stop_font_size, 0, rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/stopping_point.py b/selfdrive/ui/onroad/starpilot/stopping_point.py index 89cacbf34..eb823f01c 100644 --- a/selfdrive/ui/onroad/starpilot/stopping_point.py +++ b/selfdrive/ui/onroad/starpilot/stopping_point.py @@ -1,7 +1,18 @@ +import math import pyray as rl from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.text_measure import measure_text_cached + +def _draw_poly_outline(cx: float, cy: float, sides: int, radius: float, rotation: float, thickness: float, color: rl.Color): + """Draw a clean polygon outline as connected line segments.""" + for i in range(sides): + a1 = math.radians(rotation + i * 360.0 / sides) + a2 = math.radians(rotation + ((i + 1) % sides) * 360.0 / sides) + p1 = rl.Vector2(int(cx + radius * math.cos(a1)), int(cy + radius * math.sin(a1))) + p2 = rl.Vector2(int(cx + radius * math.cos(a2)), int(cy + radius * math.sin(a2))) + rl.draw_line_ex(p1, p2, thickness, color) + def render_stopping_point(renderer, font): params = ui_state.params if not params.get_bool("ShowStoppingPoint"): @@ -30,10 +41,10 @@ def render_stopping_point(renderer, font): # Draw programmatic stop sign (octagon) radius = 35.0 - # Draw white outer octagon - rl.draw_poly(rl.Vector2(int(cx), int(cy - radius)), 8, radius, 22.5, rl.WHITE) - # Draw red inner octagon - rl.draw_poly(rl.Vector2(int(cx), int(cy - radius)), 8, radius - 4, 22.5, rl.Color(196, 30, 58, 255)) + # Draw red octagon body + rl.draw_poly(rl.Vector2(int(cx), int(cy - radius)), 8, radius, 22.5, rl.Color(196, 30, 58, 255)) + # Draw clean thin white outline (instead of a chunky filled white border) + _draw_poly_outline(cx, cy - radius, 8, radius, 22.5, 1.5, rl.Color(255, 255, 255, 200)) # Draw "STOP" text centered in octagon font_size = 18