From 65b81c5710d8d061a00e334ef6dd65cd6b54a221 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Fri, 3 Apr 2026 09:57:23 -0400 Subject: [PATCH] BigUI WIP: Adj Path Bugfixes --- .../ui/layouts/settings/starpilot/visuals.py | 56 +++++ selfdrive/ui/onroad/model_renderer.py | 217 ++++++++++++++++- selfdrive/ui/onroad/starpilot/path.py | 219 ++++++++++++++++++ .../onroad/starpilot/starpilot_onroad_view.py | 23 ++ 4 files changed, 506 insertions(+), 9 deletions(-) create mode 100644 selfdrive/ui/onroad/starpilot/path.py diff --git a/selfdrive/ui/layouts/settings/starpilot/visuals.py b/selfdrive/ui/layouts/settings/starpilot/visuals.py index cf3ea066..2ff9c4b8 100644 --- a/selfdrive/ui/layouts/settings/starpilot/visuals.py +++ b/selfdrive/ui/layouts/settings/starpilot/visuals.py @@ -212,6 +212,15 @@ class StarPilotVisualWidgetsLayout(StarPilotPanel): "icon": "toggle_icons/icon_road.png", "color": "#8B5CF6", }, + { + "title": tr_noop("Adjacent Lane Metrics"), + "type": "toggle", + "key": "AdjacentPathMetrics", + "get_state": lambda: self._params.get_bool("AdjacentPathMetrics"), + "set_state": lambda s: self._params.put_bool("AdjacentPathMetrics", s), + "icon": "toggle_icons/icon_road.png", + "color": "#8B5CF6", + }, { "title": tr_noop("Blind Spot Path"), "type": "toggle", @@ -337,6 +346,15 @@ class StarPilotModelUILayout(StarPilotPanel): "icon": "toggle_icons/icon_road.png", "color": "#8B5CF6", }, + { + "title": tr_noop("Lane Line Color"), + "type": "value", + "key": "LaneLinesColor", + "get_value": lambda: self._get_color_display("LaneLinesColor"), + "on_click": lambda: self._show_color_selector("LaneLinesColor"), + "icon": "toggle_icons/icon_road.png", + "color": "#8B5CF6", + }, { "title": tr_noop("Path Edge Width"), "type": "value", @@ -346,6 +364,15 @@ class StarPilotModelUILayout(StarPilotPanel): "icon": "toggle_icons/icon_road.png", "color": "#8B5CF6", }, + { + "title": tr_noop("Path Edge Color"), + "type": "value", + "key": "PathEdgesColor", + "get_value": lambda: self._get_color_display("PathEdgesColor"), + "on_click": lambda: self._show_color_selector("PathEdgesColor"), + "icon": "toggle_icons/icon_road.png", + "color": "#8B5CF6", + }, { "title": tr_noop("Path Width"), "type": "value", @@ -355,6 +382,15 @@ class StarPilotModelUILayout(StarPilotPanel): "icon": "toggle_icons/icon_road.png", "color": "#8B5CF6", }, + { + "title": tr_noop("Path Color"), + "type": "value", + "key": "PathColor", + "get_value": lambda: self._get_color_display("PathColor"), + "on_click": lambda: self._show_color_selector("PathColor"), + "icon": "toggle_icons/icon_road.png", + "color": "#8B5CF6", + }, { "title": tr_noop("Road Edge Width"), "type": "value", @@ -423,6 +459,26 @@ class StarPilotModelUILayout(StarPilotPanel): gui_app.set_modal_overlay(AetherSliderDialog(tr(key), min_v, max_v, step, current, on_close, unit=unit, color="#8B5CF6")) + def _get_color_display(self, key): + val = self._params.get(key, encoding='utf-8') or "" + if not val: + return "Stock" + return val.upper() + + def _show_color_selector(self, key): + presets = ["Stock", "#FFFFFF", "#178644", "#3B82F6", "#E63956", "#8B5CF6", "#F59E0B"] + current = self._params.get(key, encoding='utf-8') or "Stock" + + def on_select(res, val): + if res == DialogResult.CONFIRM: + if val == "Stock": + self._params.remove(key) + else: + self._params.put(key, val) + self._rebuild_grid() + + gui_app.set_modal_overlay(SelectionDialog(tr(key), presets, current, on_close=on_select)) + class StarPilotNavigationVisualsLayout(StarPilotPanel): def __init__(self): diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 7d130196..5818e8b3 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -6,7 +6,7 @@ from dataclasses import dataclass, field from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT -from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient from openpilot.system.ui.widgets import Widget @@ -53,6 +53,15 @@ class ModelRenderer(Widget): self._lead_vehicles = [LeadVehicle(), LeadVehicle()] self._path_offset_z = HEIGHT_INIT[0] + # Adjacent path vertices (left, right) + self._adjacent_path_vertices = [np.empty((0, 2), dtype=np.float32), np.empty((0, 2), dtype=np.float32)] + # Outer path polygon for edge rendering + self._track_edge_vertices = np.empty((0, 2), dtype=np.float32) + # Rainbow animation state + self._rainbow_hue_offset = 0.0 + # Cached speed for rainbow animation + self._speed = 0.0 + # Initialize ModelPoints objects self._path = ModelPoints() self._lane_lines = [ModelPoints() for _ in range(4)] @@ -72,7 +81,8 @@ class ModelRenderer(Widget): ) # Get longitudinal control setting from car parameters - if car_params := Params().get("CarParams"): + self._params = Params() + if car_params := self._params.get("CarParams"): cp = messaging.log_from_bytes(car_params, car.CarParams) self._longitudinal_control = cp.openpilotLongitudinalControl @@ -95,6 +105,7 @@ class ModelRenderer(Widget): # Update state self._experimental_mode = sm['selfdriveState'].experimentalMode + self._speed = sm['carState'].vEgo live_calib = sm['liveCalibration'] self._path_offset_z = live_calib.height[0] if live_calib.height else HEIGHT_INIT[0] @@ -174,18 +185,44 @@ class ModelRenderer(Widget): def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" + path_width = self._params.get_float('PathWidth') # stored in feet, convert to meters + if path_width <= 0: + path_width = 0.9 # default in meters + + lane_line_width = self._params.get_int('LaneLinesWidth') # stored in inches, convert to meters + if lane_line_width <= 0: + lane_line_width = int(0.025 / 0.0254) # default ~1 inch + lane_line_width_m = lane_line_width * 0.0254 + + road_edge_width = self._params.get_int('RoadEdgesWidth') # stored in inches + if road_edge_width <= 0: + road_edge_width = int(0.025 / 0.0254) + road_edge_width_m = road_edge_width * 0.0254 + + path_edge_width_pct = self._params.get_int('PathEdgeWidth') / 100.0 + + # Dynamic path width + if self._params.get_bool('DynamicPathWidth'): + status = ui_state.status + if status == UIStatus.ENGAGED: + path_width *= 1.0 + elif status == UIStatus.ALWAYS_ON_LATERAL: + path_width *= 0.75 + else: + path_width *= 0.50 + max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance) # Update lane lines using raw points for i, lane_line in enumerate(self._lane_lines): lane_line.projected_points = self._map_line_to_polygon( - lane_line.raw_points, 0.025 * self._lane_line_probs[i], 0.0, max_idx, max_distance + lane_line.raw_points, lane_line_width_m * self._lane_line_probs[i], 0.0, max_idx, max_distance ) # Update road edges using raw points for road_edge in self._road_edges: - road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, 0.025, 0.0, max_idx, max_distance) + road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, road_edge_width_m, 0.0, max_idx, max_distance) # Update path using raw points if lead and lead.status: @@ -194,14 +231,25 @@ class ModelRenderer(Widget): max_idx = self._get_path_length_idx(path_x_array, max_distance) self._path.projected_points = self._map_line_to_polygon( - self._path.raw_points, 0.9, self._path_offset_z, max_idx, max_distance, allow_invert=False + self._path.raw_points, path_width * (1 - path_edge_width_pct), self._path_offset_z, max_idx, max_distance, allow_invert=False ) + # Compute track edge vertices (outer path polygon at full width) + self._track_edge_vertices = self._map_line_to_polygon( + self._path.raw_points, path_width, self._path_offset_z, max_idx, max_distance, allow_invert=False + ) + + # Compute adjacent path vertices + self._update_adjacent_paths(max_idx, max_distance) + self._update_experimental_gradient() def _update_experimental_gradient(self): """Pre-calculate experimental mode gradient colors""" - if not self._experimental_mode: + use_acceleration = self._experimental_mode or self._params.get_bool('AccelerationPath') + use_rainbow = self._params.get_bool('RainbowPath') and not use_acceleration + + if not use_acceleration and not use_rainbow: return max_len = min(len(self._path.projected_points) // 2, len(self._acceleration_x)) @@ -220,6 +268,21 @@ class ModelRenderer(Widget): # Calculate color based on acceleration (0 is bottom, 1 is top) lin_grad_point = 1 - (track_y - self._rect.y) / self._rect.height + if use_rainbow: + # Rainbow: hue based on gradient position + animated offset + self._rainbow_hue_offset += self._speed * 0.02 + if self._rainbow_hue_offset >= 360.0: + self._rainbow_hue_offset = self._rainbow_hue_offset % 360.0 + path_hue = (lin_grad_point * 120.0 + self._rainbow_hue_offset) % 360.0 + saturation = 1.0 + lightness = 0.5 + alpha = np.interp(lin_grad_point, [0.0, 1.0], [0.5, 0.1]) + color = self._hsla_to_color(path_hue / 360.0, saturation, lightness, alpha) + gradient_stops.append(lin_grad_point) + segment_colors.append(color) + i += 1 + (1 if (i + 2) < max_len else 0) + continue + # speed up: 120, slow down: 0 path_hue = np.clip(60 + self._acceleration_x[i] * 35, 0, 120) @@ -270,12 +333,19 @@ class ModelRenderer(Widget): def _draw_lane_lines(self): """Draw lane lines and road edges""" + color_scheme = self._params.get('ColorScheme', encoding='utf-8') or 'stock' + lane_lines_color = self._params.get('LaneLinesColor', encoding='utf-8') + for i, lane_line in enumerate(self._lane_lines): if lane_line.projected_points.size == 0: continue alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7) - color = rl.Color(255, 255, 255, int(alpha * 255)) + if color_scheme != 'stock' and lane_lines_color: + base_color = self._hex_to_color(lane_lines_color) + color = rl.Color(base_color.r, base_color.g, base_color.b, int(alpha * 255)) + else: + color = rl.Color(255, 255, 255, int(alpha * 255)) draw_polygon(self._rect, lane_line.projected_points, color) for i, road_edge in enumerate(self._road_edges): @@ -294,8 +364,8 @@ class ModelRenderer(Widget): allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control self._blend_filter.update(int(allow_throttle)) - if self._experimental_mode: - # Draw with acceleration coloring + use_accel_path = self._params.get_bool('AccelerationPath') + if self._experimental_mode or use_accel_path: if len(self._exp_gradient.colors) > 1: draw_polygon(self._rect, self._path.projected_points, gradient=self._exp_gradient) else: @@ -321,6 +391,135 @@ class ModelRenderer(Widget): rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255)) rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha)) + def _update_adjacent_paths(self, max_idx: int, max_distance: float): + """Compute adjacent lane path polygons by averaging lane line pairs.""" + sm = ui_state.sm + if sm.recv_frame.get("starpilotPlan", 0) < ui_state.started_frame: + self._adjacent_path_vertices = [np.empty((0, 2), dtype=np.float32), np.empty((0, 2), dtype=np.float32)] + return + + plan = sm["starpilotPlan"] + lane_width_left = float(plan.laneWidthLeft) if plan.laneWidthLeft > 0 else 0.0 + lane_width_right = float(plan.laneWidthRight) if plan.laneWidthRight > 0 else 0.0 + + if lane_width_left <= 0 or lane_width_right <= 0: + self._adjacent_path_vertices = [np.empty((0, 2), dtype=np.float32), np.empty((0, 2), dtype=np.float32)] + return + + # Left adjacent: average of lane_lines[0] and lane_lines[1] + self._adjacent_path_vertices[0] = self._map_averaged_line_to_polygon( + self._lane_lines[0].raw_points, self._lane_lines[1].raw_points, + lane_width_left / 2.0, 0.0, max_idx, max_distance + ) + + # Right adjacent: average of lane_lines[2] and lane_lines[3] + self._adjacent_path_vertices[1] = self._map_averaged_line_to_polygon( + self._lane_lines[2].raw_points, self._lane_lines[3].raw_points, + lane_width_right / 2.0, 0.0, max_idx, max_distance + ) + + def _map_averaged_line_to_polygon(self, line1: np.ndarray, line2: np.ndarray, y_off: float, z_off: float, max_idx: int, max_distance: float) -> np.ndarray: + """Convert averaged 3D line pair to 2D polygon for adjacent path rendering. + + Averages the Y coordinates of two lane lines, uses X from line1, Z from line1. + Then projects to screen space like _map_line_to_polygon. + Grounds the path to the bottom of the screen. + """ + if line1.shape[0] == 0 or line2.shape[0] == 0: + return np.empty((0, 2), dtype=np.float32) + + # Use the shorter of the two lines + min_len = min(line1.shape[0], line2.shape[0], max_idx + 1) + if min_len == 0: + return np.empty((0, 2), dtype=np.float32) + + # Average Y, use X from line1, Z from line1 + avg_x = line1[:min_len, 0] + avg_y = (line1[:min_len, 1] + line2[:min_len, 1]) / 2.0 + avg_z = line1[:min_len, 2] + + # Filter non-negative x + valid = avg_x >= 0 + if not np.any(valid): + return np.empty((0, 2), dtype=np.float32) + + points = np.column_stack([avg_x[valid], avg_y[valid], avg_z[valid]]) + N = points.shape[0] + + # Generate left and right 3D points + offsets = np.array([[0, -y_off, z_off], [0, y_off, z_off]], dtype=np.float32) + points_3d = points[None, :, :] + offsets[:, None, :] + points_3d = points_3d.reshape(2 * N, 3) + + # Transform + proj = self._car_space_transform @ points_3d.T + proj = proj.reshape(3, 2, N) + left_proj = proj[:, 0, :] + right_proj = proj[:, 1, :] + + # Filter valid z + valid_proj = (np.abs(left_proj[2]) >= 1e-6) & (np.abs(right_proj[2]) >= 1e-6) + if not np.any(valid_proj): + return np.empty((0, 2), dtype=np.float32) + + left_screen = left_proj[:2, valid_proj] / left_proj[2, valid_proj][None, :] + right_screen = right_proj[:2, valid_proj] / right_proj[2, valid_proj][None, :] + + # Clip region filter + clip = self._clip_region + x_min, x_max = clip.x, clip.x + clip.width + y_min, y_max = clip.y, clip.y + clip.height + + left_in_clip = (left_screen[0] >= x_min) & (left_screen[0] <= x_max) & (left_screen[1] >= y_min) & (left_screen[1] <= y_max) + right_in_clip = (right_screen[0] >= x_min) & (right_screen[0] <= x_max) & (right_screen[1] >= y_min) & (right_screen[1] <= y_max) + both_in_clip = left_in_clip & right_in_clip + + if not np.any(both_in_clip): + return np.empty((0, 2), dtype=np.float32) + + left_screen = left_screen[:, both_in_clip] + right_screen = right_screen[:, both_in_clip] + + # No inversion check for adjacent paths (allow_invert=True equivalent) + polygon = np.vstack((left_screen.T, right_screen[:, ::-1].T)).astype(np.float32) + + # Ground to bottom of screen + if polygon.shape[0] >= 4: + height = self._rect.y + self._rect.height + + # Extend left_near (index 0) using slope from left_near → left_2nd + p0 = polygon[0] + p1 = polygon[1] + dy = p0[1] - p1[1] + if abs(dy) > 0.1: + slope = (p0[0] - p1[0]) / dy + p0[0] = p0[0] + (height - p0[1]) * slope + p0[1] = height + + # Extend right_near (index -1) using slope from right_near → right_2nd + p0 = polygon[-1] + p1 = polygon[-2] + dy = p0[1] - p1[1] + if abs(dy) > 0.1: + slope = (p0[0] - p1[0]) / dy + p0[0] = p0[0] + (height - p0[1]) * slope + p0[1] = height + + return polygon + + @staticmethod + def _hex_to_color(hex_str: str) -> rl.Color: + """Convert hex color string to rl.Color.""" + hex_str = hex_str.lstrip('#') + if len(hex_str) == 6: + return rl.Color( + int(hex_str[0:2], 16), + int(hex_str[2:4], 16), + int(hex_str[4:6], 16), + 255 + ) + return rl.Color(255, 255, 255, 255) + @staticmethod def _get_path_length_idx(pos_x_array: np.ndarray, path_distance: float) -> int: """Get the index corresponding to the given path distance""" diff --git a/selfdrive/ui/onroad/starpilot/path.py b/selfdrive/ui/onroad/starpilot/path.py new file mode 100644 index 00000000..f0b60b97 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/path.py @@ -0,0 +1,219 @@ +from __future__ import annotations + +import colorsys + +import numpy as np +import pyray as rl + +from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient + +_METRICS_FONT = None +_METRICS_FONT_SIZE = 45 + + +def _get_metrics_font(): + global _METRICS_FONT + if _METRICS_FONT is None or _METRICS_FONT.baseSize != _METRICS_FONT_SIZE: + if _METRICS_FONT is not None: + rl.unload_font(_METRICS_FONT) + _METRICS_FONT = rl.load_font_ex("fonts/Inter-SemiBold.ttf", _METRICS_FONT_SIZE, None, 256) + return _METRICS_FONT + + +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 _make_gradient(hue: float) -> Gradient: + return Gradient( + start=(0.0, 1.0), + end=(0.0, 0.0), + colors=[ + _hsla_to_color(hue, 0.75, 0.5, 0.4), + _hsla_to_color(hue, 0.75, 0.5, 0.35), + _hsla_to_color(hue, 0.75, 0.5, 0.0), + ], + stops=[0.0, 0.5, 1.0], + ) + + +def _draw_text_with_outline(text: str, x: float, y: float, font, font_size: int): + pos = rl.Vector2(x, y) + rl.draw_text_ex(font, text, rl.Vector2(pos.x - 1, pos.y - 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, rl.Vector2(pos.x + 1, pos.y - 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, rl.Vector2(pos.x - 1, pos.y + 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, rl.Vector2(pos.x + 1, pos.y + 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, pos, font_size, 0, rl.WHITE) + + +def render_adjacent_paths(renderer) -> None: + """Draw left and right adjacent lane paths with gradient coloring. + + Qt reference: paintAdjacentPaths in starpilot_annotated_camera.cc:337-392 + + Gradient color is based on lane width ratio compared to LaneDetectionWidth toggle. + hue = (ratio^2) * (120/360), where ratio = laneWidth / lane_detection_width + Gradient: HSL(hue, 0.75, 0.5, 0.4) at bottom, 0.35 at mid, 0.0 at top. + + If AdjacentPathMetrics is enabled, show lane width text on each path. + """ + sm = ui_state.sm + if sm.recv_frame["starpilotPlan"] < ui_state.started_frame: + return + + plan = sm["starpilotPlan"] + lane_width_left = float(plan.laneWidthLeft) + lane_width_right = float(plan.laneWidthRight) + lane_detection_width = renderer._params.get_float("LaneDetectionWidth") if renderer._params.get("LaneDetectionWidth") else 3.5 + + vertices = renderer._adjacent_path_vertices + if not vertices or len(vertices) < 2: + return + + show_metrics = renderer._params.get_bool("AdjacentPathMetrics") + rect = renderer._rect + + distance_conversion = 3.28084 if not ui_state.is_metric else 1.0 + unit = "ft" if not ui_state.is_metric else "m" + font = _get_metrics_font() + + for i, (verts, lane_width) in enumerate(zip(vertices, [lane_width_left, lane_width_right], strict=True)): + if verts.size < 4 or lane_width == 0.0: + continue + + ratio = np.clip(lane_width / lane_detection_width, 0.0, 1.0) + hue = (ratio * ratio) * (120.0 / 360.0) + gradient = _make_gradient(hue) + draw_polygon(rect, verts, gradient=gradient) + + if show_metrics: + is_left = i == 0 + mid_index = len(verts) // 2 + anchor_idx = mid_index // 2 if is_left else mid_index + (len(verts) - mid_index) // 2 + anchor = verts[anchor_idx] + + text = f"{lane_width * distance_conversion:.2f}{unit}" + text_width = rl.measure_text_ex(font, text, _METRICS_FONT_SIZE, 0).x + text_height = rl.measure_text_ex(font, text, _METRICS_FONT_SIZE, 0).y + + text_x = anchor[0] - text_width if is_left else anchor[0] + text_y = anchor[1] - text_height / 2 + text_height * 0.75 + _draw_text_with_outline(text, text_x, text_y, font, _METRICS_FONT_SIZE) + + +def render_blind_spot_path(renderer) -> None: + """Draw red gradient overlay on adjacent lanes when vehicle detected in blind spot. + + Qt reference: paintBlindSpotPath in starpilot_annotated_camera.cc:394-411 + + Red gradient: HSL(0, 0.75, 0.5, 0.4) at bottom, 0.35 at mid, 0.0 at top. + Only draws on the side where blind spot is active. + """ + sm = ui_state.sm + if sm.recv_frame["carState"] < ui_state.started_frame: + return + + if not renderer._params.get_bool("BlindSpotPath"): + return + + car_state = sm["carState"] + blindspot_left = bool(car_state.leftBlindspot) + blindspot_right = bool(car_state.rightBlindspot) + + if not blindspot_left and not blindspot_right: + return + + vertices = renderer._adjacent_path_vertices + if not vertices or len(vertices) < 2: + return + + gradient = _make_gradient(0.0) + rect = renderer._rect + + if blindspot_left and vertices[0].size >= 4: + draw_polygon(rect, vertices[0], gradient=gradient) + + if blindspot_right and vertices[1].size >= 4: + draw_polygon(rect, vertices[1], gradient=gradient) + + +def render_path_edges(renderer) -> None: + """Draw colored edge strips along the driving path. + + Qt reference: paintPathEdges in starpilot_annotated_camera.cc:732-769 + + Path edges are the area between track_edge_vertices (outer) and track_vertices (inner). + Color is based on current mode: + - If switchback_mode_enabled: use STATUS_SWITCHBACK_MODE_ENABLED color + - If always_on_lateral_active: use STATUS_ALWAYS_ON_LATERAL_ACTIVE color + - If conditional_status == 1: use STATUS_CEM_DISABLED color + - If experimental_mode: use STATUS_EXPERIMENTAL_MODE_ENABLED color + - If traffic_mode_enabled: use STATUS_TRAFFIC_MODE_ENABLED color + - If color_scheme != "stock" and path_edges_color is set: use that color + - Else: stock green gradient HSL(148/360, 0.94, 0.41, 0.4) → HSL(112/360, 1.0, 0.54, 0.35) → transparent + + The path_edges_color param is a hex string like "#178644". + """ + outer = renderer._track_edge_vertices + inner = renderer._path.projected_points + + if outer.size < 4 or inner.size < 4: + return + + edge_strip = np.vstack([outer, inner[::-1]]) + + if ui_state.switchback_mode_enabled: + base_color = rl.Color(139, 108, 197, 241) + elif ui_state.always_on_lateral_active: + base_color = rl.Color(10, 186, 181, 241) + elif ui_state.conditional_status == 1: + base_color = rl.Color(255, 255, 0, 241) + elif renderer._experimental_mode: + base_color = rl.Color(218, 111, 37, 241) + elif ui_state.traffic_mode_enabled: + base_color = rl.Color(201, 34, 49, 241) + else: + color_scheme = renderer._params.get("ColorScheme") + if color_scheme and color_scheme.decode("utf-8") != "stock": + path_edges_color = renderer._params.get("PathEdgesColor") + if path_edges_color: + hex_str = path_edges_color.decode("utf-8").lstrip("#") + if len(hex_str) == 6: + r = int(hex_str[0:2], 16) + g = int(hex_str[2:4], 16) + b = int(hex_str[4:6], 16) + base_color = rl.Color(r, g, b, 241) + else: + base_color = rl.Color(23, 134, 68, 241) + else: + base_color = rl.Color(23, 134, 68, 241) + else: + base_color = None + + if base_color is not None: + gradient = Gradient( + start=(0.0, 1.0), + end=(0.0, 0.0), + colors=[ + rl.Color(base_color.r, base_color.g, base_color.b, int(255 * 0.4)), + rl.Color(base_color.r, base_color.g, base_color.b, int(255 * 0.35)), + rl.Color(base_color.r, base_color.g, base_color.b, 0), + ], + stops=[0.0, 0.5, 1.0], + ) + else: + gradient = Gradient( + start=(0.0, 1.0), + end=(0.0, 0.0), + colors=[ + _hsla_to_color(148.0 / 360.0, 0.94, 0.41, 0.4), + _hsla_to_color(112.0 / 360.0, 1.0, 0.54, 0.35), + _hsla_to_color(112.0 / 360.0, 1.0, 0.54, 0.0), + ], + stops=[0.0, 0.5, 1.0], + ) + + draw_polygon(renderer._rect, edge_strip, gradient=gradient) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index e798a68b..d8e5af45 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -4,6 +4,7 @@ from openpilot.common.params import Params from openpilot.selfdrive.ui import UI_BORDER_SIZE from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView, BORDER_COLORS from openpilot.selfdrive.ui.onroad.starpilot.curve_speed_border import render_glow, render_filament +from openpilot.selfdrive.ui.onroad.starpilot.path import render_adjacent_paths, render_blind_spot_path, render_path_edges from openpilot.selfdrive.ui.onroad.starpilot.personality_button import PersonalityButton, BTN_SIZE from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import ( render_speed_limit, handle_slc_click, SET_SPEED_X_OFFSET, SET_SPEED_Y_OFFSET, @@ -30,6 +31,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._render_slc(rect) self._render_overlays() + self._render_path_features(rect) def _render_slc(self, rect: rl.Rectangle): content_rect = rl.Rectangle( @@ -47,6 +49,27 @@ class StarPilotOnroadView(AugmentedRoadView): self._position_personality_button() self._personality_button.render() + def _render_path_features(self, rect: rl.Rectangle): + """Render path-related features (adjacent paths, blind spot, path edges).""" + mr = self.model_renderer + + # Only render if we have path data + if not mr._path.projected_points.size: + return + + # Path edges (always rendered if track_edge_vertices exist) + if mr._track_edge_vertices.size >= 4: + render_path_edges(mr) + + # Adjacent paths or blind spot path (mutually exclusive, matching Qt behavior) + adjacent_enabled = self._params.get_bool("AdjacentPath") + blind_spot_enabled = self._params.get_bool("BlindSpotPath") + + if adjacent_enabled and mr._adjacent_path_vertices[0].size >= 4: + render_adjacent_paths(mr) + elif blind_spot_enabled and mr._adjacent_path_vertices[0].size >= 4: + render_blind_spot_path(mr) + def _position_personality_button(self): dm = self.driver_state_renderer toggle_on = self._params.get_bool("OnroadDistanceButton")