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