mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-10-04 18:43:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

Render lead vehicles as lane-following bars

Browse Source 
Co-Authored-By: Codebuff <noreply@codebuff.com>
This commit is contained in:
rav4kumar
2026-10-01 14:17:20 -07:00
parent 95a529e23b
commit be85cb61c8
1 changed files with 46 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/ui/mici/onroad/model_renderer.py
+46 -68
View File
@@ -1,11 +1,12 @@
import colorsys
import numpy as np
import pyray as rl
from openpilot.cereal import messaging
from openpilot.cereal import log, messaging
from opendbc.car.structs import car
from dataclasses import dataclass, field
from openpilot.common.params import Params
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
from openpilot.selfdrive.ui.mici.onroad import blend_colors
@@ -18,6 +19,7 @@ from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LI
CLIP_MARGIN = 500
MIN_DRAW_DISTANCE = 10.0
MAX_DRAW_DISTANCE = 100.0
LEAD_BAR_LENGTH = 12.0 # max on-screen depth in px
THROTTLE_COLORS = [
rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4)
@@ -45,11 +47,11 @@ class ModelPoints:
projected_points: np.ndarray = field(default_factory=lambda: np.empty((0, 2), dtype=np.float32))
@dataclass
class LeadVehicle:
glow: list[tuple[float, float]] = field(default_factory=list)
chevron: list[tuple[float, float]] = field(default_factory=list)
fill_alpha: int = 0
def __init__(self):
self.bar = np.empty((0, 2), dtype=np.float32)
self.y_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False)
self.fade_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps)
class ModelRenderer(Widget, ModelRendererSP):
@@ -132,7 +134,7 @@ class ModelRenderer(Widget, ModelRendererSP):
model = sm['modelV2']
radar_state = sm['radarState'] if sm.valid['radarState'] else None
lead_one = radar_state.leadOne if radar_state else None
render_lead_indicator = self._longitudinal_control and radar_state is not None
render_lead_indicator = self._longitudinal_control and radar_state is not None and sm['selfdriveState'].engageable
# Update model data when needed
model_updated = sm.updated['modelV2']
@@ -145,8 +147,6 @@ class ModelRenderer(Widget, ModelRendererSP):
return
self._update_model(lead_one, path_x_array)
if render_lead_indicator:
self._update_leads(radar_state, path_x_array)
self._transform_dirty = False
# Draw elements (hide when disengaged)
@@ -154,8 +154,11 @@ class ModelRenderer(Widget, ModelRendererSP):
self._draw_lane_lines()
self._draw_path(sm)
if render_lead_indicator and radar_state:
if render_lead_indicator:
self._update_leads(sm)
self._draw_lead_indicator()
else:
self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
def _update_raw_points(self, model):
"""Update raw 3D points from model data"""
@@ -171,21 +174,40 @@ class ModelRenderer(Widget, ModelRendererSP):
self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32)
self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32)
def _update_leads(self, radar_state, path_x_array):
"""Update positions of lead vehicles"""
self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
leads = [radar_state.leadOne, radar_state.leadTwo]
def _update_leads(self, sm):
plan = sm['longitudinalPlan']
if plan.longitudinalPlanSource == log.LongitudinalPlan.LongitudinalPlanSource.e2e and len(sm['modelV2'].leadsV3) > 1:
leads = [(lead.prob > 0.5, lead.x[0], -lead.y[0]) for lead in list(sm['modelV2'].leadsV3)[:2]]
else:
radar = sm['radarState']
leads = [(lead.present, lead.dRel + RADAR_TO_CAMERA, lead.yRel) for lead in (radar.leadOne, radar.leadTwo)]
for i, lead_data in enumerate(leads):
if lead_data and lead_data.present:
d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel
idx = self._get_path_length_idx(path_x_array, d_rel)
# both leads can be the same vehicle
if leads[0][0] and abs(leads[1][1] - leads[0][1]) < 3.0:
leads[1] = (False, 0.0, 0.0)
# Get z-coordinate from path at the lead vehicle position
z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0
point = self._map_to_screen(d_rel, -y_rel + self._camera_offset, z + self._path_offset_z)
if point:
self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect)
lane = (self._lane_lines[1].raw_points + self._lane_lines[2].raw_points) / 2
opacity = 0.4 if ui_state.status == UIStatus.DISENGAGED else 0.8
for lead, (present, d_rel, y_rel) in zip(self._lead_vehicles, leads, strict=True):
visible = present and d_rel < MAX_DRAW_DISTANCE and len(lane) > 0
if not visible or abs(y_rel - lead.y_filter.x) > 1.0:
lead.y_filter.initialized = False
lead.fade_filter.update(opacity if visible else 0.0)
if visible:
lead.bar = self._get_lead_bar(lane, d_rel, lead.y_filter.update(y_rel))
def _get_lead_bar(self, lane, d_rel, y_rel):
# bar on the road behind the lead, following the lane
x = np.array([d_rel, d_rel - min(6.0, 0.25 * d_rel)])
y = np.interp(x, lane[:, 0], lane[:, 1]) - np.interp(d_rel, lane[:, 0], lane[:, 1]) - y_rel
z = np.interp(x, self._path.raw_points[:, 0], self._path.raw_points[:, 2]) + self._path_offset_z
corners = np.vstack((np.column_stack((x, y + 0.9, z)), np.column_stack((x, y - 0.9, z))[::-1]))
pts = self._car_space_transform @ corners.T
bar = (pts[:2] / pts[2]).T
far, near = bar[[0, 3]], bar[[1, 2]]
length = np.linalg.norm(near.mean(axis=0) - far.mean(axis=0))
bar[[1, 2]] = far + (near - far) * np.clip(length, 3.0, LEAD_BAR_LENGTH) / length
return bar.astype(np.float32)
def _update_model(self, lead, path_x_array):
"""Update model visualization data based on model message"""
@@ -270,30 +292,6 @@ class ModelRenderer(Widget, ModelRendererSP):
self._exp_gradient.colors = segment_colors
self._exp_gradient.stops = gradient_stops
def _update_lead_vehicle(self, d_rel, v_rel, point, rect):
speed_buff, lead_buff = 10.0, 40.0
# Calculate fill alpha
fill_alpha = 0
if d_rel < lead_buff:
fill_alpha = 255 * (1.0 - (d_rel / lead_buff))
if v_rel < 0:
fill_alpha += 255 * (-1 * (v_rel / speed_buff))
fill_alpha = min(fill_alpha, 255)
# Calculate size and position
sz = np.clip((25 * 30) / (d_rel / 3 + 30), 15.0, 30.0) * 1
x = np.clip(point[0], 0.0, rect.width - sz / 2)
y = min(point[1], rect.height - sz * 0.6)
g_xo = sz / 5
g_yo = sz / 10
glow = [(x + (sz * 1.35) + g_xo, y + sz + g_yo), (x, y - g_yo), (x - (sz * 1.35) - g_xo, y + sz + g_yo)]
chevron = [(x + (sz * 1.25), y + sz), (x, y), (x - (sz * 1.25), y + sz)]
return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
def _get_ll_color(self, prob: float, adjacent: bool, left: bool):
alpha = np.clip(prob, 0.0, 0.7)
if adjacent:
@@ -375,13 +373,9 @@ class ModelRenderer(Widget, ModelRendererSP):
draw_polygon(self._rect, path_pts, gradient=gradient)
def _draw_lead_indicator(self):
# Draw lead vehicles if available
offset = np.array([self._rect.x, self._rect.y], dtype=np.float32)
for lead in self._lead_vehicles:
if not lead.glow or not lead.chevron:
continue
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))
draw_polygon(self._rect, lead.bar + offset, rl.Color(255, 255, 255, int(255 * lead.fade_filter.x)))
@staticmethod
def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int:
@@ -391,22 +385,6 @@ class ModelRenderer(Widget, ModelRendererSP):
indices = np.where(pos_x_array <= path_height)[0]
return indices[-1] if indices.size > 0 else 0
def _map_to_screen(self, in_x, in_y, in_z):
"""Project a point in car space to screen space"""
input_pt = np.array([in_x, in_y, in_z])
pt = self._car_space_transform @ input_pt
if abs(pt[2]) < 1e-6:
return None
x, y = pt[0] / pt[2], pt[1] / pt[2]
clip = self._clip_region
if not (clip.x <= x <= clip.x + clip.width and clip.y <= y <= clip.y + clip.height):
return None
return (x, y)
def _map_line_to_polygon(self, line: np.ndarray, y_off: float, z_off: float, max_idx: int, allow_invert: bool = True) -> np.ndarray:
"""Convert 3D line to 2D polygon for rendering."""
if line.shape[0] == 0: