def plot_model(m, VM, v_ego, curvature, imgw, calibration, top_down, top_down_color=216):
# Draw bar representing position and distribution of lead car from unfiltered vision model
if top_down is not None:
_, _ = to_lid_pt(m.lead, 0)
_, py_top = to_lid_pt(m.lead + m.lead_std, 0)
px, py_bottom = to_lid_pt(m.lead - m.lead_std, 0)
top_down[1][int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = top_down_color
if calibration is None:
return
if m.cpath.valid:
draw_path(m.cpath.y, _PATH_XD, YELLOW, imgw, calibration, top_down, YELLOW)
draw_var(m.cpath.y, _PATH_XD, m.cpath.std, YELLOW, imgw, calibration, top_down)
dpath_poly, _, _ = calc_desired_path(m.lpath.poly, m.rpath.poly, m.cpath.poly,
m.lpath.prob, m.rpath.prob, m.cpath.prob, v_ego)
dpath_poly = np.array(dpath_poly)
dpath_y = np.polyval(dpath_poly, _PATH_X)
draw_path(dpath_y, _PATH_X, RED, imgw, calibration, top_down, RED)
if m.lpath.valid:
color = (0, int(255 * m.lpath.prob), 0)
draw_path(m.lpath.y, _PATH_XD, color, imgw, calibration, top_down, YELLOW)
draw_var(m.lpath.y, _PATH_XD, m.lpath.std, color, imgw, calibration, top_down)
if m.rpath.valid:
color = (0, int(255 * m.rpath.prob), 0)
draw_path(m.rpath.y, _PATH_XD, color, imgw, calibration, top_down, YELLOW)
draw_var(m.rpath.y, _PATH_XD, m.rpath.std, color, imgw, calibration, top_down)
if len(m.freepath) > 0:
for i, p in enumerate(m.freepath):
d = i*2
px, py = to_lid_pt(d, 0)
cols = [36, 73, 109, 146, 182, 219, 255]
if p >= 0.4:
top_down[1][int(round(px - 4)):int(round(px + 4)), int(round(py - 4)):int(round(py + 4))] = find_color(top_down[0], (0, cols[int((p-0.4)*10)], 0))
elif p <= 0.2:
top_down[1][int(round(px - 4)):int(round(px + 4)), int(round(py - 4)):int(round(py + 4))] = 192 #find_color(top_down[0], (192, 0, 0))
# draw user path from curvature
draw_steer_path(v_ego, curvature, BLUE, imgw, calibration, top_down, VM, BLUE)
def maybe_update_radar_points(lt, lid_overlay):
ar_pts = []
if lt is not None:
ar_pts = {}
for track in lt.liveTracks:
ar_pts[track.trackId] = [track.dRel, track.yRel, track.vRel, track.aRel, track.oncoming, track.stationary]
for ids, pt in ar_pts.viewitems():
px, py = to_lid_pt(pt[0], pt[1])
if px != -1:
if pt[-1]:
color = 240
elif pt[-2]:
color = 230
else:
color = 255
if int(ids) == 1:
lid_overlay[px - 2:px + 2, py - 10:py + 10] = 100
else:
lid_overlay[px - 2:px + 2, py - 2:py + 2] = color