def ui_thread(addr, frame_address): context = zmq.Context.instance() # TODO: Detect car from replay and use that to select carparams CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017") VM = VehicleModel(CP) CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]]) vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]]) pygame.init() pygame.font.init() assert pygame_modules_have_loaded() if HOR: size = (640 + 384 + 640, 960) write_x = 5 write_y = 680 else: size = (640 + 384, 960 + 300) write_x = 645 write_y = 970 pygame.display.set_caption("openpilot debug UI") screen = pygame.display.set_mode(size, pygame.DOUBLEBUF) alert1_font = pygame.font.SysFont("arial", 30) alert2_font = pygame.font.SysFont("arial", 20) info_font = pygame.font.SysFont("arial", 15) camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert() cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert() cameraw_test_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24) top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8) frame = context.socket(zmq.SUB) frame.connect(frame_address or "tcp://%s:%d" % (addr, service_list['frame'].port)) frame.setsockopt(zmq.SUBSCRIBE, b"") frame.setsockopt(zmq.CONFLATE, 1) carState = sub_sock(service_list['carState'].port, addr=addr, conflate=True) plan = sub_sock(service_list['plan'].port, addr=addr, conflate=True) carControl = sub_sock(service_list['carControl'].port, addr=addr, conflate=True) radar_state_sock = sub_sock(service_list['radarState'].port, addr=addr, conflate=True) liveCalibration = sub_sock(service_list['liveCalibration'].port, addr=addr, conflate=True) controls_state_sock = sub_sock(service_list['controlsState'].port, addr=addr, conflate=True) liveTracks = sub_sock(service_list['liveTracks'].port, addr=addr, conflate=True) model = sub_sock(service_list['model'].port, addr=addr, conflate=True) test_model = sub_sock(8040, addr=addr, conflate=True) liveMpc = sub_sock(service_list['liveMpc'].port, addr=addr, conflate=True) liveParameters = sub_sock(service_list['liveParameters'].port, addr=addr, conflate=True) pathPlan = sub_sock(service_list['pathPlan'].port, addr=addr, conflate=True) v_ego, angle_steers, angle_steers_des = 0., 0., 0. params_ao, params_ao_average, params_stiffness, params_sr = None, None, None, None enabled = False gas = 0. accel_override = 0. computer_gas = 0. brake = 0. steer_torque = 0. angle_steers_k = np.inf curvature = 0. computer_brake = 0. plan_source = 'none' long_control_state = 'none' d_poly = None model_data = None test_model_data = None a_ego = 0.0 a_target = 0.0 d_rel, y_rel, lead_status = 0., 0., False d_rel2, y_rel2, lead_status2 = 0., 0., False v_ego, v_pid, v_cruise, v_override = 0., 0., 0., 0. brake_lights = False alert_text1, alert_text2 = "", "" intrinsic_matrix = None calibration = None #img = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype='uint8') img = np.zeros((480, 640, 3), dtype='uint8') imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8) imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image good_lt = None lid_overlay_blank = get_blank_lid_overlay(UP) img_offset = (0, 0) # plots name_to_arr_idx = { "gas": 0, "computer_gas": 1, "user_brake": 2, "computer_brake": 3, "v_ego": 4, "v_pid": 5, "angle_steers_des": 6, "angle_steers": 7, "angle_steers_k": 8, "steer_torque": 9, "v_override": 10, "v_cruise": 11, "a_ego": 12, "a_target": 13, "accel_override": 14 } plot_arr = np.zeros((100, len(name_to_arr_idx.values()))) plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])] plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)] plot_names = [[ "gas", "computer_gas", "user_brake", "computer_brake", "accel_override" ], ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"], ["v_ego", "v_override", "v_pid", "v_cruise"], ["a_ego", "a_target"]] plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "y", "r"], ["b", "g", "r", "y"], ["b", "r"]] plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-"]] draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True) counter = 0 while 1: list(pygame.event.get()) screen.fill((64, 64, 64)) lid_overlay = lid_overlay_blank.copy() top_down = top_down_surface, lid_overlay # ***** frame ***** fpkt = recv_one(frame) rgb_img_raw = fpkt.frame.image if fpkt.frame.transform: img_transform = np.array(fpkt.frame.transform).reshape(3, 3) else: # assume frame is flipped img_transform = np.array([[-1.0, 0.0, FULL_FRAME_SIZE[0] - 1], [0.0, -1.0, FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]]) if rgb_img_raw and len( rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3: imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape( (FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3)) imgff = imgff[:, :, ::-1] # Convert BGR to RGB cv2.warpAffine(imgff, np.dot(img_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) intrinsic_matrix = eon_intrinsics else: img.fill(0) intrinsic_matrix = np.eye(3) if calibration is not None: transform = np.dot(img_transform, calibration.model_to_full_frame) imgw = cv2.warpAffine(imgff, transform[:2], (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]), flags=cv2.WARP_INVERSE_MAP) else: imgw.fill(0) imgw_test_model = imgw.copy() # ***** controlsState ***** controls_state = recv_one_or_none(controls_state_sock) if controls_state is not None: v_ego = controls_state.controlsState.vEgo angle_steers = controls_state.controlsState.angleSteers w = controls_state.controlsState.lateralControlState.which() if w == 'lqrState': angle_steers_k = controls_state.controlsState.lateralControlState.lqrState.steerAngle elif w == 'indiState': angle_steers_k = controls_state.controlsState.lateralControlState.indiState.steerAngle curvature = controls_state.controlsState.curvature v_pid = controls_state.controlsState.vPid enabled = controls_state.controlsState.enabled alert_text1 = controls_state.controlsState.alertText1 alert_text2 = controls_state.controlsState.alertText2 long_control_state = controls_state.controlsState.longControlState cs = recv_one_or_none(carState) if cs is not None: gas = cs.carState.gas brake_lights = cs.carState.brakeLights a_ego = cs.carState.aEgo brake = cs.carState.brake v_cruise = cs.carState.cruiseState.speed cc = recv_one_or_none(carControl) if cc is not None: v_override = cc.carControl.cruiseControl.speedOverride computer_brake = cc.carControl.actuators.brake computer_gas = cc.carControl.actuators.gas steer_torque = cc.carControl.actuators.steer * ANGLE_SCALE angle_steers_des = cc.carControl.actuators.steerAngle accel_override = cc.carControl.cruiseControl.accelOverride p = recv_one_or_none(plan) if p is not None: a_target = p.plan.aTarget plan_source = p.plan.longitudinalPlanSource pp = recv_one_or_none(pathPlan) if pp is not None: d_poly = np.array(pp.pathPlan.dPoly) plot_arr[:-1] = plot_arr[1:] plot_arr[-1, name_to_arr_idx['angle_steers']] = angle_steers plot_arr[-1, name_to_arr_idx['angle_steers_des']] = angle_steers_des plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k plot_arr[-1, name_to_arr_idx['gas']] = gas plot_arr[-1, name_to_arr_idx['computer_gas']] = computer_gas plot_arr[-1, name_to_arr_idx['user_brake']] = brake plot_arr[-1, name_to_arr_idx['steer_torque']] = steer_torque plot_arr[-1, name_to_arr_idx['computer_brake']] = computer_brake plot_arr[-1, name_to_arr_idx['v_ego']] = v_ego plot_arr[-1, name_to_arr_idx['v_pid']] = v_pid plot_arr[-1, name_to_arr_idx['v_override']] = v_override plot_arr[-1, name_to_arr_idx['v_cruise']] = v_cruise plot_arr[-1, name_to_arr_idx['a_ego']] = a_ego plot_arr[-1, name_to_arr_idx['a_target']] = a_target plot_arr[-1, name_to_arr_idx['accel_override']] = accel_override # ***** model **** # live model md = recv_one_or_none(model) if md: model_data = extract_model_data(md) if model_data: plot_model(model_data, VM, v_ego, curvature, imgw, calibration, top_down, d_poly) if test_model is not None: test_md = recv_one_or_none(test_model) if test_md: test_model_data = extract_model_data(test_md) if test_model_data: plot_model(test_model_data, VM, v_ego, curvature, imgw_test_model, calibration, top_down, 215) # MPC mpc = recv_one_or_none(liveMpc) if mpc: draw_mpc(mpc, top_down) # LiveParams params = recv_one_or_none(liveParameters) if params: params_ao = params.liveParameters.angleOffset params_ao_average = params.liveParameters.angleOffsetAverage params_stiffness = params.liveParameters.stiffnessFactor params_sr = params.liveParameters.steerRatio # **** tracks ***** # draw all radar points lt = recv_one_or_none(liveTracks) if lt is not None: good_lt = lt if good_lt is not None: maybe_update_radar_points(good_lt, top_down[1]) # ***** radarState ***** radar_state = recv_one_or_none(radar_state_sock) if radar_state is not None: d_rel = radar_state.radarState.leadOne.dRel + RADAR_TO_CENTER y_rel = radar_state.radarState.leadOne.yRel lead_status = radar_state.radarState.leadOne.status d_rel2 = radar_state.radarState.leadTwo.dRel + RADAR_TO_CENTER y_rel2 = radar_state.radarState.leadTwo.yRel lead_status2 = radar_state.radarState.leadTwo.status lcal = recv_one_or_none(liveCalibration) if lcal is not None: calibration_message = lcal.liveCalibration extrinsic_matrix = np.asarray( calibration_message.extrinsicMatrix).reshape(3, 4) ke = intrinsic_matrix.dot(extrinsic_matrix) warp_matrix = get_camera_frame_from_model_frame(ke) calibration = CalibrationTransformsForWarpMatrix( warp_matrix, intrinsic_matrix, extrinsic_matrix) # draw red pt for lead car in the main img if lead_status: if calibration is not None: dx, dy = draw_lead_on(img, d_rel, y_rel, img_offset, calibration, color=(192, 0, 0)) # draw red line for lead car draw_lead_car(d_rel, top_down) # draw red pt for lead car2 in the main img if lead_status2: if calibration is not None: dx2, dy2 = draw_lead_on(img, d_rel2, y_rel2, img_offset, calibration, color=(192, 0, 0)) # draw red line for lead car draw_lead_car(d_rel2, top_down) # *** blits *** pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1)) screen.blit(camera_surface, (0, 0)) # display alerts alert_line1 = alert1_font.render(alert_text1, True, (255, 0, 0)) alert_line2 = alert2_font.render(alert_text2, True, (255, 0, 0)) screen.blit(alert_line1, (180, 150)) screen.blit(alert_line2, (180, 190)) if calibration is not None and img is not None: cpw = warp_points(CalP, calibration.model_to_bb) vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb) pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1) pygame.draw.circle(screen, BLUE, list(map(int, map(round, vanishing_pointw[0]))), 2) if HOR: screen.blit(draw_plots(plot_arr), (640 + 384, 0)) else: screen.blit(draw_plots(plot_arr), (0, 600)) pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1)) screen.blit(cameraw_surface, (320, 480)) pygame.surfarray.blit_array(cameraw_test_surface, imgw_test_model.swapaxes(0, 1)) screen.blit(cameraw_test_surface, (0, 480)) pygame.surfarray.blit_array(*top_down) screen.blit(top_down[0], (640, 0)) i = 0 SPACING = 25 # enabled enabled_line = info_font.render("ENABLED", True, GREEN if enabled else BLACK) screen.blit(enabled_line, (write_x, write_y + i * SPACING)) i += 1 # brake lights brake_lights_line = info_font.render("BRAKE LIGHTS", True, RED if brake_lights else BLACK) screen.blit(brake_lights_line, (write_x, write_y + i * SPACING)) i += 1 # speed v_ego_line = info_font.render( "SPEED: " + str(round(v_ego, 1)) + " m/s", True, YELLOW) screen.blit(v_ego_line, (write_x, write_y + i * SPACING)) i += 1 # long control state long_control_state_line = info_font.render( "LONG CONTROL STATE: " + str(long_control_state), True, YELLOW) screen.blit(long_control_state_line, (write_x, write_y + i * SPACING)) i += 1 # long mpc source plan_source_line = info_font.render( "LONG MPC SOURCE: " + str(plan_source), True, YELLOW) screen.blit(plan_source_line, (write_x, write_y + i * SPACING)) i += 1 if params_ao is not None: i += 1 angle_offset_avg_line = info_font.render( "ANGLE OFFSET (AVG): " + str(round(params_ao_average, 2)) + " deg", True, YELLOW) screen.blit(angle_offset_avg_line, (write_x, write_y + i * SPACING)) i += 1 angle_offset_line = info_font.render( "ANGLE OFFSET (INSTANT): " + str(round(params_ao, 2)) + " deg", True, YELLOW) screen.blit(angle_offset_line, (write_x, write_y + i * SPACING)) i += 1 angle_offset_line = info_font.render( "STIFFNESS: " + str(round(params_stiffness * 100., 2)) + " %", True, YELLOW) screen.blit(angle_offset_line, (write_x, write_y + i * SPACING)) i += 1 steer_ratio_line = info_font.render( "STEER RATIO: " + str(round(params_sr, 2)), True, YELLOW) screen.blit(steer_ratio_line, (write_x, write_y + i * SPACING)) i += 1 # this takes time...vsync or something pygame.display.flip()
def ui_thread(addr, frame_address): # TODO: Detect car from replay and use that to select carparams CP = ToyotaInterface.get_params("TOYOTA PRIUS 2017") VM = VehicleModel(CP) CalP = np.asarray([[0, 0], [MODEL_INPUT_SIZE[0], 0], [MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]], [0, MODEL_INPUT_SIZE[1]]]) vanishing_point = np.asarray([[MODEL_CX, MODEL_CY]]) pygame.init() pygame.font.init() assert pygame_modules_have_loaded() if HOR: size = (640 + 384 + 640, 960) write_x = 5 write_y = 680 else: size = (640 + 384, 960 + 300) write_x = 645 write_y = 970 pygame.display.set_caption("openpilot debug UI") screen = pygame.display.set_mode(size, pygame.DOUBLEBUF) alert1_font = pygame.font.SysFont("arial", 30) alert2_font = pygame.font.SysFont("arial", 20) info_font = pygame.font.SysFont("arial", 15) camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert() cameraw_surface = pygame.surface.Surface(MODEL_INPUT_SIZE, 0, 24).convert() top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8) frame = messaging.sub_sock('frame', addr=addr, conflate=True) sm = messaging.SubMaster([ 'carState', 'plan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', 'liveTracks', 'model', 'liveMpc', 'liveParameters', 'pathPlan' ], addr=addr) calibration = None img = np.zeros((480, 640, 3), dtype='uint8') imgff = np.zeros((FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3), dtype=np.uint8) imgw = np.zeros((160, 320, 3), dtype=np.uint8) # warped image lid_overlay_blank = get_blank_lid_overlay(UP) # plots name_to_arr_idx = { "gas": 0, "computer_gas": 1, "user_brake": 2, "computer_brake": 3, "v_ego": 4, "v_pid": 5, "angle_steers_des": 6, "angle_steers": 7, "angle_steers_k": 8, "steer_torque": 9, "v_override": 10, "v_cruise": 11, "a_ego": 12, "a_target": 13, "accel_override": 14 } plot_arr = np.zeros((100, len(name_to_arr_idx.values()))) plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])] plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0., 75.), (-3.0, 2.0)] plot_names = [[ "gas", "computer_gas", "user_brake", "computer_brake", "accel_override" ], ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"], ["v_ego", "v_override", "v_pid", "v_cruise"], ["a_ego", "a_target"]] plot_colors = [["b", "b", "g", "r", "y"], ["b", "g", "y", "r"], ["b", "g", "r", "y"], ["b", "r"]] plot_styles = [["-", "-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-", "-", "-"], ["-", "-"]] draw_plots = init_plots(plot_arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles, bigplots=True) while 1: list(pygame.event.get()) screen.fill((64, 64, 64)) lid_overlay = lid_overlay_blank.copy() top_down = top_down_surface, lid_overlay # ***** frame ***** fpkt = messaging.recv_one(frame) rgb_img_raw = fpkt.frame.image if fpkt.frame.transform: img_transform = np.array(fpkt.frame.transform).reshape(3, 3) else: # assume frame is flipped img_transform = np.array([[-1.0, 0.0, FULL_FRAME_SIZE[0] - 1], [0.0, -1.0, FULL_FRAME_SIZE[1] - 1], [0.0, 0.0, 1.0]]) if rgb_img_raw and len( rgb_img_raw) == FULL_FRAME_SIZE[0] * FULL_FRAME_SIZE[1] * 3: imgff = np.frombuffer(rgb_img_raw, dtype=np.uint8).reshape( (FULL_FRAME_SIZE[1], FULL_FRAME_SIZE[0], 3)) imgff = imgff[:, :, ::-1] # Convert BGR to RGB cv2.warpAffine(imgff, np.dot(img_transform, _BB_TO_FULL_FRAME)[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) intrinsic_matrix = eon_intrinsics else: img.fill(0) intrinsic_matrix = np.eye(3) if calibration is not None: transform = np.dot(img_transform, calibration.model_to_full_frame) imgw = cv2.warpAffine(imgff, transform[:2], (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1]), flags=cv2.WARP_INVERSE_MAP) else: imgw.fill(0) sm.update() w = sm['controlsState'].lateralControlState.which() if w == 'lqrState': angle_steers_k = sm[ 'controlsState'].lateralControlState.lqrState.steerAngle elif w == 'indiState': angle_steers_k = sm[ 'controlsState'].lateralControlState.indiState.steerAngle else: angle_steers_k = np.inf plot_arr[:-1] = plot_arr[1:] plot_arr[ -1, name_to_arr_idx['angle_steers']] = sm['controlsState'].angleSteers plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm[ 'carControl'].actuators.steerAngle plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas plot_arr[ -1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake plot_arr[-1, name_to_arr_idx['steer_torque']] = sm[ 'carControl'].actuators.steer * ANGLE_SCALE plot_arr[-1, name_to_arr_idx['computer_brake']] = sm[ 'carControl'].actuators.brake plot_arr[-1, name_to_arr_idx['v_ego']] = sm['controlsState'].vEgo plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid plot_arr[-1, name_to_arr_idx['v_override']] = sm[ 'carControl'].cruiseControl.speedOverride plot_arr[ -1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo plot_arr[-1, name_to_arr_idx['a_target']] = sm['plan'].aTarget plot_arr[-1, name_to_arr_idx['accel_override']] = sm[ 'carControl'].cruiseControl.accelOverride # ***** model **** if len(sm['model'].path.poly) > 0: model_data = extract_model_data(sm['model']) plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration, top_down, np.array(sm['pathPlan'].dPoly)) # MPC if sm.updated['liveMpc']: draw_mpc(sm['liveMpc'], top_down) # draw all radar points maybe_update_radar_points(sm['liveTracks'], top_down[1]) if sm.updated['liveCalibration']: extrinsic_matrix = np.asarray( sm['liveCalibration'].extrinsicMatrix).reshape(3, 4) ke = intrinsic_matrix.dot(extrinsic_matrix) warp_matrix = get_camera_frame_from_model_frame(ke) calibration = CalibrationTransformsForWarpMatrix( warp_matrix, intrinsic_matrix, extrinsic_matrix) # draw red pt for lead car in the main img for lead in [sm['radarState'].leadOne, sm['radarState'].leadTwo]: if lead.status: if calibration is not None: draw_lead_on(img, lead.dRel, lead.yRel, calibration, color=(192, 0, 0)) draw_lead_car(lead.dRel, top_down) # *** blits *** pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1)) screen.blit(camera_surface, (0, 0)) # display alerts alert_line1 = alert1_font.render(sm['controlsState'].alertText1, True, (255, 0, 0)) alert_line2 = alert2_font.render(sm['controlsState'].alertText2, True, (255, 0, 0)) screen.blit(alert_line1, (180, 150)) screen.blit(alert_line2, (180, 190)) if calibration is not None and img is not None: cpw = warp_points(CalP, calibration.model_to_bb) vanishing_pointw = warp_points(vanishing_point, calibration.model_to_bb) pygame.draw.polygon(screen, BLUE, tuple(map(tuple, cpw)), 1) pygame.draw.circle(screen, BLUE, list(map(int, map(round, vanishing_pointw[0]))), 2) if HOR: screen.blit(draw_plots(plot_arr), (640 + 384, 0)) else: screen.blit(draw_plots(plot_arr), (0, 600)) pygame.surfarray.blit_array(cameraw_surface, imgw.swapaxes(0, 1)) screen.blit(cameraw_surface, (320, 480)) pygame.surfarray.blit_array(*top_down) screen.blit(top_down[0], (640, 0)) i = 0 SPACING = 25 lines = [ info_font.render("ENABLED", True, GREEN if sm['controlsState'].enabled else BLACK), info_font.render("BRAKE LIGHTS", True, RED if sm['carState'].brakeLights else BLACK), info_font.render( "SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", True, YELLOW), info_font.render( "LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), True, YELLOW), info_font.render( "LONG MPC SOURCE: " + str(sm['plan'].longitudinalPlanSource), True, YELLOW), None, info_font.render( "ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverage, 2)) + " deg", True, YELLOW), info_font.render( "ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffset, 2)) + " deg", True, YELLOW), info_font.render( "STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) + " %", True, YELLOW), info_font.render( "STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW) ] for i, line in enumerate(lines): if line is not None: screen.blit(line, (write_x, write_y + i * SPACING)) # this takes time...vsync or something pygame.display.flip()