def send_data(self, livecalibration):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
ke = eon_intrinsics.dot(extrinsic_matrix)
warp_matrix = get_camera_frame_from_model_frame(ke, model_height)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100)
cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten())
cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten())
livecalibration.send(cal_send.to_bytes())
def send_data(self, livecalibration):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(
0, calib[1], calib[2], model_height)
ke = eon_intrinsics.dot(extrinsic_matrix)
warp_matrix = get_camera_frame_from_model_frame(ke)
warp_matrix_big = get_camera_frame_from_bigmodel_frame(ke)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(
len(self.vps) * 100 // INPUTS_NEEDED, 100)
cal_send.liveCalibration.warpMatrix2 = [
float(x) for x in warp_matrix.flatten()
]
cal_send.liveCalibration.warpMatrixBig = [
float(x) for x in warp_matrix_big.flatten()
]
cal_send.liveCalibration.extrinsicMatrix = [
float(x) for x in extrinsic_matrix.flatten()
]
livecalibration.send(cal_send.to_bytes())
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()
