From 74b9619efe1394fd8a02d4ae7eee0b2d2e9ac669 Mon Sep 17 00:00:00 2001 From: royjr Date: Fri, 17 Jul 2026 08:51:22 -0400 Subject: [PATCH] Update ui.py --- tools/radar/ui.py | 739 ++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 680 insertions(+), 59 deletions(-) diff --git a/tools/radar/ui.py b/tools/radar/ui.py index 7cfdfb00ec..35365db2c5 100755 --- a/tools/radar/ui.py +++ b/tools/radar/ui.py @@ -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