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

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -21,6 +21,10 @@ from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import Lat
 
 
 class ControlsExt(ModelStateBase):
+  _radar_tracks_active: bool
+  _radar_tracks_cache: tuple[dict[str, int | float], ...]
+  _radar_tracks_cache_initialized: bool
+
   def __init__(self, CP: structs.CarParams, params: Params):
     ModelStateBase.__init__(self)
     self.CP = CP
@@ -34,6 +38,12 @@ class ControlsExt(ModelStateBase):
 
     self.sm_services_ext = ['radarState', 'selfdriveStateSP', 'liveTracks']
     self.pm_services_ext = ['carControlSP']
+    self._reset_radar_track_cache()
+
+  def _reset_radar_track_cache(self) -> None:
+    self._radar_tracks_active = False
+    self._radar_tracks_cache = ()
+    self._radar_tracks_cache_initialized = False
 
   def initialize_lateral_control(self, lac, CI, dt):
     enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
@@ -87,33 +97,64 @@ class ControlsExt(ModelStateBase):
     _lead.radarTrackId = src.radarTrackId
 
   @staticmethod
-  def get_radar_track_data(CC_SP: custom.CarControlSP, live_tracks, valid: bool, model=None, model_valid: bool = False) -> None:
-    CC_SP.radarTracksActive = valid and len(live_tracks.trackSources) > 0
-    source_tracks = (
-      [track for track in live_tracks.points if track.measured and track.motionState in (1, 2)]
-      if CC_SP.radarTracksActive else ()
-    )
+  def build_radar_track_data(live_tracks, valid: bool, model=None,
+                             model_valid: bool = False) -> tuple[bool, tuple[dict[str, int | float], ...]]:
+    radar_tracks_active = valid and len(live_tracks.trackSources) > 0
+    if not radar_tracks_active:
+      return False, ()
+
+    source_tracks = [track for track in live_tracks.points if track.measured and track.motionState in (1, 2)]
+    if not source_tracks:
+      return True, ()
+
     path_x = np.asarray(model.position.x, dtype=float) if model_valid else np.empty(0)
     path_y = np.asarray(model.position.y, dtype=float) if model_valid else np.empty(0)
     path_valid = len(path_x) >= 2 and len(path_x) == len(path_y) and np.all(np.isfinite(path_x)) and np.all(np.isfinite(path_y))
-    radar_tracks = CC_SP.init('radarTracks', len(source_tracks))
-    for dst, src in zip(radar_tracks, source_tracks, strict=True):
-      dst.trackId = src.trackId
-      dst.dRel = src.dRel
+    radar_tracks = []
+    for src in source_tracks:
       center_y = float(np.interp(src.dRel, path_x, path_y)) if path_valid else 0.0
-      dst.yRel = src.yRel + center_y
-      dst.vRel = src.vRel
-      dst.motionState = src.motionState
-      dst.age = src.trackAge
+      radar_tracks.append({
+        "trackId": int(src.trackId),
+        "dRel": float(src.dRel),
+        "yRel": float(src.yRel + center_y),
+        "vRel": float(src.vRel),
+        "motionState": int(src.motionState),
+        "age": int(src.trackAge),
+      })
+    return radar_tracks_active, tuple(radar_tracks)
+
+  @staticmethod
+  def set_radar_track_data(CC_SP: custom.CarControlSP, active: bool,
+                           radar_tracks: tuple[dict[str, int | float], ...]) -> None:
+    CC_SP.radarTracksActive = active
+    CC_SP.radarTracks = radar_tracks
+
+  @classmethod
+  def get_radar_track_data(cls, CC_SP: custom.CarControlSP, live_tracks, valid: bool,
+                           model=None, model_valid: bool = False) -> None:
+    active, radar_tracks = cls.build_radar_track_data(live_tracks, valid, model, model_valid)
+    cls.set_radar_track_data(CC_SP, active, radar_tracks)
+
+  def update_radar_track_cache(self, live_tracks, valid: bool, model=None,
+                               model_valid: bool = False, inputs_updated: bool = True) -> None:
+    if self._radar_tracks_cache_initialized and not inputs_updated:
+      return
+
+    self._radar_tracks_active, self._radar_tracks_cache = self.build_radar_track_data(
+      live_tracks, valid, model, model_valid,
+    )
+    self._radar_tracks_cache_initialized = True
 
   def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
     CC_SP = custom.CarControlSP.new_message()
 
     self.get_lead_data(CC_SP.leadOne, sm['radarState'].leadOne)
     self.get_lead_data(CC_SP.leadTwo, sm['radarState'].leadTwo)
-    self.get_radar_track_data(
-      CC_SP, sm['liveTracks'], sm.valid['liveTracks'], sm['modelV2'], sm.valid['modelV2'],
+    self.update_radar_track_cache(
+      sm['liveTracks'], sm.valid['liveTracks'], sm['modelV2'], sm.valid['modelV2'],
+      sm.updated['liveTracks'] or sm.updated['modelV2'],
     )
+    self.set_radar_track_data(CC_SP, self._radar_tracks_active, self._radar_tracks_cache)
 
     # MADS state
     mads_src = sm['selfdriveStateSP'].mads

sunnypilot/selfdrive/controls/test_controlsd_ext.py

@@ -66,3 +66,19 @@ def test_radar_tracks_are_positioned_relative_to_curved_model_path():
 
   assert cc_sp.radarTracks[0].yRel == 0
   assert cc_sp.radarTracks[1].yRel == -3.5
+
+
+def test_radar_track_cache_only_rebuilds_when_inputs_update():
+  controls_ext = object.__new__(ControlsExt)
+  controls_ext._reset_radar_track_cache()
+  live_tracks = make_live_tracks((2,))
+
+  controls_ext.update_radar_track_cache(live_tracks, valid=True, inputs_updated=True)
+  assert controls_ext._radar_tracks_cache[0]["dRel"] == 12
+
+  live_tracks.points[0].dRel = 30
+  controls_ext.update_radar_track_cache(live_tracks, valid=True, inputs_updated=False)
+  assert controls_ext._radar_tracks_cache[0]["dRel"] == 12
+
+  controls_ext.update_radar_track_cache(live_tracks, valid=True, inputs_updated=True)
+  assert controls_ext._radar_tracks_cache[0]["dRel"] == 30