From 1faa61d34d54841ede3fdae9dea762481786212f Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 17 Jul 2026 18:21:51 -0400 Subject: [PATCH] why drift --- selfdrive/ui/mici/onroad/model_renderer.py | 39 +++---------------- selfdrive/ui/onroad/model_renderer.py | 20 ++++++++-- .../ui/sunnypilot/onroad/radar_tracks.py | 31 +++++++++++++++ .../ui/sunnypilot/onroad/test_radar_tracks.py | 35 ++++++++++++++++- 4 files changed, 88 insertions(+), 37 deletions(-) diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index eac39b9d71..e88f1d4ac9 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -1,5 +1,4 @@ import colorsys -import math import numpy as np import pyray as rl from cereal import messaging, car @@ -14,6 +13,7 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.sunnypilot.mici.onroad.model_renderer import LANE_LINE_COLORS_SP, ModelRendererSP +from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors CLIP_MARGIN = 500 MIN_DRAW_DISTANCE = 10.0 @@ -38,12 +38,6 @@ LANE_LINE_COLORS = { **LANE_LINE_COLORS_SP, } -LEAD_TRACK_COLORS = ( - rl.Color(255, 215, 0, 255), - rl.Color(255, 140, 0, 220), -) - - @dataclass class ModelPoints: raw_points: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.float32)) @@ -163,14 +157,17 @@ class ModelRenderer(Widget, ModelRendererSP): self._draw_path(sm) if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: - highlighted_tracks = self._lead_track_colors(radar_state) if render_lead_indicator else {} + highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {} matched_positions = self.radar_tracks.draw_radar_tracks( sm['liveTracks'], self._map_to_screen, self._path_offset_z, screen_offset=(self._rect.x, self._rect.y), v_ego=sm['carState'].vEgo, highlighted_tracks=highlighted_tracks, ) if render_lead_indicator: - self._draw_lead_connectors(matched_positions, highlighted_tracks) + draw_radar_lead_connectors( + self._lead_vehicles, matched_positions, highlighted_tracks, + screen_offset=(self._rect.x, self._rect.y), + ) if render_lead_indicator: self._draw_lead_indicator() @@ -407,30 +404,6 @@ class ModelRenderer(Widget, ModelRendererSP): rl.draw_triangle_fan(offset_glow, len(offset_glow), rl.Color(218, 202, 37, 255)) rl.draw_triangle_fan(offset_chevron, len(offset_chevron), rl.Color(201, 34, 49, lead.fill_alpha)) - @staticmethod - def _lead_track_colors(radar_state): - highlighted_tracks = {} - if radar_state is None: - return highlighted_tracks - - for lead, color in zip((radar_state.leadOne, radar_state.leadTwo), LEAD_TRACK_COLORS, strict=True): - if lead.status and lead.radar and lead.radarTrackId >= 0: - highlighted_tracks.setdefault(int(lead.radarTrackId), color) - return highlighted_tracks - - def _draw_lead_connectors(self, matched_positions, highlighted_tracks): - for lead in self._lead_vehicles: - if not lead.radar or lead.position is None or lead.radar_track_id not in matched_positions: - continue - - radar_position = matched_positions[lead.radar_track_id] - lead_position = (lead.position[0] + self._rect.x, lead.position[1] + self._rect.y) - if math.dist(lead_position, radar_position) < 4: - continue - - rl.draw_line_ex(rl.Vector2(*lead_position), rl.Vector2(*radar_position), 2, - highlighted_tracks[lead.radar_track_id]) - @staticmethod def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int: """Get the index corresponding to the given path height""" diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 5f7e108625..580a4b94e3 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -12,6 +12,7 @@ from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.sunnypilot.onroad.model_renderer import ChevronMetrics, ModelRendererSP +from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, radar_lead_track_colors CLIP_MARGIN = 500 MIN_DRAW_DISTANCE = 10.0 @@ -41,6 +42,9 @@ class LeadVehicle: glow: list[tuple[float, float]] = field(default_factory=list) chevron: list[tuple[float, float]] = field(default_factory=list) fill_alpha: int = 0 + position: tuple[float, float] | None = None + radar_track_id: int = -1 + radar: bool = False class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): @@ -114,7 +118,7 @@ class ModelRenderer(Widget, ChevronMetrics, 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 = ui_state.radar_tracks != 0 and radar_state is not None # Update model data when needed model_updated = sm.updated['modelV2'] @@ -136,7 +140,13 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): self._draw_path(sm) if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: - self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z, v_ego=sm['carState'].vEgo) + highlighted_tracks = radar_lead_track_colors(radar_state) if render_lead_indicator else {} + matched_positions = self.radar_tracks.draw_radar_tracks( + sm['liveTracks'], self._map_to_screen, self._path_offset_z, + v_ego=sm['carState'].vEgo, highlighted_tracks=highlighted_tracks, + ) + if render_lead_indicator: + draw_radar_lead_connectors(self._lead_vehicles, matched_positions, highlighted_tracks) if render_lead_indicator and radar_state: self._draw_lead_indicator() @@ -170,7 +180,11 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): 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) + lead_vehicle = self._update_lead_vehicle(d_rel, v_rel, point, self._rect) + lead_vehicle.position = lead_vehicle.chevron[1] + lead_vehicle.radar_track_id = int(lead_data.radarTrackId) + lead_vehicle.radar = lead_data.radar + self._lead_vehicles[i] = lead_vehicle def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py index 089f31c839..2ae752db13 100644 --- a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -17,6 +17,10 @@ DBC_MOTION_STATIONARY = 1 DBC_MOTION_MOVING = 2 COASTED_ALPHA = 96 COASTED_RADIUS_REDUCTION = 2 +LEAD_TRACK_COLORS = ( + rl.Color(255, 215, 0, 255), + rl.Color(255, 140, 0, 220), +) def is_preferred_radar_source(source) -> bool: @@ -66,6 +70,33 @@ def radar_track_display(motion_state: int, measured: bool = True) -> tuple[rl.Co return rl.Color(*DBC_UNKNOWN_COLOR, alpha), False +def radar_lead_track_colors(radar_state) -> dict[int, rl.Color]: + highlighted_tracks = {} + if radar_state is None: + return highlighted_tracks + + for lead, color in zip((radar_state.leadOne, radar_state.leadTwo), LEAD_TRACK_COLORS, strict=True): + if lead.status and lead.radar and lead.radarTrackId >= 0: + highlighted_tracks.setdefault(int(lead.radarTrackId), color) + return highlighted_tracks + + +def draw_radar_lead_connectors(lead_vehicles, matched_positions, highlighted_tracks, screen_offset=(0, 0)) -> None: + for lead in lead_vehicles: + if not lead.radar or lead.position is None or lead.radar_track_id not in matched_positions: + continue + + radar_position = matched_positions[lead.radar_track_id] + lead_position = (lead.position[0] + screen_offset[0], lead.position[1] + screen_offset[1]) + if math.dist(lead_position, radar_position) < 4: + continue + + rl.draw_line_ex( + rl.Vector2(*lead_position), rl.Vector2(*radar_position), 2, + highlighted_tracks[lead.radar_track_id], + ) + + def format_radar_tracks_onroad_columns(live_tracks, v_ego: float = 0.0) -> tuple[str, str, str, str, str, str]: sources = sorted_radar_sources(live_tracks) if not sources: diff --git a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index c830a565ec..4f45f45a02 100644 --- a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -1,7 +1,10 @@ +from types import SimpleNamespace + from cereal import car from openpilot.selfdrive.ui.sunnypilot.onroad import radar_tracks -from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import format_radar_tracks_onroad_columns, radar_track_display +from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import draw_radar_lead_connectors, format_radar_tracks_onroad_columns, \ + radar_lead_track_colors, radar_track_display def color_tuple(color): @@ -29,6 +32,36 @@ def test_unknown_dbc_motion_uses_neutral_dbc_color(): assert not stationary +def test_radar_lead_track_colors_only_highlight_radar_matches(): + radar_state = SimpleNamespace( + leadOne=SimpleNamespace(status=True, radar=True, radarTrackId=7), + leadTwo=SimpleNamespace(status=True, radar=False, radarTrackId=9), + ) + + colors = radar_lead_track_colors(radar_state) + + assert list(colors) == [7] + assert color_tuple(colors[7]) == color_tuple(radar_tracks.LEAD_TRACK_COLORS[0]) + + +def test_draw_radar_lead_connectors_applies_screen_offset(monkeypatch): + lead = SimpleNamespace(radar=True, position=(10, 20), radar_track_id=7) + color = radar_tracks.LEAD_TRACK_COLORS[0] + drawn = [] + monkeypatch.setattr( + radar_tracks.rl, "draw_line_ex", + lambda start, end, width, line_color: drawn.append( + ((start.x, start.y), (end.x, end.y), width, color_tuple(line_color)) + ), + ) + + draw_radar_lead_connectors( + [lead], {7: (120, 30)}, {7: color}, screen_offset=(100, 5), + ) + + assert drawn == [((110, 25), (120, 30), 2, color_tuple(color))] + + def test_format_radar_tracks_columns_none(): live_tracks = car.RadarData.new_message()