From be85cb61c86c5bdd06ca1cbf4f4f3def58523315 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Thu, 1 Oct 2026 14:17:20 -0700 Subject: [PATCH] Render lead vehicles as lane-following bars Co-Authored-By: Codebuff --- .../ui/mici/onroad/model_renderer.py | 114 +++++++----------- 1 file changed, 46 insertions(+), 68 deletions(-) diff --git a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py index 1917e28711..60d8085b29 100644 --- a/openpilot/selfdrive/ui/mici/onroad/model_renderer.py +++ b/openpilot/selfdrive/ui/mici/onroad/model_renderer.py @@ -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: