From feba46db306063846a40af5b793bfce67b955809 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Tue, 26 May 2026 00:56:31 -0400 Subject: [PATCH] BigUI WIP: Onroad Path Cleanup --- selfdrive/ui/onroad/model_renderer.py | 84 +++--------- selfdrive/ui/onroad/starpilot/path.py | 126 ++++++++---------- .../onroad/starpilot/starpilot_onroad_view.py | 12 +- 3 files changed, 79 insertions(+), 143 deletions(-) diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 301af89ae..17eeadfb5 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -225,20 +225,21 @@ class ModelRenderer(Widget): 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) + unclipped_max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) + unclipped_max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], unclipped_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, lane_line_width_m * 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, unclipped_max_idx, unclipped_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, road_edge_width_m, 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, unclipped_max_idx, unclipped_max_distance) # Update path using raw points + max_distance = unclipped_max_distance if lead and lead.status: lead_d = lead.dRel * 2.0 max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance) @@ -254,7 +255,7 @@ class ModelRenderer(Widget): ) # Compute adjacent path vertices - self._update_adjacent_paths(max_idx, max_distance) + self._update_adjacent_paths(unclipped_max_idx, unclipped_max_distance) self._update_experimental_gradient() @@ -646,81 +647,34 @@ class ModelRenderer(Widget): return # Left adjacent: average of lane_lines[0] and lane_lines[1] - self._adjacent_path_vertices[0] = self._map_averaged_line_to_polygon( + self._adjacent_path_vertices[0] = self._get_adjacent_path_polygon( self._lane_lines[0].raw_points, self._lane_lines[1].raw_points, - lane_width_left / 2.0, 0.0, max_idx, max_distance + lane_width_left / 2.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._adjacent_path_vertices[1] = self._get_adjacent_path_polygon( self._lane_lines[2].raw_points, self._lane_lines[3].raw_points, - lane_width_right / 2.0, 0.0, max_idx, max_distance + lane_width_right / 2.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. - """ + def _get_adjacent_path_polygon(self, line1: np.ndarray, line2: np.ndarray, y_off: float, max_idx: int, max_distance: float) -> np.ndarray: 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) + min_len = min(line1.shape[0], line2.shape[0]) 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] + # Average Y, use X and Z from line1 + avg_line = np.empty((min_len, 3), dtype=np.float32) + avg_line[:, 0] = line1[:min_len, 0] + avg_line[:, 1] = (line1[:min_len, 1] + line2[:min_len, 1]) / 2.0 + avg_line[:, 2] = 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) + # Map the averaged line to a polygon using the standard path mapper + polygon = self._map_line_to_polygon(avg_line, y_off, 0.0, max_idx, max_distance, allow_invert=True) # Ground to bottom of screen if polygon.shape[0] >= 4: diff --git a/selfdrive/ui/onroad/starpilot/path.py b/selfdrive/ui/onroad/starpilot/path.py index 6c488c512..94b5a3ad0 100644 --- a/selfdrive/ui/onroad/starpilot/path.py +++ b/selfdrive/ui/onroad/starpilot/path.py @@ -43,95 +43,83 @@ def _draw_text_with_outline(text: str, x: float, y: float, font, font_size: int) 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. +def render_adjacent_lanes(renderer) -> None: + """Draw left and right adjacent lane paths. - 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. + Consolidates adjacent width path rendering and blind spot warning overlays. + If a blind spot is active on a side and BlindSpotPath is enabled, that side draws red. + Otherwise, if AdjacentPath is enabled, it draws with width-based gradient coloring. """ sm = ui_state.sm - if sm.recv_frame["starpilotPlan"] < ui_state.started_frame: - return + adjacent_enabled = renderer._params.get_bool("AdjacentPath") + blind_spot_enabled = renderer._params.get_bool("BlindSpotPath") - 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 + if not (adjacent_enabled or blind_spot_enabled): + return vertices = renderer._adjacent_path_vertices if not vertices or len(vertices) < 2: return - show_metrics = renderer._params.get_bool("AdjacentPathMetrics") - rect = renderer._rect + # Fetch blindspot status if blind spot path is enabled + blindspot_left = False + blindspot_right = False + if blind_spot_enabled and sm.recv_frame["carState"] >= ui_state.started_frame: + car_state = sm["carState"] + blindspot_left = bool(car_state.leftBlindspot) + blindspot_right = bool(car_state.rightBlindspot) + # Fetch adjacent lane widths if adjacent path is enabled + lane_width_left = 0.0 + lane_width_right = 0.0 + lane_detection_width = 3.5 + if adjacent_enabled and sm.recv_frame["starpilotPlan"] >= ui_state.started_frame: + 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 + + rect = renderer._rect + show_metrics = renderer._params.get_bool("AdjacentPathMetrics") distance_conversion = 3.28084 if not ui_state.is_metric else 1.0 unit = "ft" if not ui_state.is_metric else "m" font = gui_app.font(FontWeight.SEMI_BOLD) - 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: + sides = [ + (0, blindspot_left, lane_width_left), + (1, blindspot_right, lane_width_right) + ] + + for side_idx, has_blindspot, lane_width in sides: + verts = vertices[side_idx] + if verts.size < 4: 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) + hue = None + if has_blindspot: + hue = 0.0 # Red for blind spot warning + elif adjacent_enabled and lane_width > 0.0: + ratio = np.clip(lane_width / lane_detection_width, 0.0, 1.0) + hue = (ratio * ratio) * (120.0 / 360.0) - if show_metrics: - mid_index = len(verts) // 2 - left = verts[mid_index // 2] - right = verts[mid_index + (len(verts) - mid_index) // 2] + if hue is not None: + gradient = _make_gradient(hue) + draw_polygon(rect, verts, gradient=gradient) - text = f"{lane_width * distance_conversion:.2f}{unit}" - text_sz = measure_text_cached(font, text, _METRICS_FONT_SIZE) - text_width = text_sz.x - text_height = text_sz.y + # Draw width text if adjacent path is enabled on this side + if adjacent_enabled and lane_width > 0.0 and show_metrics: + mid_index = len(verts) // 2 + left = verts[mid_index // 2] + right = verts[mid_index + (len(verts) - mid_index) // 2] - text_x = (left[0] + right[0]) / 2.0 - text_width / 2.0 - text_y = (left[1] + right[1]) / 2.0 - text_height / 2.0 + text_height * 0.75 - _draw_text_with_outline(text, text_x, text_y, font, _METRICS_FONT_SIZE) + text = f"{lane_width * distance_conversion:.2f}{unit}" + text_sz = measure_text_cached(font, text, _METRICS_FONT_SIZE) + text_width = text_sz.x + text_height = text_sz.y - -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) + text_x = (left[0] + right[0]) / 2.0 - text_width / 2.0 + text_y = (left[1] + right[1]) / 2.0 - text_height / 2.0 + text_height * 0.75 + _draw_text_with_outline(text, text_x, text_y, font, _METRICS_FONT_SIZE) def render_path_edges(renderer) -> None: diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index acbf4db80..9fa16a8f0 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -4,7 +4,7 @@ from msgq.visionipc import VisionStreamType from openpilot.common.params import Params from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import render_behind, render_overlay, render_background_effects -from openpilot.selfdrive.ui.onroad.starpilot.path import render_adjacent_paths, render_blind_spot_path, render_path_edges +from openpilot.selfdrive.ui.onroad.starpilot.path import render_adjacent_lanes, 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, @@ -81,14 +81,8 @@ class StarPilotOnroadView(AugmentedRoadView): 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) + # Render adjacent lanes (incorporates both adjacent path and blind spot warnings) + render_adjacent_lanes(mr) # Render stopping point atop the path from openpilot.selfdrive.ui.onroad.starpilot.stopping_point import render_stopping_point