diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index e88f1d4ac9..64d0ab8ad6 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -138,7 +138,8 @@ class ModelRenderer(Widget, ModelRendererSP): # Update model data when needed model_updated = sm.updated['modelV2'] - if model_updated or sm.updated['radarState'] or self._transform_dirty: + transform_updated = self._transform_dirty + if model_updated or sm.updated['radarState'] or transform_updated: if model_updated: self._update_raw_points(model) @@ -157,10 +158,14 @@ class ModelRenderer(Widget, ModelRendererSP): self._draw_path(sm) if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: + if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or + not self.radar_tracks.projection_initialized): + self.radar_tracks.update_radar_tracks( + sm['liveTracks'], self._map_to_screen, self._path_offset_z, + ) 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, + matched_positions = self.radar_tracks.draw_cached_radar_tracks( + screen_offset=(self._rect.x, self._rect.y), highlighted_tracks=highlighted_tracks, ) if render_lead_indicator: @@ -168,6 +173,8 @@ class ModelRenderer(Widget, ModelRendererSP): self._lead_vehicles, matched_positions, highlighted_tracks, screen_offset=(self._rect.x, self._rect.y), ) + else: + self.radar_tracks.clear_projection() if render_lead_indicator: self._draw_lead_indicator() diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 580a4b94e3..83c03d83a9 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -122,7 +122,8 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): # Update model data when needed model_updated = sm.updated['modelV2'] - if model_updated or sm.updated['radarState'] or self._transform_dirty: + transform_updated = self._transform_dirty + if model_updated or sm.updated['radarState'] or transform_updated: if model_updated: self._update_raw_points(model) @@ -140,13 +141,19 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): self._draw_path(sm) if sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: + if (sm.updated['liveTracks'] or sm.updated['liveCalibration'] or transform_updated or + not self.radar_tracks.projection_initialized): + self.radar_tracks.update_radar_tracks( + sm['liveTracks'], self._map_to_screen, self._path_offset_z, + ) 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, + matched_positions = self.radar_tracks.draw_cached_radar_tracks( + highlighted_tracks=highlighted_tracks, ) if render_lead_indicator: draw_radar_lead_connectors(self._lead_vehicles, matched_positions, highlighted_tracks) + else: + self.radar_tracks.clear_projection() if render_lead_indicator and radar_state: self._draw_lead_indicator() diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py index 2ae752db13..b2c334b767 100644 --- a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -4,6 +4,7 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +from dataclasses import dataclass import math import pyray as rl from opendbc.car.hyundai.radar_interface import RADAR_3A5_3C4 @@ -23,6 +24,16 @@ LEAD_TRACK_COLORS = ( ) +@dataclass(frozen=True) +class ProjectedRadarTrack: + x: float + y: float + radius: float + color: object + source_index: int + track_id: int + + def is_preferred_radar_source(source) -> bool: return source.startAddress == RADAR_3A5_3C4.start_addr and source.endAddress == RADAR_3A5_3C4.end_addr @@ -238,10 +249,20 @@ class RadarTracksStatus: class RadarTracks: - def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7, screen_offset=(0, 0), v_ego=0.0, - highlighted_tracks=None): - highlighted_tracks = highlighted_tracks or {} - highlighted_positions = {} + def __init__(self): + self._projected_tracks: tuple[ProjectedRadarTrack, ...] = () + self._projection_initialized = False + + @property + def projection_initialized(self) -> bool: + return self._projection_initialized + + def clear_projection(self) -> None: + self._projected_tracks = () + self._projection_initialized = False + + def update_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7) -> None: + projected_tracks = [] sources = sorted_radar_sources(live_tracks) for track in live_tracks.points: @@ -257,20 +278,41 @@ class RadarTracks: if pt is None: continue - x, y = pt[0] + screen_offset[0], pt[1] + screen_offset[1] measured = bool(track.measured) color, stationary = radar_track_display(motion_state, measured) radius = max(1, track_size - 5) if stationary else track_size if not measured: radius = max(1, radius - COASTED_RADIUS_REDUCTION) - track_id = int(track.trackId) - highlight_color = highlighted_tracks.get(track_id) + projected_tracks.append(ProjectedRadarTrack( + x=pt[0], + y=pt[1], + radius=radius, + color=color, + source_index=radar_track_source_index(track, sources), + track_id=int(track.trackId), + )) + + self._projected_tracks = tuple(projected_tracks) + self._projection_initialized = True + + def draw_cached_radar_tracks(self, screen_offset=(0, 0), highlighted_tracks=None): + highlighted_tracks = highlighted_tracks or {} + highlighted_positions = {} + + for track in self._projected_tracks: + x, y = track.x + screen_offset[0], track.y + screen_offset[1] + highlight_color = highlighted_tracks.get(track.track_id) if highlight_color is not None: center = rl.Vector2(int(x), int(y)) - rl.draw_ring(center, radius + 2, radius + 5, 0, 360, 24, highlight_color) - highlighted_positions[track_id] = (x, y) + rl.draw_ring(center, track.radius + 2, track.radius + 5, 0, 360, 24, highlight_color) + highlighted_positions[track.track_id] = (x, y) draw_radar_source_marker( - rl.Vector2(x, y), radius, color, radar_track_source_index(track, sources), + rl.Vector2(x, y), track.radius, track.color, track.source_index, ) return highlighted_positions + + def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=7, screen_offset=(0, 0), v_ego=0.0, + highlighted_tracks=None): + self.update_radar_tracks(live_tracks, map_to_screen, path_offset_z, track_size) + return self.draw_cached_radar_tracks(screen_offset, highlighted_tracks) diff --git a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index 4f45f45a02..f1e046857d 100644 --- a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -292,3 +292,32 @@ def test_draw_radar_tracks_shrinks_and_fades_coasted_moving_track(monkeypatch): ) assert drawn == [(4, (*radar_tracks.DBC_MOVING_COLOR, radar_tracks.COASTED_ALPHA))] + + +def test_cached_radar_tracks_only_reproject_on_update(monkeypatch): + live_tracks = car.RadarData.new_message() + point = live_tracks.init("points", 1)[0] + point.trackId = 7 + point.dRel = 10 + point.yRel = 1 + point.vRel = 0 + point.motionState = radar_tracks.DBC_MOTION_MOVING + point.measured = True + projected = [] + drawn = [] + + def map_to_screen(d_rel, y_rel, z): + projected.append((d_rel, y_rel, z)) + return (20, 30) + + monkeypatch.setattr( + radar_tracks.rl, "draw_circle", + lambda x, y, radius, color: drawn.append((x, y)), + ) + renderer = radar_tracks.RadarTracks() + renderer.update_radar_tracks(live_tracks, map_to_screen, path_offset_z=1.2) + renderer.draw_cached_radar_tracks(screen_offset=(100, 7)) + renderer.draw_cached_radar_tracks(screen_offset=(200, 9)) + + assert projected == [(10, -1, 1.2)] + assert drawn == [(120, 37), (220, 39)]