def plot_model(m,
VM,
v_ego,
curvature,
imgw,
calibration,
top_down,
d_poly,
top_down_color=216):
if calibration is None or top_down is None:
return
for lead in [m.lead, m.lead_future]:
if lead.prob < 0.5:
continue
_, py_top = to_lid_pt(lead.dist + lead.std, lead.relY)
px, py_bottom = to_lid_pt(lead.dist - lead.std, lead.relY)
top_down[1][int(round(px - 4)):int(round(px + 4)),
py_top:py_bottom] = top_down_color
color = (0, int(255 * m.lpath.prob), 0)
for path in [m.cpath, m.lpath, m.rpath]:
if path.valid:
draw_path(path.y, _PATH_XD, color, imgw, calibration, top_down,
YELLOW)
draw_var(path.y, _PATH_XD, path.std, color, imgw, calibration,
top_down)
if d_poly is not None:
dpath_y = np.polyval(d_poly, _PATH_X)
draw_path(dpath_y, _PATH_X, RED, imgw, calibration, top_down, RED)
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 + RADAR_TO_CENTER, 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
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)