mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-01 19:42:07 +08:00
BigUI WIP: Onroad Path Cleanup
This commit is contained in:
firestarsdog
@@ -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:
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user