diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py index 690388154a..9e0292ae47 100644 --- a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -7,7 +7,7 @@ See the LICENSE.md file in the root directory for more details. import math import pyray as rl -RELATIVE_SPEED_COLOR_RANGE = 10.0 # m/s; speeds beyond this use the full endpoint color +RELATIVE_SPEED_MOVING_THRESHOLD = 0.5 # m/s relative speed deadband STATIONARY_SPEED_THRESHOLD = 1.0 # m/s estimated ground speed APPROACHING_COLOR = (0, 64, 255) NEUTRAL_COLOR = (255, 255, 255) @@ -15,14 +15,12 @@ RECEDING_COLOR = (255, 0, 0) 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: + """Classify tracks as stationary, approaching, or receding with discrete colors.""" + if abs(v_ego + v_rel) <= STATIONARY_SPEED_THRESHOLD or abs(v_rel) <= RELATIVE_SPEED_MOVING_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)) - return rl.Color(*rgb, 255) + color = APPROACHING_COLOR if v_rel < 0.0 else RECEDING_COLOR + return rl.Color(*color, 255) def format_radar_tracks_onroad_status(live_tracks) -> str: diff --git a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py index 4d34a87ffa..e9d7881826 100644 --- a/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py +++ b/selfdrive/ui/sunnypilot/onroad/test_radar_tracks.py @@ -12,11 +12,15 @@ def test_radar_track_relative_speed_colors(): assert color_tuple(radar_track_color(-10.0)) == (0, 64, 255, 255) assert color_tuple(radar_track_color(0.0)) == (255, 255, 255, 255) assert color_tuple(radar_track_color(10.0)) == (255, 0, 0, 255) + assert color_tuple(radar_track_color(-5.0)) == (0, 64, 255, 255) + assert color_tuple(radar_track_color(5.0)) == (255, 0, 0, 255) - approaching = color_tuple(radar_track_color(-5.0)) - receding = color_tuple(radar_track_color(5.0)) - assert approaching == (128, 160, 255, 255) - assert receding == (255, 128, 128, 255) + +def test_radar_track_relative_speed_deadband_is_white(): + assert color_tuple(radar_track_color(-0.5, v_ego=10.0)) == (255, 255, 255, 255) + assert color_tuple(radar_track_color(0.5, v_ego=10.0)) == (255, 255, 255, 255) + assert color_tuple(radar_track_color(-0.51, v_ego=10.0)) == (0, 64, 255, 255) + assert color_tuple(radar_track_color(0.51, v_ego=10.0)) == (255, 0, 0, 255) def test_radar_track_stationary_world_object_is_white():