From fcdfeb1beca2024061b7446ac46d607f1d290e0c Mon Sep 17 00:00:00 2001 From: royjr Date: Wed, 15 Jul 2026 13:44:54 -0400 Subject: [PATCH] doppler --- .../ui/sunnypilot/onroad/radar_tracks.py | 19 +++++++++-- .../ui/sunnypilot/onroad/test_radar_tracks.py | 32 ++++++++++++++++++- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py index 28ae96a117..d888b32374 100644 --- a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -7,6 +7,19 @@ See the LICENSE.md file in the root directory for more details. import math import pyray as rl +RELATIVE_SPEED_COLOR_RANGE = 15.0 # m/s; speeds beyond this use the full endpoint color +APPROACHING_COLOR = (32, 128, 255) +NEUTRAL_COLOR = (255, 255, 255) +RECEDING_COLOR = (255, 48, 48) + + +def radar_track_color(v_rel: float) -> rl.Color: + """Map relative speed from approaching blue through neutral white to receding red.""" + 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)) + return rl.Color(*rgb, 255) + def format_radar_tracks_onroad_status(live_tracks) -> str: range_text, count_text = format_radar_tracks_onroad_columns(live_tracks) @@ -29,8 +42,8 @@ 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)): for track in live_tracks.points: - d_rel, y_rel, v_rel, a_rel = track.dRel, track.yRel, track.vRel, track.aRel - if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel) and math.isfinite(a_rel)): + 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)): continue pt = map_to_screen(d_rel, -y_rel, path_offset_z) @@ -38,4 +51,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, rl.Color(0, 255, 64, 255)) + rl.draw_circle(int(x), int(y), track_size, radar_track_color(v_rel)) diff --git a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index b0d0046875..27de45d722 100644 --- a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -1,7 +1,22 @@ from cereal import car from openpilot.selfdrive.ui.sunnypilot.onroad import radar_tracks -from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import format_radar_tracks_onroad_columns, format_radar_tracks_onroad_status +from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import format_radar_tracks_onroad_columns, format_radar_tracks_onroad_status, radar_track_color + + +def color_tuple(color): + return color.r, color.g, color.b, color.a + + +def test_radar_track_relative_speed_colors(): + assert color_tuple(radar_track_color(-15.0)) == (32, 128, 255, 255) + assert color_tuple(radar_track_color(0.0)) == (255, 255, 255, 255) + assert color_tuple(radar_track_color(15.0)) == (255, 48, 48, 255) + + approaching = color_tuple(radar_track_color(-7.5)) + receding = color_tuple(radar_track_color(7.5)) + assert approaching == (144, 192, 255, 255) + assert receding == (255, 152, 152, 255) def test_format_radar_tracks_status_none(): @@ -55,3 +70,18 @@ def test_draw_radar_tracks_applies_screen_offset(monkeypatch): ) assert drawn_circles == [(120, 37, 3)] + + +def test_draw_radar_tracks_allows_unknown_acceleration(monkeypatch): + live_tracks = car.RadarData.new_message() + point = live_tracks.init("points", 1)[0] + point.dRel = 10 + point.yRel = 1 + point.vRel = -5 + point.aRel = float("nan") + drawn_colors = [] + monkeypatch.setattr(radar_tracks.rl, "draw_circle", lambda x, y, size, color: drawn_colors.append(color_tuple(color))) + + radar_tracks.RadarTracks().draw_radar_tracks(live_tracks, lambda d_rel, y_rel, z: (20, 30), path_offset_z=1.2) + + assert drawn_colors == [color_tuple(radar_track_color(-5))]