cache

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()

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()

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)

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)]