mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-29 18:42:07 +08:00
@@ -113,7 +113,6 @@ dev = [
|
||||
"opencv-python-headless",
|
||||
"parameterized >=0.8, <0.9",
|
||||
"pyautogui",
|
||||
"pygame",
|
||||
"pyopencl; platform_machine != 'aarch64'", # broken on arm64
|
||||
"pytools>=2025.1.6; platform_machine != 'aarch64'",
|
||||
"pywinctl",
|
||||
|
||||
@@ -1,9 +1,8 @@
|
||||
import itertools
|
||||
from typing import Any
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pygame
|
||||
import pyray as rl
|
||||
|
||||
from matplotlib.backends.backend_agg import FigureCanvasAgg
|
||||
|
||||
@@ -18,21 +17,25 @@ YELLOW = (255, 255, 0)
|
||||
BLACK = (0, 0, 0)
|
||||
WHITE = (255, 255, 255)
|
||||
|
||||
|
||||
class UIParams:
|
||||
lidar_x, lidar_y, lidar_zoom = 384, 960, 6
|
||||
lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
|
||||
lidar_car_x, lidar_car_y = lidar_x / 2.0, lidar_y / 1.1
|
||||
car_hwidth = 1.7272 / 2 * lidar_zoom
|
||||
car_front = 2.6924 * lidar_zoom
|
||||
car_back = 1.8796 * lidar_zoom
|
||||
car_color = 110
|
||||
|
||||
|
||||
UP = UIParams
|
||||
|
||||
METER_WIDTH = 20
|
||||
|
||||
|
||||
class Calibration:
|
||||
def __init__(self, num_px, rpy, intrinsic, calib_scale):
|
||||
self.intrinsic = intrinsic
|
||||
self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:,:3]
|
||||
self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:, :3]
|
||||
self.zoom = calib_scale
|
||||
|
||||
def car_space_to_ff(self, x, y, z):
|
||||
@@ -47,19 +50,18 @@ class Calibration:
|
||||
return pts / self.zoom
|
||||
|
||||
|
||||
_COLOR_CACHE : dict[tuple[int, int, int], Any] = {}
|
||||
_COLOR_CACHE: dict[tuple[int, int, int], int] = {
|
||||
(255, 0, 0): 1, # RED
|
||||
(0, 255, 0): 2, # GREEN
|
||||
(0, 0, 255): 3, # BLUE
|
||||
(255, 255, 0): 4, # YELLOW
|
||||
(0, 0, 0): 0, # BLACK
|
||||
(255, 255, 255): 255, # WHITE
|
||||
}
|
||||
|
||||
|
||||
def find_color(lidar_surface, color):
|
||||
if color in _COLOR_CACHE:
|
||||
return _COLOR_CACHE[color]
|
||||
tcolor = 0
|
||||
ret = 255
|
||||
for x in lidar_surface.get_palette():
|
||||
if x[0:3] == color:
|
||||
ret = tcolor
|
||||
break
|
||||
tcolor += 1
|
||||
_COLOR_CACHE[color] = ret
|
||||
return ret
|
||||
return _COLOR_CACHE.get(color, 255)
|
||||
|
||||
|
||||
def to_topdown_pt(y, x):
|
||||
@@ -91,13 +93,7 @@ def draw_path(path, color, img, calibration, top_down, lid_color=None, z_off=0):
|
||||
|
||||
|
||||
def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles):
|
||||
color_palette = { "r": (1, 0, 0),
|
||||
"g": (0, 1, 0),
|
||||
"b": (0, 0, 1),
|
||||
"k": (0, 0, 0),
|
||||
"y": (1, 1, 0),
|
||||
"p": (0, 1, 1),
|
||||
"m": (1, 0, 1)}
|
||||
color_palette = {"r": (1, 0, 0), "g": (0, 1, 0), "b": (0, 0, 1), "k": (0, 0, 0), "y": (1, 1, 0), "p": (0, 1, 1), "m": (1, 0, 1)}
|
||||
|
||||
dpi = 90
|
||||
fig = plt.figure(figsize=(575 / dpi, 600 / dpi), dpi=dpi)
|
||||
@@ -107,7 +103,7 @@ def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_co
|
||||
|
||||
axs = []
|
||||
for pn in range(len(plot_ylims)):
|
||||
ax = fig.add_subplot(len(plot_ylims), 1, len(axs)+1)
|
||||
ax = fig.add_subplot(len(plot_ylims), 1, len(axs) + 1)
|
||||
ax.set_xlim(plot_xlims[pn][0], plot_xlims[pn][1])
|
||||
ax.set_ylim(plot_ylims[pn][0], plot_ylims[pn][1])
|
||||
ax.patch.set_facecolor((0.4, 0.4, 0.4))
|
||||
@@ -116,15 +112,11 @@ def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_co
|
||||
plots, idxs, plot_select = [], [], []
|
||||
for i, pl_list in enumerate(plot_names):
|
||||
for j, item in enumerate(pl_list):
|
||||
plot, = axs[i].plot(arr[:, name_to_arr_idx[item]],
|
||||
label=item,
|
||||
color=color_palette[plot_colors[i][j]],
|
||||
linestyle=plot_styles[i][j])
|
||||
(plot,) = axs[i].plot(arr[:, name_to_arr_idx[item]], label=item, color=color_palette[plot_colors[i][j]], linestyle=plot_styles[i][j])
|
||||
plots.append(plot)
|
||||
idxs.append(name_to_arr_idx[item])
|
||||
plot_select.append(i)
|
||||
axs[i].set_title(", ".join(f"{nm} ({cl})"
|
||||
for (nm, cl) in zip(pl_list, plot_colors[i], strict=False)), fontsize=10)
|
||||
axs[i].set_title(", ".join(f"{nm} ({cl})" for (nm, cl) in zip(pl_list, plot_colors[i], strict=False)), fontsize=10)
|
||||
axs[i].tick_params(axis="x", colors="white")
|
||||
axs[i].tick_params(axis="y", colors="white")
|
||||
axs[i].title.set_color("white")
|
||||
@@ -134,6 +126,12 @@ def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_co
|
||||
|
||||
canvas.draw()
|
||||
|
||||
# Pre-create texture for plots (reuse each frame to avoid log spam)
|
||||
w, h = canvas.get_width_height()
|
||||
plot_image = rl.gen_image_color(w, h, rl.BLACK)
|
||||
plot_texture = rl.load_texture_from_image(plot_image)
|
||||
rl.unload_image(plot_image)
|
||||
|
||||
def draw_plots(arr):
|
||||
for ax in axs:
|
||||
ax.draw_artist(ax.patch)
|
||||
@@ -141,17 +139,13 @@ def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_co
|
||||
plots[i].set_ydata(arr[:, idxs[i]])
|
||||
axs[plot_select[i]].draw_artist(plots[i])
|
||||
|
||||
raw_data = canvas.buffer_rgba()
|
||||
plot_surface = pygame.image.frombuffer(raw_data, canvas.get_width_height(), "RGBA").convert()
|
||||
return plot_surface
|
||||
raw_data = np.ascontiguousarray(canvas.buffer_rgba(), dtype=np.uint8)
|
||||
rl.update_texture(plot_texture, rl.ffi.cast("void *", raw_data.ctypes.data))
|
||||
return plot_texture
|
||||
|
||||
return draw_plots
|
||||
|
||||
|
||||
def pygame_modules_have_loaded():
|
||||
return pygame.display.get_init() and pygame.font.get_init()
|
||||
|
||||
|
||||
def plot_model(m, img, calibration, top_down):
|
||||
if calibration is None or top_down is None:
|
||||
return
|
||||
@@ -166,7 +160,7 @@ def plot_model(m, img, calibration, top_down):
|
||||
|
||||
_, py_top = to_topdown_pt(x + x_std, y)
|
||||
px, py_bottom = to_topdown_pt(x - x_std, y)
|
||||
top_down[1][int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = find_color(top_down[0], YELLOW)
|
||||
top_down[1][int(round(px - 4)) : int(round(px + 4)), py_top:py_bottom] = find_color(top_down[0], YELLOW)
|
||||
|
||||
for path, prob, _ in zip(m.laneLines, m.laneLineProbs, m.laneLineStds, strict=True):
|
||||
color = (0, int(255 * prob), 0)
|
||||
@@ -202,22 +196,15 @@ def maybe_update_radar_points(lt, lid_overlay):
|
||||
# negative here since radar is left positive
|
||||
px, py = to_topdown_pt(pt[0], -pt[1])
|
||||
if px != -1:
|
||||
lid_overlay[px - 4:px + 4, py - 4:py + 4] = 0
|
||||
lid_overlay[px - 2:px + 2, py - 2:py + 2] = 255
|
||||
lid_overlay[px - 4 : px + 4, py - 4 : py + 4] = 0
|
||||
lid_overlay[px - 2 : px + 2, py - 2 : py + 2] = 255
|
||||
|
||||
|
||||
def get_blank_lid_overlay(UP):
|
||||
lid_overlay = np.zeros((UP.lidar_x, UP.lidar_y), 'uint8')
|
||||
# Draw the car.
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y -
|
||||
UP.car_front))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)):int(
|
||||
round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y +
|
||||
UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x + UP.car_hwidth)), int(
|
||||
round(UP.lidar_car_y - UP.car_front)):int(round(
|
||||
UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)) : int(round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y - UP.car_front))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)) : int(round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x - UP.car_hwidth)), int(round(UP.lidar_car_y - UP.car_front)) : int(round(UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
lid_overlay[int(round(UP.lidar_car_x + UP.car_hwidth)), int(round(UP.lidar_car_y - UP.car_front)) : int(round(UP.lidar_car_y + UP.car_back))] = UP.car_color
|
||||
return lid_overlay
|
||||
|
||||
+143
-101
@@ -5,57 +5,88 @@ import sys
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pygame
|
||||
import pyray as rl
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.tools.replay.lib.ui_helpers import (UP,
|
||||
BLACK, GREEN,
|
||||
YELLOW, Calibration,
|
||||
get_blank_lid_overlay, init_plots,
|
||||
maybe_update_radar_points, plot_lead,
|
||||
plot_model,
|
||||
pygame_modules_have_loaded)
|
||||
from openpilot.tools.replay.lib.ui_helpers import (
|
||||
UP,
|
||||
BLACK,
|
||||
GREEN,
|
||||
YELLOW,
|
||||
Calibration,
|
||||
get_blank_lid_overlay,
|
||||
init_plots,
|
||||
maybe_update_radar_points,
|
||||
plot_lead,
|
||||
plot_model,
|
||||
)
|
||||
from msgq.visionipc import VisionIpcClient, VisionStreamType
|
||||
|
||||
os.environ['BASEDIR'] = BASEDIR
|
||||
|
||||
ANGLE_SCALE = 5.0
|
||||
|
||||
|
||||
def ui_thread(addr):
|
||||
cv2.setNumThreads(1)
|
||||
pygame.init()
|
||||
pygame.font.init()
|
||||
assert pygame_modules_have_loaded()
|
||||
|
||||
disp_info = pygame.display.Info()
|
||||
max_height = disp_info.current_h
|
||||
# Get monitor info before creating window
|
||||
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
|
||||
rl.init_window(1, 1, "")
|
||||
max_height = rl.get_monitor_height(0)
|
||||
rl.close_window()
|
||||
|
||||
hor_mode = os.getenv("HORIZONTAL") is not None
|
||||
hor_mode = True if max_height < 960+300 else hor_mode
|
||||
hor_mode = True if max_height < 960 + 300 else hor_mode
|
||||
|
||||
if hor_mode:
|
||||
size = (640+384+640, 960)
|
||||
size = (640 + 384 + 640, 960)
|
||||
write_x = 5
|
||||
write_y = 680
|
||||
else:
|
||||
size = (640+384, 960+300)
|
||||
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)
|
||||
rl.set_trace_log_level(rl.TraceLogLevel.LOG_ERROR)
|
||||
rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
|
||||
rl.init_window(size[0], size[1], "openpilot debug UI")
|
||||
rl.set_target_fps(60)
|
||||
|
||||
alert1_font = pygame.font.SysFont("arial", 30)
|
||||
alert2_font = pygame.font.SysFont("arial", 20)
|
||||
info_font = pygame.font.SysFont("arial", 15)
|
||||
# Load font
|
||||
font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf")
|
||||
font = rl.load_font_ex(font_path, 32, None, 0)
|
||||
|
||||
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
|
||||
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
|
||||
# Create textures for camera and top-down view
|
||||
camera_image = rl.gen_image_color(640, 480, rl.BLACK)
|
||||
camera_texture = rl.load_texture_from_image(camera_image)
|
||||
rl.unload_image(camera_image)
|
||||
|
||||
sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState',
|
||||
'selfdriveState', 'liveTracks', 'modelV2', 'liveParameters', 'roadCameraState'], addr=addr)
|
||||
# lid_overlay array is (lidar_x, lidar_y) = (384, 960)
|
||||
# pygame treats first axis as width, so texture is 384 wide x 960 tall
|
||||
# For raylib, we need to transpose to get (height, width) = (960, 384) for the RGBA array
|
||||
top_down_image = rl.gen_image_color(UP.lidar_x, UP.lidar_y, rl.BLACK)
|
||||
top_down_texture = rl.load_texture_from_image(top_down_image)
|
||||
rl.unload_image(top_down_image)
|
||||
|
||||
sm = messaging.SubMaster(
|
||||
[
|
||||
'carState',
|
||||
'longitudinalPlan',
|
||||
'carControl',
|
||||
'radarState',
|
||||
'liveCalibration',
|
||||
'controlsState',
|
||||
'selfdriveState',
|
||||
'liveTracks',
|
||||
'modelV2',
|
||||
'liveParameters',
|
||||
'roadCameraState',
|
||||
],
|
||||
addr=addr,
|
||||
)
|
||||
|
||||
img = np.zeros((480, 640, 3), dtype='uint8')
|
||||
imgff = None
|
||||
@@ -65,74 +96,78 @@ def ui_thread(addr):
|
||||
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}
|
||||
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,
|
||||
}
|
||||
|
||||
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"],
|
||||
["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 = [["-", "-", "-", "-", "-"],
|
||||
["-", "-", "-", "-"],
|
||||
["-", "-", "-", "-"],
|
||||
["-", "-"]]
|
||||
plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.0, 2.0)]
|
||||
plot_names = [
|
||||
["gas", "computer_gas", "user_brake", "computer_brake"],
|
||||
["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)
|
||||
|
||||
# Palette for converting lid_overlay grayscale indices to RGBA colors
|
||||
palette = np.zeros((256, 4), dtype=np.uint8)
|
||||
palette[:, 3] = 255 # alpha
|
||||
palette[1] = [255, 0, 0, 255] # RED
|
||||
palette[2] = [0, 255, 0, 255] # GREEN
|
||||
palette[3] = [0, 0, 255, 255] # BLUE
|
||||
palette[4] = [255, 255, 0, 255] # YELLOW
|
||||
palette[110] = [110, 110, 110, 255] # car_color (gray)
|
||||
palette[255] = [255, 255, 255, 255] # WHITE
|
||||
|
||||
vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
|
||||
while True:
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
pygame.quit()
|
||||
sys.exit()
|
||||
|
||||
screen.fill((64, 64, 64))
|
||||
lid_overlay = lid_overlay_blank.copy()
|
||||
top_down = top_down_surface, lid_overlay
|
||||
|
||||
while not rl.window_should_close():
|
||||
# ***** frame *****
|
||||
if not vipc_client.is_connected():
|
||||
vipc_client.connect(True)
|
||||
vipc_client.connect(False)
|
||||
|
||||
rl.begin_drawing()
|
||||
rl.clear_background(rl.Color(64, 64, 64, 255))
|
||||
|
||||
yuv_img_raw = vipc_client.recv()
|
||||
if yuv_img_raw is None or not yuv_img_raw.data.any():
|
||||
rl.draw_text_ex(font, "waiting for frames", rl.Vector2(200, 200), 30, 0, rl.WHITE)
|
||||
rl.end_drawing()
|
||||
continue
|
||||
|
||||
lid_overlay = lid_overlay_blank.copy()
|
||||
top_down = top_down_texture, lid_overlay
|
||||
|
||||
sm.update(0)
|
||||
|
||||
camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))]
|
||||
|
||||
imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8).reshape((len(yuv_img_raw.data) // vipc_client.stride, vipc_client.stride))
|
||||
num_px = vipc_client.width * vipc_client.height
|
||||
rgb = cv2.cvtColor(imgff[:vipc_client.height * 3 // 2, :vipc_client.width], cv2.COLOR_YUV2RGB_NV12)
|
||||
rgb = cv2.cvtColor(imgff[: vipc_client.height * 3 // 2, : vipc_client.width], cv2.COLOR_YUV2RGB_NV12)
|
||||
|
||||
qcam = "QCAM" in os.environ
|
||||
bb_scale = (528 if qcam else camera.fcam.width) / 640.
|
||||
calib_scale = camera.fcam.width / 640.
|
||||
zoom_matrix = np.asarray([
|
||||
[bb_scale, 0., 0.],
|
||||
[0., bb_scale, 0.],
|
||||
[0., 0., 1.]])
|
||||
bb_scale = (528 if qcam else camera.fcam.width) / 640.0
|
||||
calib_scale = camera.fcam.width / 640.0
|
||||
zoom_matrix = np.asarray([[bb_scale, 0.0, 0.0], [0.0, bb_scale, 0.0], [0.0, 0.0, 1.0]])
|
||||
cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
|
||||
|
||||
intrinsic_matrix = camera.fcam.intrinsics
|
||||
@@ -151,11 +186,11 @@ def ui_thread(addr):
|
||||
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
|
||||
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gasDEPRECATED
|
||||
# TODO gas is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_gas']] = np.clip(sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['computer_gas']] = np.clip(sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake
|
||||
plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.torque * ANGLE_SCALE
|
||||
# TODO brake is deprecated
|
||||
plot_arr[-1, name_to_arr_idx['computer_brake']] = np.clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['computer_brake']] = np.clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
|
||||
plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo
|
||||
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
|
||||
@@ -177,56 +212,63 @@ def ui_thread(addr):
|
||||
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
|
||||
|
||||
# *** blits ***
|
||||
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))
|
||||
screen.blit(camera_surface, (0, 0))
|
||||
# Update camera texture from numpy array
|
||||
img_rgba = cv2.cvtColor(img, cv2.COLOR_RGB2RGBA)
|
||||
rl.update_texture(camera_texture, rl.ffi.cast("void *", img_rgba.ctypes.data))
|
||||
rl.draw_texture(camera_texture, 0, 0, rl.WHITE)
|
||||
|
||||
# display alerts
|
||||
alert_line1 = alert1_font.render(sm['selfdriveState'].alertText1, True, (255, 0, 0))
|
||||
alert_line2 = alert2_font.render(sm['selfdriveState'].alertText2, True, (255, 0, 0))
|
||||
screen.blit(alert_line1, (180, 150))
|
||||
screen.blit(alert_line2, (180, 190))
|
||||
rl.draw_text_ex(font, sm['selfdriveState'].alertText1, rl.Vector2(180, 150), 30, 0, rl.RED)
|
||||
rl.draw_text_ex(font, sm['selfdriveState'].alertText2, rl.Vector2(180, 190), 20, 0, rl.RED)
|
||||
|
||||
# draw plots (texture is reused internally)
|
||||
plot_texture = draw_plots(plot_arr)
|
||||
if hor_mode:
|
||||
screen.blit(draw_plots(plot_arr), (640+384, 0))
|
||||
rl.draw_texture(plot_texture, 640 + 384, 0, rl.WHITE)
|
||||
else:
|
||||
screen.blit(draw_plots(plot_arr), (0, 600))
|
||||
rl.draw_texture(plot_texture, 0, 600, rl.WHITE)
|
||||
|
||||
pygame.surfarray.blit_array(*top_down)
|
||||
screen.blit(top_down[0], (640, 0))
|
||||
# Convert lid_overlay to RGBA and update top_down texture
|
||||
# lid_overlay is (384, 960), need to transpose to (960, 384) for row-major RGBA buffer
|
||||
lid_rgba = palette[lid_overlay.T]
|
||||
rl.update_texture(top_down_texture, rl.ffi.cast("void *", np.ascontiguousarray(lid_rgba).ctypes.data))
|
||||
rl.draw_texture(top_down_texture, 640, 0, rl.WHITE)
|
||||
|
||||
SPACING = 25
|
||||
|
||||
lines = [
|
||||
info_font.render("ENABLED", True, GREEN if sm['selfdriveState'].enabled 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['longitudinalPlan'].longitudinalPlanSource), True, YELLOW),
|
||||
("ENABLED", GREEN if sm['selfdriveState'].enabled else BLACK),
|
||||
("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", YELLOW),
|
||||
("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), YELLOW),
|
||||
("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), YELLOW),
|
||||
None,
|
||||
info_font.render("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", True, YELLOW),
|
||||
info_font.render("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 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)
|
||||
("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", YELLOW),
|
||||
("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", YELLOW),
|
||||
("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100.0, 2)) + " %", YELLOW),
|
||||
("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), YELLOW),
|
||||
]
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if line is not None:
|
||||
screen.blit(line, (write_x, write_y + i * SPACING))
|
||||
color = rl.Color(line[1][0], line[1][1], line[1][2], 255)
|
||||
rl.draw_text_ex(font, line[0], rl.Vector2(write_x, write_y + i * SPACING), 20, 0, color)
|
||||
|
||||
rl.end_drawing()
|
||||
|
||||
rl.unload_texture(camera_texture)
|
||||
rl.unload_texture(top_down_texture)
|
||||
rl.unload_font(font)
|
||||
rl.close_window()
|
||||
|
||||
# this takes time...vsync or something
|
||||
pygame.display.flip()
|
||||
|
||||
def get_arg_parser():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Show replay data in a UI.",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
parser = argparse.ArgumentParser(description="Show replay data in a UI.", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("ip_address", nargs="?", default="127.0.0.1",
|
||||
help="The ip address on which to receive zmq messages.")
|
||||
parser.add_argument("ip_address", nargs="?", default="127.0.0.1", help="The ip address on which to receive zmq messages.")
|
||||
|
||||
parser.add_argument("--frame-address", default=None,
|
||||
help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
|
||||
parser.add_argument("--frame-address", default=None, help="The frame address (fully qualified ZMQ endpoint for frames) on which to receive zmq messages.")
|
||||
return parser
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_arg_parser().parse_args(sys.argv[1:])
|
||||
|
||||
|
||||
Reference in New Issue
Block a user