mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-18 08:22:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update ui.py

Browse Source
This commit is contained in:
royjr
2026-07-17 08:51:22 -04:00
parent 4157f882dd
commit 74b9619efe
1 changed files with 680 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/radar/ui.py
+680 -59
View File
@@ -4,11 +4,18 @@ import logging
import math
import multiprocessing
import os
import pty
import queue
import re
import signal
import struct
import subprocess
import sys
import termios
import time
from dataclasses import dataclass
from datetime import datetime, timedelta
import fcntl
import numpy as np
import pyray as rl
@@ -39,6 +46,8 @@ CAMERA_BUFFER_HEIGHT = 480
MAX_FORWARD_DISTANCE = 160.0
MAX_LATERAL_DISTANCE = 30.0
FUSED_CAMERA_ZOOM = 1.7
FUSED_STREAM_GRACE_SECONDS = 1.0
REPLAY_EXECUTABLE = os.path.join(BASEDIR, "tools/replay/replay")
BACKGROUND = rl.Color(11, 16, 24, 255)
PANEL = rl.Color(17, 25, 36, 255)
@@ -61,6 +70,137 @@ RADAR_DETAIL_SIGNALS = {
"AGE", "COAST_AGE", "STATE_ALT", "TRACK_COUNTER",
}
TABLE_MODES = ("comparison", "kinematics", "object")
PLAYBACK_SPEEDS = (0.2, 0.5, 1.0, 2.0, 4.0, 8.0)
@dataclass
class ManagedReplay:
process: subprocess.Popen
master_fd: int
paused: bool = False
playback_speed: float = 1.0
progress_current: int = 0
progress_total: int = 0
terminal_buffer: str = ""
def poll(self):
return self.process.poll()
def send(self, keys: str) -> None:
if self.master_fd >= 0 and self.poll() is None:
try:
os.write(self.master_fd, keys.encode())
except OSError:
pass
if keys == " ":
self.paused = not self.paused
elif keys == "+":
self.playback_speed = next(
(speed for speed in PLAYBACK_SPEEDS if speed > self.playback_speed),
self.playback_speed,
)
elif keys == "-":
self.playback_speed = next(
(speed for speed in reversed(PLAYBACK_SPEEDS) if speed < self.playback_speed),
self.playback_speed,
)
def drain_output(self) -> None:
if self.master_fd < 0:
return
output = bytearray()
while True:
try:
chunk = os.read(self.master_fd, 65536)
if not chunk:
break
output.extend(chunk)
except BlockingIOError:
break
except OSError:
break
if output:
self.terminal_buffer = (self.terminal_buffer + output.decode(errors="ignore"))[-65536:]
progress_matches = list(re.finditer(r"PROGRESS:(\d+):(\d+)", self.terminal_buffer))
if progress_matches:
self.progress_current = int(progress_matches[-1].group(1))
self.progress_total = int(progress_matches[-1].group(2))
def close_terminal(self) -> None:
if self.master_fd >= 0:
os.close(self.master_fd)
self.master_fd = -1
def start_route_replay(route: str) -> ManagedReplay:
master_fd, slave_fd = pty.openpty()
try:
fcntl.ioctl(slave_fd, termios.TIOCSWINSZ, struct.pack("HHHH", 40, 140, 0, 0))
os.set_blocking(master_fd, False)
env = os.environ.copy()
env["TERM"] = "xterm-256color"
process = subprocess.Popen(
[REPLAY_EXECUTABLE, "--ecam", route],
cwd=BASEDIR,
env=env,
stdin=slave_fd,
stdout=slave_fd,
stderr=slave_fd,
close_fds=True,
)
except Exception:
os.close(master_fd)
raise
finally:
os.close(slave_fd)
return ManagedReplay(process, master_fd)
def stop_route_replay(replay: ManagedReplay | None) -> None:
if replay is None:
return
if replay.poll() is None:
replay.process.terminate()
try:
replay.process.wait(timeout=2.0)
except subprocess.TimeoutExpired:
replay.process.kill()
replay.process.wait(timeout=1.0)
replay.close_terminal()
def replace_route_replay(process: ManagedReplay | None, route: str) -> tuple[ManagedReplay | None, str]:
route = route.strip()
if not route:
return process, "Enter a route or segment ID."
stop_route_replay(process)
try:
return start_route_replay(route), ""
except OSError as exc:
return None, f"Could not start replay: {exc}"
def route_start_seconds(route: str) -> float:
match = re.search(r"(?:--|/)(\d+)(?::[^/]*)?(?:/[qra])?$", route)
return int(match.group(1)) * 60.0 if match is not None else 0.0
def route_wall_time(route: str, route_seconds: float) -> str:
match = re.search(r"\d{4}-\d{2}-\d{2}--\d{2}-\d{2}-\d{2}", route)
if match is None:
return "--"
try:
route_datetime = datetime.strptime(match.group(), "%Y-%m-%d--%H-%M-%S") + timedelta(seconds=route_seconds)
except ValueError:
return "--"
return route_datetime.strftime("%a %b %d %H:%M:%S %Y")
def format_download_progress(current: int, total: int) -> str:
if total <= 0:
return "--"
percent = int(np.clip(current / total, 0.0, 1.0) * 100)
return f"{current / (1024 * 1024):.1f}/{total / (1024 * 1024):.1f} MB ({percent}%)"
@dataclass(frozen=True)
@@ -284,6 +424,40 @@ def draw_text(font, text: str, x: float, y: float, size: float, color: rl.Color
rl.draw_text_ex(font, text, rl.Vector2(round(x), round(y)), size, 0, color)
def measure_standalone_text(font, text: str, size: float) -> rl.Vector2:
return rl.measure_text_ex(font, text, size, 0) # noqa: TID251 - standalone raylib tool
def edit_key_pressed(key) -> bool:
return rl.is_key_pressed(key) or rl.is_key_pressed_repeat(key)
def replay_keyboard_command() -> str | None:
shift_down = (
rl.is_key_down(rl.KeyboardKey.KEY_LEFT_SHIFT)
or rl.is_key_down(rl.KeyboardKey.KEY_RIGHT_SHIFT)
)
for key, command in (
(rl.KeyboardKey.KEY_S, "S" if shift_down else "s"),
(rl.KeyboardKey.KEY_M, "M" if shift_down else "m"),
(rl.KeyboardKey.KEY_SPACE, " "),
(rl.KeyboardKey.KEY_E, "e"),
(rl.KeyboardKey.KEY_D, "d"),
(rl.KeyboardKey.KEY_T, "t"),
(rl.KeyboardKey.KEY_I, "i"),
(rl.KeyboardKey.KEY_W, "w"),
(rl.KeyboardKey.KEY_C, "c"),
(rl.KeyboardKey.KEY_EQUAL, "+"),
(rl.KeyboardKey.KEY_KP_ADD, "+"),
(rl.KeyboardKey.KEY_MINUS, "-"),
(rl.KeyboardKey.KEY_KP_SUBTRACT, "-"),
(rl.KeyboardKey.KEY_Q, "q"),
):
if rl.is_key_pressed(key):
return command
return None
def camera_content_rect(camera_view: CameraView, rect: rl.Rectangle) -> rl.Rectangle:
if camera_view.frame is None:
return rect
@@ -365,7 +539,7 @@ def top_down_track_geometry(rect: rl.Rectangle, track) -> tuple[float, float, fl
car_y = plot.y + plot.height - 18
return (
center_x - track.yRel * plot.width / (MAX_LATERAL_DISTANCE * 2),
car_y - track.dRel * plot.height / MAX_FORWARD_DISTANCE,
car_y - track.dRel * (plot.height - 18) / MAX_FORWARD_DISTANCE,
9.0,
)
@@ -416,21 +590,22 @@ def draw_camera_tracks(font, calibration: Calibration | None, tracks, camera_rec
if geometry is None:
continue
x, y, radius = geometry
dot_radius = max(4.5, radius * 0.68)
color = display_track_color(track, v_ego, motion_states, use_dbc_colors)
is_hovered = int(track.trackId) == hovered_id
is_selected = int(track.trackId) == selected_id
is_previewed = int(track.trackId) == preview_id and not is_selected
rl.draw_circle_v(rl.Vector2(x, y), radius + 4.0, rl.Color(0, 0, 0, 180))
rl.draw_circle_v(rl.Vector2(x, y), dot_radius + 3.0, rl.Color(0, 0, 0, 180))
if is_selected:
rl.draw_circle_lines_v(rl.Vector2(x, y), radius + 6.0, CYAN)
rl.draw_circle_lines_v(rl.Vector2(x, y), dot_radius + 4.0, CYAN)
elif is_previewed:
rl.draw_circle_lines_v(rl.Vector2(x, y), radius + 6.0, ORANGE)
rl.draw_circle_v(rl.Vector2(x, y), radius, color)
rl.draw_circle_v(rl.Vector2(x, y), max(2.0, radius * 0.32), BACKGROUND)
rl.draw_circle_lines_v(rl.Vector2(x, y), dot_radius + 4.0, ORANGE)
rl.draw_circle_v(rl.Vector2(x, y), dot_radius, color)
rl.draw_circle_v(rl.Vector2(x, y), max(1.5, dot_radius * 0.32), BACKGROUND)
if show_labels or is_hovered:
draw_track_popup(font, track, x, y, radius, camera_rect, color)
draw_track_popup(font, track, x, y, dot_radius, camera_rect, color)
def draw_fused_camera_mode(font, road_camera_view: CameraView, wide_camera_view: CameraView, device_camera,
@@ -506,10 +681,18 @@ def draw_top_down(font, rect: rl.Rectangle, tracks, v_ego: float, show_labels: b
plot = rl.Rectangle(rect.x + 48, rect.y + 44, rect.width - 72, rect.height - 72)
center_x = plot.x + plot.width / 2
car_y = plot.y + plot.height - 18
longitudinal_scale = plot.height / MAX_FORWARD_DISTANCE
longitudinal_scale = (plot.height - 18) / MAX_FORWARD_DISTANCE
lateral_scale = plot.width / (MAX_LATERAL_DISTANCE * 2)
draw_text(font, f"0-{MAX_FORWARD_DISTANCE:.0f} m", rect.x + rect.width - 118, rect.y + 16, 18, MUTED)
scale_x = plot.x - 5
scale_top = car_y - MAX_FORWARD_DISTANCE * longitudinal_scale
rl.draw_line_ex(rl.Vector2(scale_x, scale_top), rl.Vector2(scale_x, car_y), 1.0, rl.Color(MUTED.r, MUTED.g, MUTED.b, 110))
for distance in range(0, int(MAX_FORWARD_DISTANCE) + 1, 20):
y = car_y - distance * longitudinal_scale
rl.draw_line_ex(rl.Vector2(scale_x - 7, y), rl.Vector2(scale_x, y), 1.0, MUTED)
label = f"{distance}m"
label_size = measure_text_cached(font, label, 14)
draw_text(font, label, scale_x - label_size.x - 10, y - 7, 14, MUTED)
if use_dbc_colors:
legend = ((PURPLE, "moving"), (WHITE, "stationary"), (MUTED, "unknown / n/a"))
@@ -537,7 +720,7 @@ def draw_top_down(font, rect: rl.Rectangle, tracks, v_ego: float, show_labels: b
draw_model_line(road_edge.x, road_edge.y, center_x, car_y, longitudinal_scale, lateral_scale,
rl.Color(255, 76, 89, alpha), 1.0)
draw_model_line(model.position.x, model.position.y, center_x, car_y, longitudinal_scale, lateral_scale,
rl.Color(72, 220, 255, 150), 2.0)
rl.Color(72, 220, 255, 45), 0.75)
rl.draw_rectangle_rounded(rl.Rectangle(center_x - 13, car_y - 25, 26, 48), 0.3, 6, CYAN)
@@ -685,17 +868,226 @@ def draw_status_tooltip(font, text: str, chip_rect: rl.Rectangle, bounds: rl.Rec
draw_text(font, text, x + 9, y + 5, 15, TEXT)
def draw_source_status(font, rect: rl.Rectangle, live_tracks, valid: bool, alive: bool, v_ego: float,
show_labels: bool, data_source: str, use_dbc_colors: bool, camera_mode: str,
wide_available: bool, hide_unknown: bool, hide_stationary: bool, visible_track_count: int,
def draw_route_dialog(font, bounds: rl.Rectangle, route: str, cursor_index: int, error: str,
select_all: bool) -> str | None:
rl.draw_rectangle_rec(bounds, rl.Color(0, 0, 0, 190))
width = min(760.0, bounds.width - 40.0)
dialog = rl.Rectangle(bounds.x + (bounds.width - width) / 2, bounds.y + (bounds.height - 190.0) / 2, width, 190.0)
rl.draw_rectangle_rounded(dialog, 0.08, 8, PANEL)
rl.draw_rectangle_rounded_lines_ex(dialog, 0.08, 8, 1.0, MUTED)
draw_text(font, "Open route", dialog.x + 20, dialog.y + 17, 22, TEXT)
draw_text(font, "Paste a route or segment ID, then press Enter.", dialog.x + 20, dialog.y + 48, 15, MUTED)
input_rect = rl.Rectangle(dialog.x + 20, dialog.y + 76, dialog.width - 40, 38)
rl.draw_rectangle_rounded(input_rect, 0.15, 4, BACKGROUND)
rl.draw_rectangle_rounded_lines_ex(input_rect, 0.15, 4, 1.0, CYAN)
cursor_index = int(np.clip(cursor_index, 0, len(route)))
view_start = 0
view_end = len(route)
while view_start < cursor_index and measure_standalone_text(font, route[view_start:cursor_index], 18).x > input_rect.width - 24:
view_start += 1
while view_end > cursor_index and measure_standalone_text(font, route[view_start:view_end], 18).x > input_rect.width - 24:
view_end -= 1
display_route = route[view_start:view_end]
input_text = display_route
if route and select_all:
selection_width = min(measure_standalone_text(font, display_route, 18).x + 4, input_rect.width - 20)
rl.draw_rectangle_rec(
rl.Rectangle(input_rect.x + 8, input_rect.y + 7, selection_width, 24),
rl.Color(CYAN.r, CYAN.g, CYAN.b, 70),
)
draw_text(font, input_text, input_rect.x + 10, input_rect.y + 9, 18, TEXT)
if not select_all and int(time.monotonic() * 2) % 2 == 0:
text_before_cursor = route[view_start:cursor_index]
cursor_x = input_rect.x + 10 + measure_standalone_text(font, text_before_cursor, 18).x
rl.draw_line_ex(rl.Vector2(cursor_x + 1, input_rect.y + 8), rl.Vector2(cursor_x + 1, input_rect.y + 30), 1.0, TEXT)
if error:
draw_text(font, error, dialog.x + 20, dialog.y + 124, 14, RED)
mouse_position = rl.get_mouse_position()
cancel_size = measure_standalone_text(font, "Cancel", 16)
open_size = measure_standalone_text(font, "Open", 16)
open_rect = rl.Rectangle(dialog.x + dialog.width - open_size.x - 44, dialog.y + 137, open_size.x + 24, 34)
cancel_rect = rl.Rectangle(open_rect.x - cancel_size.x - 34, dialog.y + 137, cancel_size.x + 24, 34)
cancel_hovered = rl.check_collision_point_rec(mouse_position, cancel_rect)
open_hovered = rl.check_collision_point_rec(mouse_position, open_rect)
rl.draw_rectangle_rounded(cancel_rect, 0.2, 4, rl.Color(MUTED.r, MUTED.g, MUTED.b, 55 if cancel_hovered else 28))
rl.draw_rectangle_rounded(open_rect, 0.2, 4, rl.Color(CYAN.r, CYAN.g, CYAN.b, 75 if open_hovered else 42))
draw_text(font, "Cancel", cancel_rect.x + (cancel_rect.width - cancel_size.x) / 2, cancel_rect.y + 8, 16, TEXT)
draw_text(font, "Open", open_rect.x + (open_rect.width - open_size.x) / 2, open_rect.y + 8, 16, CYAN)
rl.set_mouse_cursor(
rl.MouseCursor.MOUSE_CURSOR_POINTING_HAND if cancel_hovered or open_hovered else rl.MouseCursor.MOUSE_CURSOR_IBEAM,
)
if rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT): # noqa: TID251 - standalone raylib tool
if cancel_hovered:
return "cancel"
if open_hovered:
return "open"
return None
def draw_route_loading(font, bounds: rl.Rectangle, route: str, elapsed: float) -> None:
rl.draw_rectangle_rec(bounds, rl.Color(0, 0, 0, 175))
width = min(580.0, bounds.width - 40.0)
card = rl.Rectangle(bounds.x + (bounds.width - width) / 2, bounds.y + (bounds.height - 154.0) / 2, width, 154.0)
rl.draw_rectangle_rounded(card, 0.1, 8, PANEL)
rl.draw_rectangle_rounded_lines_ex(card, 0.1, 8, 1.0, MUTED)
draw_text(font, "Loading route", card.x + 22, card.y + 19, 22, TEXT)
draw_text(font, "Waiting for replay data...", card.x + 22, card.y + 51, 15, MUTED)
route_text = route
while route_text and measure_standalone_text(font, route_text, 15).x > card.width - 44:
route_text = route_text[1:]
draw_text(font, route_text, card.x + 22, card.y + 77, 15, CYAN)
bar = rl.Rectangle(card.x + 22, card.y + 112, card.width - 44, 12)
rl.draw_rectangle_rounded(bar, 0.5, 6, rl.Color(MUTED.r, MUTED.g, MUTED.b, 35))
segment_width = min(140.0, bar.width * 0.3)
segment_left = (elapsed * 190.0) % (bar.width + segment_width) - segment_width
visible_left = max(0.0, segment_left)
visible_right = min(bar.width, segment_left + segment_width)
if visible_right > visible_left:
progress = rl.Rectangle(bar.x + visible_left, bar.y, visible_right - visible_left, bar.height)
rl.draw_rectangle_rounded(progress, 0.5, 6, CYAN)
def draw_replay_control_bar(font, bounds: rl.Rectangle, replay: ManagedReplay, route: str, route_seconds: float,
v_ego: float, loading: bool, seek_input: str, seek_active: bool) -> str | None:
rl.draw_rectangle_rec(bounds, rl.Color(8, 12, 18, 248))
rl.draw_line_ex(rl.Vector2(bounds.x, bounds.y), rl.Vector2(bounds.x + bounds.width, bounds.y), 1.0, MUTED)
controls = (
("-60s", "M", CYAN, "Shift+M: back 60 seconds"),
("-10s", "S", CYAN, "Shift+S: back 10 seconds"),
("PAUSE", " ", GREEN, "Space: pause / resume"),
("+10s", "s", CYAN, "S: forward 10 seconds"),
("+60s", "m", CYAN, "M: forward 60 seconds"),
("SPD-", "-", MUTED, "-: slower playback"),
("SPD+", "+", MUTED, "+: faster playback"),
("ENG", "e", GREEN, "E: next engagement"),
("DIS", "d", GREEN, "D: next disengagement"),
("TAG", "t", ORANGE, "T: next user tag"),
("INFO", "i", ORANGE, "I: next info alert"),
("WARN", "w", ORANGE, "W: next warning alert"),
("CRIT", "c", RED, "C: next critical alert"),
("EXIT", "q", RED, "Q: exit replay"),
)
mouse_position = rl.get_mouse_position()
chip_x = bounds.x + 10
chip_y = bounds.y + 9
clicked_action = None
hovered_tooltip = None
for label, action, color, tooltip in controls:
chip_x, chip_rect, hovered = draw_status_chip(font, label, chip_x, chip_y, color, mouse_position)
if hovered:
hovered_tooltip = (tooltip, chip_rect)
if rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT): # noqa: TID251
clicked_action = action
draw_text(font, "SEEK", chip_x + 2, bounds.y + 14, 14, MUTED)
field_x = chip_x + measure_standalone_text(font, "SEEK", 14).x + 10
field = rl.Rectangle(field_x, bounds.y + 8, 66, 27)
field_hovered = rl.check_collision_point_rec(mouse_position, field)
rl.draw_rectangle_rounded(field, 0.18, 4, BACKGROUND)
rl.draw_rectangle_rounded_lines_ex(field, 0.18, 4, 1.0, CYAN if seek_active else MUTED)
draw_text(font, seek_input, field.x + 7, field.y + 5, 15, TEXT)
if seek_active and int(time.monotonic() * 2) % 2 == 0:
cursor_x = field.x + 7 + measure_standalone_text(font, seek_input, 15).x
rl.draw_line_ex(rl.Vector2(cursor_x + 1, field.y + 4), rl.Vector2(cursor_x + 1, field.y + 22), 1.0, TEXT)
if field_hovered and rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT): # noqa: TID251
clicked_action = "seek-focus"
_, go_rect, go_hovered = draw_status_chip(font, "GO", field.x + field.width + 6, chip_y, CYAN, mouse_position)
if go_hovered:
hovered_tooltip = ("Enter: seek to absolute route seconds", go_rect)
if rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT): # noqa: TID251
clicked_action = "seek"
if hovered_tooltip is not None:
tooltip, chip_rect = hovered_tooltip
size = measure_text_cached(font, tooltip, 15)
tooltip_x = float(np.clip(chip_rect.x, bounds.x + 5, bounds.x + bounds.width - size.x - 23))
tooltip_rect = rl.Rectangle(tooltip_x, bounds.y - 33, size.x + 18, 27)
rl.draw_rectangle_rounded(tooltip_rect, 0.25, 4, rl.Color(0, 0, 0, 235))
rl.draw_rectangle_rounded_lines_ex(tooltip_rect, 0.25, 4, 1.0, MUTED)
draw_text(font, tooltip, tooltip_rect.x + 9, tooltip_rect.y + 5, 15, TEXT)
status_text = "loading" if loading else ("paused" if replay.paused else "playing")
info = "".join((
f"STATUS {status_text} ROUTE {route_seconds:.1f}s TIME {route_wall_time(route, route_seconds)} ",
f"SPEED {v_ego:.1f} m/s PLAYBACK {replay.playback_speed:.1f}x ",
f"DOWNLOAD {format_download_progress(replay.progress_current, replay.progress_total)}",
))
draw_text(font, info, bounds.x + 12, bounds.y + 43, 14, TEXT)
rl.set_mouse_cursor(
rl.MouseCursor.MOUSE_CURSOR_POINTING_HAND
if hovered_tooltip is not None or field_hovered or go_hovered
else rl.MouseCursor.MOUSE_CURSOR_DEFAULT,
)
return clicked_action
def apply_replay_control(replay: ManagedReplay, action: str | None, seek_input: str,
seek_active: bool) -> tuple[str, bool]:
if action == "seek-focus":
return seek_input, True
if action == "seek":
if seek_input:
replay.send(f"\n{seek_input}\n")
return "", False
if action is not None:
replay.send(action)
return seek_input, seek_active
def both_camera_streams_available(available_streams) -> bool:
return (
VisionStreamType.VISION_STREAM_ROAD in available_streams
and VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams
)
def fused_stream_loss_state(available_streams, missing_since: float | None,
now: float) -> tuple[float | None, bool]:
if both_camera_streams_available(available_streams):
return None, False
missing_since = now if missing_since is None else missing_since
return missing_since, now - missing_since >= FUSED_STREAM_GRACE_SECONDS
def draw_fused_video_button(font, bounds: rl.Rectangle, data_source: str, use_dbc_colors: bool, show_labels: bool,
hide_unknown: bool, hide_stationary: bool) -> bool:
source_title = "CAN" if data_source == "can" else "LIVE"
preceding_labels = (
f"R {source_title}",
f"C {'DBC' if use_dbc_colors else 'IMPL'}",
f"L {'ON' if show_labels else 'OFF'}",
"U UNKNOWN",
"S STATIONARY",
)
chip_x = bounds.x + 14
for label in preceding_labels:
chip_x += measure_text_cached(font, label, 15).x + 23
mouse_position = rl.get_mouse_position()
_, chip_rect, hovered = draw_status_chip(font, "V FUSE", chip_x, bounds.y + 49, GREEN, mouse_position)
rl.set_mouse_cursor(rl.MouseCursor.MOUSE_CURSOR_POINTING_HAND if hovered else rl.MouseCursor.MOUSE_CURSOR_DEFAULT)
if hovered:
draw_status_tooltip(font, "Switch to road camera", chip_rect, bounds)
return bool(
hovered
and rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT) # noqa: TID251 - standalone raylib tool
)
def draw_source_status(font, rect: rl.Rectangle, live_tracks, valid: bool, alive: bool, source_active: bool,
v_ego: float, show_labels: bool, data_source: str, use_dbc_colors: bool, camera_mode: str,
both_cameras_available: bool, hide_unknown: bool, hide_stationary: bool, visible_track_count: int,
table_mode: str, fps: int) -> str | None:
rl.draw_rectangle_rec(rect, rl.Color(11, 16, 24, 225))
tracks = live_tracks.points if valid else ()
sources = sorted(live_tracks.trackSources, key=lambda source: (source.startAddress, source.endAddress, source.bus)) if valid else []
status_color = GREEN if valid and alive else ORANGE
status = "LIVE" if valid and alive else "WAIT"
source_title = "CAN" if data_source == "can" else "LIVE"
draw_text(font, f"{source_title} {status}", rect.x + 14, rect.y + 12, 19, status_color)
if sources:
source = sources[0]
@@ -703,13 +1095,15 @@ def draw_source_status(font, rect: rl.Rectangle, live_tracks, valid: bool, alive
source_text = f"{compact_name} / B{source.bus} / {source_details(source.startAddress, source.endAddress)}"
if len(sources) > 1:
source_text += f" / +{len(sources) - 1}"
elif not source_active:
source_text = "no route loaded"
elif data_source == "liveTracks":
source_text = "no source metadata"
elif valid and live_tracks.radarTracksAvailable:
source_text = "radar detected / parsing"
else:
source_text = "searching for supported radar"
draw_text(font, source_text, rect.x + 112, rect.y + 14, 16, CYAN if sources else MUTED)
draw_text(font, source_text, rect.x + 14, rect.y + 14, 16, CYAN if sources else MUTED)
filtered = hide_unknown or hide_stationary
track_count = f"{visible_track_count}/{len(tracks)}" if filtered else str(len(tracks))
@@ -722,21 +1116,20 @@ def draw_source_status(font, rect: rl.Rectangle, live_tracks, valid: bool, alive
mouse_position = rl.get_mouse_position()
camera_label = "WIDE" if camera_mode == "wide 180" else "ROAD"
table_label = {"comparison": "COMP", "kinematics": "KIN", "object": "OBJ"}[table_mode]
chip_specs = (
(f"S {source_title}", status_color, "source", "Switch CAN / liveTracks source"),
chip_specs = [
(f"R {source_title}", status_color, "source", "Switch CAN / liveTracks source"),
(f"C {'DBC' if use_dbc_colors else 'IMPL'}", PURPLE if use_dbc_colors else CYAN, "colors",
"Switch DBC / implementation colors"),
(f"L {'ON' if show_labels else 'OFF'}", GREEN if show_labels else MUTED, "labels", "Show / hide all labels"),
(f"U {'HIDE' if hide_unknown else 'SHOW'}", CYAN if hide_unknown else MUTED, "unknown",
"Hide / show unknown tracks"),
(f"H {'HIDE' if hide_stationary else 'SHOW'}", CYAN if hide_stationary else MUTED, "stationary",
"Hide / show stationary tracks"),
(f"V {camera_label}", CYAN if wide_available else MUTED, "camera",
"Switch road / wide camera" if wide_available else "Wide camera unavailable"),
(f"T {table_label}", MUTED, "table", "Cycle comparison / kinematics / object table"),
("M FUSE", GREEN if wide_available else MUTED, "composite",
"Open full-screen fused road + wide cameras" if wide_available else "Wide camera unavailable"),
)
("U UNKNOWN", MUTED if hide_unknown else CYAN, "unknown",
"Show unknown tracks" if hide_unknown else "Hide unknown tracks"),
("S STATIONARY", MUTED if hide_stationary else CYAN, "stationary",
"Show stationary tracks" if hide_stationary else "Hide stationary tracks"),
]
if both_cameras_available:
chip_specs.append((f"V {camera_label}", CYAN, "camera", "Cycle road / wide / fused camera"))
chip_specs.append((f"T {table_label}", MUTED, "table", "Cycle comparison / kinematics / object table"))
chip_specs.append(("O OPEN", GREEN, "route", "Open a route, or paste one with Cmd/Ctrl+V"))
clicked_action = None
hovered_tooltip = None
for label, color, action, tooltip in chip_specs:
@@ -752,10 +1145,10 @@ def draw_source_status(font, rect: rl.Rectangle, live_tracks, valid: bool, alive
return clicked_action
def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) -> None:
def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False, start_route: str | None = None) -> None:
rl.set_trace_log_level(rl.TraceLogLevel.LOG_ERROR)
rl.set_config_flags(rl.ConfigFlags.FLAG_VSYNC_HINT | rl.ConfigFlags.FLAG_WINDOW_RESIZABLE)
rl.init_window(WINDOW_WIDTH, WINDOW_HEIGHT, "Hyundai auto radar tracks")
rl.init_window(WINDOW_WIDTH, WINDOW_HEIGHT, "Radar Tracks Debugger")
primary_position = rl.get_monitor_position(0)
primary_width = rl.get_monitor_width(0)
primary_height = rl.get_monitor_height(0)
@@ -764,6 +1157,7 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) ->
int(primary_position.y + (primary_height - WINDOW_HEIGHT) / 2),
)
rl.maximize_window()
rl.set_window_focused()
rl.set_target_fps(max(60, rl.get_monitor_refresh_rate(0)))
font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf")
@@ -814,39 +1208,201 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) ->
table_scroll = 0
selected_track_id = None
composite_mode = start_fused
fused_streams_missing_since = None
model_pixels = np.zeros((CAMERA_BUFFER_HEIGHT, CAMERA_BUFFER_WIDTH, 3), dtype=np.uint8)
dummy_top_down = np.zeros((384, 960), dtype=np.uint8)
overlay_dirty = True
route_dialog_open = start_route is None
route_input = start_route or ""
route_cursor = len(route_input)
route_error = ""
route_select_all = False
replay_process = None
route_loading = False
route_loading_started = 0.0
replay_seek_input = ""
replay_seek_active = False
route_message_start_mono: int | None = None
current_route_seconds = route_start_seconds(route_input)
if start_route:
replay_process, route_error = replace_route_replay(replay_process, start_route)
route_dialog_open = bool(route_error)
route_loading = replay_process is not None and not route_error
route_loading_started = time.monotonic()
composite_mode = composite_mode and not route_dialog_open
startup_focus_deadline = time.monotonic() + 2.0
next_startup_focus_request = 0.0
while not rl.window_should_close():
if rl.is_key_pressed(rl.KeyboardKey.KEY_L):
while not rl.window_should_close() and (replay_process is None or replay_process.poll() is None):
now = time.monotonic()
if replay_process is not None:
replay_process.drain_output()
if now < startup_focus_deadline and now >= next_startup_focus_request:
rl.set_window_focused()
next_startup_focus_request = now + 0.1
open_entered_route = False
modifier_down = (
rl.is_key_down(rl.KeyboardKey.KEY_LEFT_SUPER)
or rl.is_key_down(rl.KeyboardKey.KEY_RIGHT_SUPER)
or rl.is_key_down(rl.KeyboardKey.KEY_LEFT_CONTROL)
or rl.is_key_down(rl.KeyboardKey.KEY_RIGHT_CONTROL)
)
if route_dialog_open:
paste_pressed = modifier_down and rl.is_key_pressed(rl.KeyboardKey.KEY_V)
select_all_pressed = modifier_down and rl.is_key_pressed(rl.KeyboardKey.KEY_A)
codepoint = rl.get_char_pressed()
while codepoint > 0:
if not (paste_pressed or select_all_pressed) and 32 <= codepoint <= 126 and len(route_input) < 180:
if route_select_all:
route_input = ""
route_cursor = 0
route_select_all = False
route_input = route_input[:route_cursor] + chr(codepoint) + route_input[route_cursor:]
route_cursor += 1
route_error = ""
codepoint = rl.get_char_pressed()
if select_all_pressed:
route_select_all = bool(route_input)
route_cursor = len(route_input)
elif paste_pressed:
clipboard_text = rl.get_clipboard_text() or ""
printable_text = "".join(character for character in clipboard_text if 32 <= ord(character) <= 126)
if route_select_all:
route_input = ""
route_cursor = 0
inserted_text = printable_text[:180 - len(route_input)]
route_input = route_input[:route_cursor] + inserted_text + route_input[route_cursor:]
route_cursor += len(inserted_text)
route_select_all = False
route_error = ""
elif edit_key_pressed(rl.KeyboardKey.KEY_BACKSPACE):
if route_select_all:
route_input = ""
route_cursor = 0
elif route_cursor > 0:
route_input = route_input[:route_cursor - 1] + route_input[route_cursor:]
route_cursor -= 1
route_select_all = False
route_error = ""
elif edit_key_pressed(rl.KeyboardKey.KEY_DELETE):
if route_select_all:
route_input = ""
route_cursor = 0
elif route_cursor < len(route_input):
route_input = route_input[:route_cursor] + route_input[route_cursor + 1:]
route_select_all = False
route_error = ""
elif edit_key_pressed(rl.KeyboardKey.KEY_LEFT):
route_cursor = 0 if route_select_all else max(0, route_cursor - 1)
route_select_all = False
elif edit_key_pressed(rl.KeyboardKey.KEY_RIGHT):
route_cursor = len(route_input) if route_select_all else min(len(route_input), route_cursor + 1)
route_select_all = False
elif edit_key_pressed(rl.KeyboardKey.KEY_HOME):
route_cursor = 0
route_select_all = False
elif edit_key_pressed(rl.KeyboardKey.KEY_END):
route_cursor = len(route_input)
route_select_all = False
if rl.is_key_pressed(rl.KeyboardKey.KEY_ENTER):
open_entered_route = True
if rl.is_key_pressed(rl.KeyboardKey.KEY_ESCAPE):
route_dialog_open = False
route_error = ""
route_select_all = False
elif replay_seek_active:
codepoint = rl.get_char_pressed()
while codepoint > 0:
if chr(codepoint).isdigit() and len(replay_seek_input) < 7:
replay_seek_input += chr(codepoint)
codepoint = rl.get_char_pressed()
if edit_key_pressed(rl.KeyboardKey.KEY_BACKSPACE):
replay_seek_input = replay_seek_input[:-1]
elif rl.is_key_pressed(rl.KeyboardKey.KEY_ENTER) and replay_process is not None:
replay_seek_input, replay_seek_active = apply_replay_control(
replay_process, "seek", replay_seek_input, replay_seek_active,
)
elif rl.is_key_pressed(rl.KeyboardKey.KEY_ESCAPE):
replay_seek_active = False
elif modifier_down and rl.is_key_pressed(rl.KeyboardKey.KEY_V):
clipboard_text = rl.get_clipboard_text() or ""
route_input = "".join(character for character in clipboard_text if 32 <= ord(character) <= 126)[:180]
route_cursor = len(route_input)
route_select_all = False
replay_process, route_error = replace_route_replay(replay_process, route_input)
route_dialog_open = bool(route_error)
route_loading = replay_process is not None and not route_error
route_loading_started = time.monotonic()
route_message_start_mono = None
current_route_seconds = route_start_seconds(route_input)
composite_mode = composite_mode and not route_dialog_open
table_scroll = 0
selected_track_id = None
elif modifier_down:
pass
elif replay_process is not None and rl.is_key_pressed(rl.KeyboardKey.KEY_ENTER):
replay_seek_active = True
elif replay_process is not None and (replay_command := replay_keyboard_command()) is not None:
replay_process.send(replay_command)
elif rl.is_key_pressed(rl.KeyboardKey.KEY_O):
route_dialog_open = True
route_error = ""
route_select_all = bool(route_input)
route_cursor = len(route_input)
composite_mode = False
elif rl.is_key_pressed(rl.KeyboardKey.KEY_L):
show_labels = not show_labels
if rl.is_key_pressed(rl.KeyboardKey.KEY_S):
elif rl.is_key_pressed(rl.KeyboardKey.KEY_R):
use_can_source = not use_can_source
table_scroll = 0
selected_track_id = None
if rl.is_key_pressed(rl.KeyboardKey.KEY_C):
elif rl.is_key_pressed(rl.KeyboardKey.KEY_C):
use_dbc_colors = not use_dbc_colors
if rl.is_key_pressed(rl.KeyboardKey.KEY_U):
elif rl.is_key_pressed(rl.KeyboardKey.KEY_U):
hide_unknown = not hide_unknown
table_scroll = 0
if rl.is_key_pressed(rl.KeyboardKey.KEY_H):
elif rl.is_key_pressed(rl.KeyboardKey.KEY_S):
hide_stationary = not hide_stationary
table_scroll = 0
if rl.is_key_pressed(rl.KeyboardKey.KEY_T):
elif rl.is_key_pressed(rl.KeyboardKey.KEY_T):
table_mode_index = (table_mode_index + 1) % len(TABLE_MODES)
if rl.is_key_pressed(rl.KeyboardKey.KEY_M):
elif rl.is_key_pressed(rl.KeyboardKey.KEY_V):
if composite_mode:
composite_mode = False
elif VisionStreamType.VISION_STREAM_WIDE_ROAD in camera_view.available_streams:
composite_mode = True
if rl.is_key_pressed(rl.KeyboardKey.KEY_V):
if camera_view is wide_camera_view:
fused_streams_missing_since = None
camera_view = road_camera_view
elif VisionStreamType.VISION_STREAM_WIDE_ROAD in camera_view.available_streams:
camera_view = wide_camera_view
elif both_camera_streams_available(camera_view.available_streams):
if camera_view is road_camera_view:
camera_view = wide_camera_view
else:
composite_mode = True
fused_streams_missing_since = None
if open_entered_route:
replay_process, route_error = replace_route_replay(replay_process, route_input)
route_dialog_open = bool(route_error)
route_select_all = False
route_loading = replay_process is not None and not route_error
route_loading_started = time.monotonic()
route_message_start_mono = None
current_route_seconds = route_start_seconds(route_input)
sm.update(0)
route_message_times = [
sm.logMonoTime[service]
for service in ("carState", "modelV2", "roadCameraState", "liveTracks")
if sm.updated[service] and sm.logMonoTime[service] > 0
]
if replay_process is not None and route_message_times:
current_route_mono = max(route_message_times)
if route_message_start_mono is None:
route_message_start_mono = current_route_mono
current_route_seconds = (
route_start_seconds(route_input) + (current_route_mono - route_message_start_mono) / 1e9
)
if (route_loading
and time.monotonic() - route_loading_started > 0.15
and any(sm.updated[service] for service in ("carState", "modelV2", "roadCameraState", "liveTracks"))):
route_loading = False
while True:
try:
can_tracks, decoded_motion_states, can_data_alive, can_data_seen = decoder_output.get_nowait()
@@ -858,11 +1414,14 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) ->
window_width = rl.get_screen_width()
window_height = rl.get_screen_height()
replay_bar_height = 70.0 if replay_process is not None else 0.0
content_height = window_height - replay_bar_height
left_width = float(round(window_width * 0.58))
camera_height = float(round(window_height * 0.64))
camera_height = float(round(content_height * 0.64))
camera_rect = rl.Rectangle(0, 0, left_width, camera_height)
table_rect = rl.Rectangle(0, camera_height, left_width, window_height - camera_height)
radar_rect = rl.Rectangle(left_width, 0, window_width - left_width, window_height)
table_rect = rl.Rectangle(0, camera_height, left_width, content_height - camera_height)
radar_rect = rl.Rectangle(left_width, 0, window_width - left_width, content_height)
replay_bar_rect = rl.Rectangle(0, content_height, window_width, replay_bar_height)
status_rect = rl.Rectangle(0, 0, left_width, min(88, camera_height * 0.2))
if sm.valid["roadCameraState"] and sm.valid["liveCalibration"] and camera_view.frame:
@@ -928,6 +1487,7 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) ->
rl.clear_background(BACKGROUND)
if composite_mode:
rl.set_mouse_cursor(rl.MouseCursor.MOUSE_CURSOR_DEFAULT)
fused_bounds = rl.Rectangle(0, 0, window_width, content_height)
fused_device_camera = None
rpy_calib = np.zeros(3)
wide_from_device_euler = np.zeros(3)
@@ -937,12 +1497,39 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) ->
wide_from_device_euler = np.asarray(sm["liveCalibration"].wideFromDeviceEuler)
selected_track_id = draw_fused_camera_mode(
font, road_camera_view, wide_camera_view, fused_device_camera, rpy_calib, wide_from_device_euler,
rl.Rectangle(0, 0, window_width, window_height), tracks, v_ego, show_labels, motion_states,
use_dbc_colors, selected_track_id,
fused_bounds, tracks, v_ego, show_labels, motion_states, use_dbc_colors, selected_track_id,
)
fused_available_streams = set(road_camera_view.available_streams) | set(wide_camera_view.available_streams)
fused_streams_missing_since, streams_timed_out = fused_stream_loss_state(
fused_available_streams, fused_streams_missing_since, time.monotonic(),
)
if streams_timed_out:
composite_mode = False
fused_streams_missing_since = None
if VisionStreamType.VISION_STREAM_ROAD in fused_available_streams:
camera_view = road_camera_view
elif VisionStreamType.VISION_STREAM_WIDE_ROAD in fused_available_streams:
camera_view = wide_camera_view
if draw_fused_video_button(
font, fused_bounds, data_source, use_dbc_colors, show_labels, hide_unknown, hide_stationary,
):
composite_mode = False
fused_streams_missing_since = None
camera_view = road_camera_view
if route_loading:
draw_route_loading(font, fused_bounds, route_input, time.monotonic() - route_loading_started)
if replay_process is not None:
replay_action = draw_replay_control_bar(
font, replay_bar_rect, replay_process, route_input, current_route_seconds, v_ego, route_loading,
replay_seek_input, replay_seek_active,
)
replay_seek_input, replay_seek_active = apply_replay_control(
replay_process, replay_action, replay_seek_input, replay_seek_active,
)
rl.end_drawing()
continue
fused_streams_missing_since = None
camera_view.render(camera_rect)
camera_draw_rect = camera_content_rect(camera_view, camera_rect)
@@ -1004,9 +1591,10 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) ->
draw_track_table(font, table_rect, tracks, v_ego, motion_states, table_scroll, selected_track_id, table_hover_id,
track_signals, table_mode)
clicked_action = draw_source_status(
font, status_rect, selected_tracks, data_valid, data_alive, v_ego, show_labels, data_source, use_dbc_colors,
font, status_rect, selected_tracks, data_valid, data_alive,
replay_process is not None or can_data_seen or live_data_seen, v_ego, show_labels, data_source, use_dbc_colors,
"wide 180" if camera_view.stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD else "road",
VisionStreamType.VISION_STREAM_WIDE_ROAD in camera_view.available_streams, hide_unknown, hide_stationary,
both_camera_streams_available(camera_view.available_streams), hide_unknown, hide_stationary,
len(tracks), table_mode, rl.get_fps(),
)
if clicked_action == "source":
@@ -1024,19 +1612,51 @@ def ui_thread(addr: str, start_wide: bool = False, start_fused: bool = False) ->
hide_stationary = not hide_stationary
table_scroll = 0
elif clicked_action == "camera":
if camera_view is wide_camera_view:
camera_view = road_camera_view
elif VisionStreamType.VISION_STREAM_WIDE_ROAD in camera_view.available_streams:
if camera_view is road_camera_view:
camera_view = wide_camera_view
elif both_camera_streams_available(camera_view.available_streams):
composite_mode = True
fused_streams_missing_since = None
elif clicked_action == "table":
table_mode_index = (table_mode_index + 1) % len(TABLE_MODES)
elif clicked_action == "composite":
if VisionStreamType.VISION_STREAM_WIDE_ROAD in camera_view.available_streams:
composite_mode = True
if table_hover_id is not None:
elif clicked_action == "route":
route_dialog_open = True
route_error = ""
route_select_all = bool(route_input)
route_cursor = len(route_input)
if route_dialog_open:
route_action = draw_route_dialog(
font, rl.Rectangle(0, 0, window_width, window_height), route_input, route_cursor, route_error, route_select_all,
)
if route_action == "cancel":
route_dialog_open = False
route_error = ""
route_select_all = False
elif route_action == "open":
replay_process, route_error = replace_route_replay(replay_process, route_input)
route_dialog_open = bool(route_error)
route_select_all = False
route_loading = replay_process is not None and not route_error
route_loading_started = time.monotonic()
route_message_start_mono = None
current_route_seconds = route_start_seconds(route_input)
if route_loading and not route_dialog_open:
draw_route_loading(
font, rl.Rectangle(0, 0, window_width, window_height), route_input, time.monotonic() - route_loading_started,
)
if replay_process is not None and not route_dialog_open:
replay_action = draw_replay_control_bar(
font, replay_bar_rect, replay_process, route_input, current_route_seconds, v_ego, route_loading,
replay_seek_input, replay_seek_active,
)
replay_seek_input, replay_seek_active = apply_replay_control(
replay_process, replay_action, replay_seek_input, replay_seek_active,
)
if table_hover_id is not None and not route_dialog_open:
rl.set_mouse_cursor(rl.MouseCursor.MOUSE_CURSOR_POINTING_HAND)
rl.end_drawing()
stop_route_replay(replay_process)
decoder_process.terminate()
decoder_process.join(timeout=1.0)
decoder_output.close()
@@ -1053,6 +1673,7 @@ def get_arg_parser() -> argparse.ArgumentParser:
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
)
parser.add_argument("ip_address", nargs="?", default="127.0.0.1", help="Address publishing replay or on-car messages.")
parser.add_argument("--route", help="Route or segment ID to open in replay when the debugger starts.")
parser.add_argument("--wide", action="store_true", help="Start with the wide/full field-of-view road camera.")
parser.add_argument("--fused", action="store_true", help="Start with the full-screen fused road and wide cameras.")
return parser
@@ -1064,6 +1685,6 @@ if __name__ == "__main__":
os.environ["ZMQ"] = "1"
messaging.reset_context()
try:
ui_thread(args.ip_address, args.wide, args.fused)
ui_thread(args.ip_address, args.wide, args.fused, args.route)
except KeyboardInterrupt:
pass