ui.py: draw lines on radarState leads

tools/replay/lib/ui_helpers.py

@@ -214,6 +214,17 @@ def plot_model(m, img, calibration, top_down):
   draw_path(m.position, color, img, calibration, top_down, RED, 1.22)
 
 
+def plot_lead(rs, top_down):
+  for lead in [rs.leadOne, rs.leadTwo]:
+    if not lead.status:
+      continue
+
+    x = lead.dRel
+    px_left, py = to_topdown_pt(x, -10)
+    px_right, _ = to_topdown_pt(x, 10)
+    top_down[1][px_left:px_right, py] = find_color(top_down[0], RED)
+
+
 def maybe_update_radar_points(lt, lid_overlay):
   ar_pts = []
   if lt is not None:

tools/replay/ui.py

@@ -9,15 +9,16 @@ import cv2  # pylint: disable=import-error
 import numpy as np
 import pygame  # pylint: disable=import-error
 
+import cereal.messaging as messaging
 from common.basedir import BASEDIR
 from selfdrive.config import UIParams as UP
-import cereal.messaging as messaging
-from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, _FULL_FRAME_SIZE, _INTRINSICS,
-                                         BLACK,  GREEN, YELLOW, RED,
+from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, _FULL_FRAME_SIZE,
+                                         _INTRINSICS, BLACK, GREEN, RED,
+                                         YELLOW, Calibration,
                                          get_blank_lid_overlay, init_plots,
-                                         maybe_update_radar_points, plot_model,
-                                         pygame_modules_have_loaded,
-                                         Calibration)
+                                         maybe_update_radar_points, plot_lead,
+                                         plot_model,
+                                         pygame_modules_have_loaded)
 
 os.environ['BASEDIR'] = BASEDIR
 
@@ -157,10 +158,12 @@ def ui_thread(addr, frame_address):
     plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].aTarget
     plot_arr[-1, name_to_arr_idx['accel_override']] = sm['carControl'].cruiseControl.accelOverride
 
-    # ***** model ****
     if sm.rcv_frame['modelV2']:
       plot_model(sm['modelV2'], img, calibration, top_down)
 
+    if sm.rcv_frame['radarState']:
+      plot_lead(sm['radarState'], top_down)
+
     # draw all radar points
     maybe_update_radar_points(sm['liveTracks'], top_down[1])