diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index 0e86dab131..05cd96aaa7 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -155,7 +155,7 @@ class ModelRenderer(Widget, ModelRendererSP): 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, track_size=5, - screen_offset=(self._rect.x, self._rect.y)) + screen_offset=(self._rect.x, self._rect.y), v_ego=sm['carState'].vEgo) # if render_lead_indicator and radar_state: # self._draw_lead_indicator() diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 3363d4bc37..5e2ebb1fde 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -136,7 +136,7 @@ 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) + self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z, v_ego=sm['carState'].vEgo) 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 76c03abc28..fdca4ab7f0 100644 --- a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -8,13 +8,17 @@ import math import pyray as rl RELATIVE_SPEED_COLOR_RANGE = 10.0 # m/s; speeds beyond this use the full endpoint color +STATIONARY_SPEED_THRESHOLD = 1.0 # m/s estimated ground speed APPROACHING_COLOR = (0, 128, 255) NEUTRAL_COLOR = (255, 255, 255) RECEDING_COLOR = (255, 32, 32) -def radar_track_color(v_rel: float) -> rl.Color: +def radar_track_color(v_rel: float, v_ego: float = 0.0) -> rl.Color: """Map relative speed from approaching blue through neutral white to receding red.""" + if abs(v_ego + v_rel) <= STATIONARY_SPEED_THRESHOLD: + return rl.Color(*NEUTRAL_COLOR, 255) + blend = min(abs(v_rel) / RELATIVE_SPEED_COLOR_RANGE, 1.0) target = APPROACHING_COLOR if v_rel < 0.0 else RECEDING_COLOR rgb = tuple(round(neutral + (endpoint - neutral) * blend) for neutral, endpoint in zip(NEUTRAL_COLOR, target, strict=True)) @@ -40,7 +44,7 @@ def format_radar_tracks_onroad_columns(live_tracks) -> tuple[str, str]: class RadarTracks: - def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=6, screen_offset=(0, 0)): + def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=6, screen_offset=(0, 0), v_ego=0.0): for track in live_tracks.points: d_rel, y_rel, v_rel = track.dRel, track.yRel, track.vRel if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel)): @@ -51,4 +55,4 @@ class RadarTracks: continue x, y = pt[0] + screen_offset[0], pt[1] + screen_offset[1] - rl.draw_circle(int(x), int(y), track_size, radar_track_color(v_rel)) + rl.draw_circle(int(x), int(y), track_size, radar_track_color(v_rel, v_ego)) diff --git a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index 33934bb678..46f90fde0b 100644 --- a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -19,6 +19,12 @@ def test_radar_track_relative_speed_colors(): assert receding == (255, 144, 144, 255) +def test_radar_track_stationary_world_object_is_white(): + assert color_tuple(radar_track_color(-20.0, v_ego=20.0)) == (255, 255, 255, 255) + assert color_tuple(radar_track_color(-19.0, v_ego=20.0)) == (255, 255, 255, 255) + assert color_tuple(radar_track_color(-18.9, v_ego=20.0)) == (0, 128, 255, 255) + + def test_format_radar_tracks_status_none(): live_tracks = car.RadarData.new_message()