diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index a0644b74ca..8940071e5e 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/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: diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 6ce1e35f43..d93951a581 100755 --- a/tools/replay/ui.py +++ b/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])