From c33e7c1294727d35c0cbec231e6d1ca9a91ce5ff Mon Sep 17 00:00:00 2001 From: royjr Date: Sat, 18 Jul 2026 08:41:34 -0400 Subject: [PATCH] cache --- selfdrive/ui/mici/onroad/model_renderer.py | 15 +++- selfdrive/ui/onroad/model_renderer.py | 15 +++- .../ui/sunnypilot/onroad/radar_tracks.py | 62 +++++++++++++--- .../ui/sunnypilot/onroad/test_radar_tracks.py | 29 ++++++++ .../selfdrive/controls/controlsd_ext.py | 73 +++++++++++++++---- .../selfdrive/controls/test_controlsd_ext.py | 16 ++++ 6 files changed, 176 insertions(+), 34 deletions(-) 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)] diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 3a9da5a76d..32a540febe 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/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 diff --git a/sunnypilot/selfdrive/controls/test_controlsd_ext.py b/sunnypilot/selfdrive/controls/test_controlsd_ext.py index 4c097f702d..0d9ba4b386 100644 --- a/sunnypilot/selfdrive/controls/test_controlsd_ext.py +++ b/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