diff --git a/common/params_keys.h b/common/params_keys.h index 5cb052ef0f..e946622920 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -178,6 +178,7 @@ inline static std::unordered_map keys = { {"OnroadUploads", {PERSISTENT | BACKUP, BOOL, "1"}}, {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}}, {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"RadarTracks", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}}, @@ -219,6 +220,7 @@ inline static std::unordered_map keys = { // sunnypilot car specific params {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}}, + {"HyundaiRadar", {PERSISTENT | BACKUP, INT, "0"}}, {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}}, {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}}, {"TeslaCoopSteering", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/selfdrive/ui/mici/layouts/settings/toggles.py b/selfdrive/ui/mici/layouts/settings/toggles.py index 8635336f97..0aca256134 100644 --- a/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/selfdrive/ui/mici/layouts/settings/toggles.py @@ -21,8 +21,11 @@ class TogglesLayoutMici(NavScroller): record_front = BigParamControl("record & upload driver camera", "RecordFront", toggle_callback=restart_needed_callback) record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback) enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback) + hyundai_radar = BigMultiParamToggle("hyundai radar", "HyundaiRadar", ["off", "lead only", "full radar"]) + radar_tracks = BigParamControl("radar tracks", "RadarTracks") self._scroller.add_widgets([ + radar_tracks, self._personality_toggle, self._experimental_btn, is_metric_toggle, @@ -35,6 +38,8 @@ class TogglesLayoutMici(NavScroller): # Toggle lists self._refresh_toggles = ( + ("HyundaiRadar", hyundai_radar), + ("RadarTracks", radar_tracks), ("ExperimentalMode", self._experimental_btn), ("IsMetric", is_metric_toggle), ("IsLdwEnabled", ldw_toggle), diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index e88d320a3f..b15af9b32a 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -153,6 +153,9 @@ class ModelRenderer(Widget, ModelRendererSP): self._draw_lane_lines() self._draw_path(sm) + if ui_state.radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: + self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z, track_size=3) + # if render_lead_indicator and radar_state: # self._draw_lead_indicator() diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 353cc5aa40..b750f0832c 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -135,6 +135,9 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): self._draw_lane_lines() self._draw_path(sm) + if ui_state.radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: + self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z) + if render_lead_indicator and radar_state: self._draw_lead_indicator() self.chevron_metrics.draw_lead_status(sm, radar_state, self._rect, self._lead_vehicles) diff --git a/selfdrive/ui/sunnypilot/onroad/model_renderer.py b/selfdrive/ui/sunnypilot/onroad/model_renderer.py index 5d78997662..cfea9ba7ac 100644 --- a/selfdrive/ui/sunnypilot/onroad/model_renderer.py +++ b/selfdrive/ui/sunnypilot/onroad/model_renderer.py @@ -6,9 +6,12 @@ See the LICENSE.md file in the root directory for more details. """ from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath +from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks class ModelRendererSP: def __init__(self): self.rainbow_path = RainbowPath() self.chevron_metrics = ChevronMetrics() + self.radar_tracks = RadarTracks() + diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py new file mode 100644 index 0000000000..1f4da80231 --- /dev/null +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -0,0 +1,23 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import math +import pyray as rl + + +class RadarTracks: + def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=6): + for track in live_tracks.points: + d_rel, y_rel, v_rel, a_rel = track.dRel, track.yRel, track.vRel, track.aRel + if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel) and math.isfinite(a_rel)): + continue + + pt = map_to_screen(d_rel, -y_rel, path_offset_z) + if pt is None: + continue + + x, y = pt + rl.draw_circle(int(x), int(y), track_size, rl.Color(0, 255, 64, 255)) diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index 1c21c35c6f..77bc97df9e 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/selfdrive/ui/sunnypilot/ui_state.py @@ -169,6 +169,7 @@ class UIStateSP: self.turn_signals = self.params.get_bool("ShowTurnSignals") self.boot_offroad_mode = self.params.get("DeviceBootMode", return_default=True) self.always_offroad = self.params.get_bool("OffroadMode") + self.radar_tracks = self.params.get_bool("RadarTracks") if not self._sp_initialized: self._sp_initialized = True diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 806dc11a6d..7ba537ce6f 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -63,6 +63,7 @@ class UIState(UIStateSP): "liveParameters", "testJoystick", "rawAudioData", + "liveTracks", ] + self.sm_services_ext ) diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 5be227c262..a2e41ff196 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -112,6 +112,7 @@ def initialize_params(params) -> list[dict[str, Any]]: # hyundai keys.extend([ "HyundaiLongitudinalTuning", + "HyundaiRadar", ]) # subaru diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 1745235d96..0620dfc045 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -1296,6 +1296,12 @@ "title": "Display Turn Signals", "description": "When enabled, visual turn indicators are drawn on the HUD." }, + { + "key": "RadarTracks", + "widget": "toggle", + "title": "Radar Tracks", + "description": "Show radar tracks" + }, { "key": "RoadNameToggle", "widget": "toggle", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml index 3170991157..d63833168e 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml @@ -24,6 +24,10 @@ sections: widget: toggle title: Display Turn Signals description: When enabled, visual turn indicators are drawn on the HUD. + - key: RadarTracks + widget: toggle + title: Radar Tracks + description: Show radar tracks - key: RoadNameToggle widget: toggle title: Display Road Name diff --git a/tools/replay/auto_radar_detect.py b/tools/replay/auto_radar_detect.py new file mode 100755 index 0000000000..34966054fd --- /dev/null +++ b/tools/replay/auto_radar_detect.py @@ -0,0 +1,343 @@ +#!/usr/bin/env python3 +import argparse +import concurrent.futures +import os +import signal +import subprocess +import sys +import time +from collections import Counter, defaultdict + +import cereal.messaging as messaging +from msgq.ipc_pyx import IpcError +from opendbc.car.tests.routes import CarTestRoute, routes +from openpilot.tools.replay.custom_routes import CUSTOM_ROUTES +from openpilot.tools.replay.radar_helpers import ( + build_seen_address_map, + get_radar_spec, + is_exclusive_full_range_match, +) + + +REPLAY_PATH = os.path.join(os.path.dirname(__file__), "replay") +REPLAY_PLAYBACK_SPEED = "3.0" +REPLAY_ALLOW_SERVICES = "can" +DETECTION_TIMEOUT_SECONDS = 20.0 +DETECTION_MIN_HITS = 10 +SOCKET_WAIT_TIMEOUT_SECONDS = 10.0 +CAR_MODEL_WIDTH = 38 +RADAR_TYPE_WIDTH = 10 + +ANSI_RESET = "\033[0m" +ANSI_BOLD = "\033[1m" +ANSI_DIM = "\033[2m" +ANSI_RED = "\033[31m" +ANSI_GREEN = "\033[32m" +ANSI_YELLOW = "\033[33m" +ANSI_BLUE = "\033[34m" +ANSI_MAGENTA = "\033[35m" +ANSI_CYAN = "\033[36m" + +RADAR_FAMILY_COLORS = { + "RADAR_500_51F": ANSI_CYAN, + "RADAR_210_21F": ANSI_YELLOW, + "RADAR_3A5_3C4": ANSI_GREEN, + "RADAR_602_611": ANSI_BLUE, +} + + +def build_arg_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Replay Hyundai/Kia/Genesis test routes and auto-detect radar type.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument("--routes", action="store_true", + help="Use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.") + parser.add_argument("--custom-routes", action="store_true", + help="Use routes from tools/replay/custom_routes.py.") + parser.add_argument("--route", action="append", default=[], + help="Specific route(s) to test. If omitted, use Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.") + parser.add_argument("--data-dir", default=None, + help="Optional local directory of route data to pass through to replay.") + parser.add_argument("--timeout", type=float, default=DETECTION_TIMEOUT_SECONDS, + help="Seconds to wait for a radar-family detection before giving up on a route.") + parser.add_argument("--min-hits", type=int, default=DETECTION_MIN_HITS, + help="Minimum CAN hits in a radar-family address range before considering it detected.") + parser.add_argument("--playback", default=REPLAY_PLAYBACK_SPEED, + help="Replay playback speed.") + parser.add_argument("--limit", type=int, default=None, + help="Only process the first N matching routes.") + parser.add_argument("--jobs", type=int, default=1, + help="Number of routes to process in parallel.") + parser.add_argument("--prefix", default="auto-radar-detect", + help="Base OPENPILOT_PREFIX to isolate replay sockets.") + return parser + + +def get_hkg_routes() -> list[CarTestRoute]: + hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_") + return [ + route for route in routes + if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes) + ] + + +def get_selected_routes(args: argparse.Namespace) -> list[CarTestRoute]: + selected_sources = int(bool(args.route)) + int(bool(args.routes)) + int(bool(args.custom_routes)) + if selected_sources > 1: + raise ValueError("Use only one of --route, --routes, or --custom-routes.") + + if args.route: + route_map = {route.route: route for route in get_hkg_routes()} + selected = [] + for route_name in args.route: + selected.append(route_map.get(route_name, CarTestRoute(route_name, None))) + return selected[:args.limit] if args.limit is not None else selected + + if args.custom_routes: + selected = [CarTestRoute(route.route, route.car_model) for route in CUSTOM_ROUTES] + return selected[:args.limit] if args.limit is not None else selected + + selected = get_hkg_routes() + return selected[:args.limit] if args.limit is not None else selected + + +def terminate_process(proc: subprocess.Popen) -> None: + if proc.poll() is not None: + return + + proc.terminate() + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + proc.kill() + proc.wait(timeout=5) + + +def start_replay(route: str, prefix: str, args: argparse.Namespace) -> subprocess.Popen: + cmd = [ + REPLAY_PATH, + "--no-vipc", + "--no-loop", + "--playback", args.playback, + "--allow", REPLAY_ALLOW_SERVICES, + "--prefix", prefix, + ] + if args.data_dir: + cmd.extend(["--data_dir", args.data_dir]) + cmd.append(route) + + env = os.environ.copy() + env["OPENPILOT_PREFIX"] = prefix + return subprocess.Popen( + cmd, + cwd=os.path.dirname(REPLAY_PATH), + env=env, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + preexec_fn=os.setsid if os.name != "nt" else None, + ) + + +def stop_replay(proc: subprocess.Popen) -> None: + if proc.poll() is not None: + return + + if os.name != "nt": + try: + os.killpg(proc.pid, signal.SIGTERM) + except ProcessLookupError: + return + else: + proc.terminate() + + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + if os.name != "nt": + try: + os.killpg(proc.pid, signal.SIGKILL) + except ProcessLookupError: + return + else: + proc.kill() + proc.wait(timeout=5) + + +def wait_for_can_socket(prefix: str, timeout: float) -> messaging.SubSocket: + socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can") + started = time.monotonic() + while True: + if os.path.exists(socket_path): + break + if time.monotonic() - started > timeout: + raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}") + time.sleep(0.1) + + while True: + try: + return messaging.sub_sock("can", conflate=False, timeout=100) + except IpcError: + if time.monotonic() - started > timeout: + raise + time.sleep(0.1) + + +def detect_radar_family(route: CarTestRoute, route_idx: int, args: argparse.Namespace) -> dict: + prefix = f"{args.prefix}-{os.getpid()}-{route_idx}" + os.environ["OPENPILOT_PREFIX"] = prefix + messaging.reset_context() + proc = start_replay(route.route, prefix, args) + logcan = wait_for_can_socket(prefix, min(args.timeout, SOCKET_WAIT_TIMEOUT_SECONDS)) + counts = Counter() + buses = defaultdict(set) + seen_addresses = build_seen_address_map() + started = time.monotonic() + detected = None + + try: + while True: + if proc.poll() is not None and (time.monotonic() - started) > 1.0: + break + if time.monotonic() - started > args.timeout: + break + + msgs = messaging.drain_sock(logcan, wait_for_one=True) + for msg in msgs: + for can_msg in msg.can: + radar_spec = get_radar_spec(can_msg.address) + if radar_spec is not None: + counts[radar_spec.name] += 1 + buses[radar_spec.name].add(can_msg.src) + seen_addresses[radar_spec.name].add(can_msg.address) + if counts[radar_spec.name] >= args.min_hits and is_exclusive_full_range_match(radar_spec, seen_addresses): + detected = radar_spec.name + if detected is not None: + break + if detected is not None: + break + if detected is not None: + break + finally: + stop_replay(proc) + + dominant_family = detected + + return { + "route": route.route, + "car_model": str(route.car_model) if route.car_model is not None else "UNKNOWN", + "detected": dominant_family or "NONE", + "counts": dict(counts), + "buses": {name: sorted(bus_set) for name, bus_set in buses.items()}, + "seen_addresses": {name: sorted(addresses) for name, addresses in seen_addresses.items()}, + } + + +def print_results(results: list[dict]) -> None: + print(f"{'Car Model':<{CAR_MODEL_WIDTH}} {'Detected':<{RADAR_TYPE_WIDTH}} Route") + print("-" * (CAR_MODEL_WIDTH + RADAR_TYPE_WIDTH + 9 + 60)) + for result in results: + detected_plain = "" if result["detected"] == "NONE" else result["detected"] + model = format_car_model(result["car_model"])[:CAR_MODEL_WIDTH] + detected = f"{detected_plain:<{RADAR_TYPE_WIDTH}}" + if supports_color() and detected_plain: + detected = f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain)) + print(f"{model:<{CAR_MODEL_WIDTH}} {detected} {result['route']}") + + +def supports_color() -> bool: + return sys.stdout.isatty() and os.getenv("TERM") not in (None, "dumb") + + +def format_car_model(car_model: object) -> str: + return str(car_model) if car_model is not None else "UNKNOWN" + + +def colorize_detected(detected: str) -> str: + if detected == "NONE": + return "" + + if not supports_color(): + return detected + + color = RADAR_FAMILY_COLORS.get(detected, ANSI_MAGENTA) + return f"{color}{detected}{ANSI_RESET}" + + +def print_progress_line(index: int, total: int, route: CarTestRoute, detected: str) -> None: + left = f"[{index}/{total}]" + model = format_car_model(route.car_model)[:CAR_MODEL_WIDTH] + detected_plain = "" if detected == "NONE" else detected + route_text = route.route + if supports_color(): + left = f"{ANSI_DIM}{left}{ANSI_RESET}" + model = f"{ANSI_CYAN}{model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}" + detected_text = colorize_detected(detected_plain).ljust(len(colorize_detected(detected_plain))) + route_text = f"{ANSI_DIM}{route_text}{ANSI_RESET}" + else: + model = f"{model:<{CAR_MODEL_WIDTH}}" + detected_text = detected_plain + + detected_text = f"{detected_plain:<{RADAR_TYPE_WIDTH}}" if not supports_color() else ( + f"{colorize_detected(detected_plain)}" + " " * max(0, RADAR_TYPE_WIDTH - len(detected_plain)) + ) + + print(f"{left} {model} {detected_text} {route_text}") + + +def main() -> int: + args = build_arg_parser().parse_args(sys.argv[1:]) + try: + selected_routes = get_selected_routes(args) + except ValueError as e: + print(str(e), file=sys.stderr) + return 2 + if not selected_routes: + print("No matching routes found.", file=sys.stderr) + return 1 + + results = [] + total = len(selected_routes) + jobs = max(1, min(args.jobs, total)) + + header_model = "Car Model" + header_detected = "Radar" + if supports_color(): + header_model = f"{ANSI_BOLD}{header_model:<{CAR_MODEL_WIDTH}}{ANSI_RESET}" + header_detected = f"{ANSI_BOLD}{header_detected:<{RADAR_TYPE_WIDTH}}{ANSI_RESET}" + else: + header_model = f"{header_model:<{CAR_MODEL_WIDTH}}" + header_detected = f"{header_detected:<{RADAR_TYPE_WIDTH}}" + print(f" {header_model} {header_detected} Route") + + if jobs == 1: + for idx, route in enumerate(selected_routes): + result = detect_radar_family(route, idx, args) + print_progress_line(idx + 1, total, route, result['detected']) + results.append((idx, result)) + else: + with concurrent.futures.ProcessPoolExecutor(max_workers=jobs) as executor: + future_to_job = { + executor.submit(detect_radar_family, route, idx, args): (idx, route) + for idx, route in enumerate(selected_routes) + } + completed = 0 + for future in concurrent.futures.as_completed(future_to_job): + idx, route = future_to_job[future] + result = future.result() + completed += 1 + print_progress_line(completed, total, route, result['detected']) + results.append((idx, result)) + + results = sorted( + (result for _, result in results), + key=lambda result: (result["car_model"], result["route"]), + ) + + print() + print_results(results) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/replay/custom_routes.py b/tools/replay/custom_routes.py new file mode 100755 index 0000000000..01589879d2 --- /dev/null +++ b/tools/replay/custom_routes.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +from dataclasses import dataclass + + +@dataclass(frozen=True) +class CustomRoute: + car_model: str + route: str + + +CUSTOM_ROUTES = [ + CustomRoute("HYUNDAI_ELANTRA_HEV_2024", "07a48901db7b2503|0000012d--61aba9f832"), + CustomRoute("HYUNDAI_IONIQ_5", "c5a5f79df9b6a084/0000005d--24c0a7a7ee"), + CustomRoute("HYUNDAI_IONIQ_5", "90950642f47cf05b/00000681--6a904aaa98"), + CustomRoute("HYUNDAI_IONIQ_5", "102161de13e822d1/0000000d--f74a52a624"), + CustomRoute("HYUNDAI_PALISADE_2023", "17ef028fec0ccf81/00000010--dba8455015"), + CustomRoute("HYUNDAI_PALISADE_2023", "6b8052042ba3ee05/00000199--3ed3fb5799"), + CustomRoute("KIA_EV6", "455a0ab75ce5c1e0/000000d1--098f32028c"), + CustomRoute("KIA_EV6", "6d2092783bf67457/0000008c--5caf525813"), + CustomRoute("KIA_EV6", "4961cb0f7bdd77a2/00000105--b636ed2f65"), + CustomRoute("KIA_K8_HEV_1ST_GEN", "78ad5150de133637|2023-09-13--16-15-57"), + CustomRoute("GENESIS_GV60_EV_1ST_GEN", "b1e441e63f1c99dd|0000009d--14cde5aeaf"), + CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000187--ce36ae7c11"), + CustomRoute("HYUNDAI_IONIQ_6", "faa815c21279fef7/00000191--e5c3ffb55b"), + CustomRoute("HYUNDAI_KONA_2ND_GEN", "32025f26789d8fab/00000022--a499e8ffa3"), + CustomRoute("HYUNDAI_KONA_EV_2ND_GEN", "1618132d68afc876/00000021--bf0f957649"), + CustomRoute("HYUNDAI_KONA_HEV_2ND_GEN", "97ca61196eb73e0d/00000052--4555329470"), + CustomRoute("HYUNDAI_SANTA_FE_HEV_5TH_GEN", "d54302f1d5e7a7cc|00000656--5dae9f54a7"), + CustomRoute("HYUNDAI_SONATA_2024", "4267ea8a353cdb36/00000262--8a427003c7"), + CustomRoute("KIA_CARNIVAL_HEV_4TH_GEN_2026", "7b8cc7bb46000e53/0000000b--2abacaff78"), + CustomRoute("KIA_EV6_2025", "48c27f77f9fd1a9b|00000199--193ee1ba20"), + CustomRoute("KIA_EV9", "ccfd4a1af758ee73/00000091--7fa49719a5"), + CustomRoute("KIA_K4_2025", "baf39eeaba1217ca/00000002--b36e3fa031"), + CustomRoute("KIA_K5_2025", "c4a804b067623789/0000007c--163f831540"), + CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032d--8379197bd3"), + CustomRoute("KIA_NIRO_EV_2ND_GEN", "80b8e9a6ad0acec3/0000032c--0055eeee96"), + CustomRoute("KIA_NIRO_HEV_2ND_GEN", "0d4257d1c6741384/00000067--42230e4b8d"), + CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000003--d0591c14ec"), + CustomRoute("HYUNDAI_KONA_EV_2022", "e174e1a0b92263ed/00000001--18955a967e"), + CustomRoute("KIA_CEED_PHEV", "26064e18b24ae44c/00000000--4eb95fc137"), + CustomRoute("HYUNDAI_IONIQ", "ac27c9808ac91710/00000002--e0aab81e4d"), + CustomRoute("KIA_K7_2017", "74fbff45aa20fe9e/00000010--6f173d5799"), + CustomRoute("KIA_NIRO_EV", "b27d9eafeb61976e/00000233--03a9c0858c"), + CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000157--ecaf77a801"), + CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000158--f7e70e1435"), + CustomRoute("KIA_CARNIVAL_4TH_GEN", "a0cb448c2ffb9383/00000154--3471fc4ab3"), + CustomRoute("KIA_SORENTO_4TH_GEN", "bf42073ef09e0af2/00000017--3edc1f8259"), + CustomRoute("KIA_SORENTO_HEV_4TH_GEN", "833262b0c9e4016a/0000004a--0227a058e3"), + CustomRoute("HYUNDAI_PALISADE", "662bedbf8453b81e/00000124--7dc358f20c"), + CustomRoute("HYUNDAI_SONATA", "8f52823c702cb300/00000177--177d1aa6ff"), + CustomRoute("HYUNDAI_SONATA_HYBRID", "e3ae9e987a2a4e57/0000002b--30a6963e88"), + CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000031--4cb0d8aca5"), + CustomRoute("HYUNDAI_ELANTRA_HEV_2021", "7bc0de9da607c543/00000042--302b371266"), + CustomRoute("HYUNDAI_IONIQ_HEV_2022", "4a975fc1d9e71801/00000005--50d176008a"), + CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/00000014--25fc6b216f"), + CustomRoute("HYUNDAI_IONIQ_9", "71e4e67d29034771/0000000e--d3b19ad6f6/10"), + CustomRoute("HYUNDAI_SANTA_CRUZ_2025", "6e7904b03a4aafc2/00000010--31034184b6"), + CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "26da1db30eff4fcc/00000061--a60470e363"), + CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "a6d25e95d936fdc4/000002ed--89d7944c52"), + CustomRoute("HYUNDAI_TUCSON_4TH_GEN", "6722665fbbf2a644/00000534--4ab7b733c5"), + CustomRoute("HYUNDAI_TUCSON_HEV_2025", "5868ec006e2bb61e/00000021--7d10df95e8"), + CustomRoute("KIA_SPORTAGE_5TH_GEN", "ce05e32158479f42/000003f4--9543087719"), + CustomRoute("KIA_SPORTAGE_HEV_2026", "343d5e350abaedcf/00000020--1c864750b6"), +] diff --git a/tools/replay/radar_helpers.py b/tools/replay/radar_helpers.py new file mode 100755 index 0000000000..0087efdb19 --- /dev/null +++ b/tools/replay/radar_helpers.py @@ -0,0 +1,209 @@ +#!/usr/bin/env python3 +import math +import os +import tempfile +from dataclasses import dataclass + +from opendbc.can.parser import CANParser + + +RADAR_500_51F_DBC_TEMPLATE = """ +BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR + SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX + SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX + SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX + SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX + SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX + SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX + SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX + SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX + SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX +""" + +RADAR_3A5_3C4_DBC_TEMPLATE = """ +BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 24 RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_1 : 25|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_3 : 28|2@0+ (1,0) [0|3] "" XXX + SG_ COUNTER_3 : 31|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_2 : 38|7@0- (1,0) [0|127] "" XXX + SG_ COUNTER_256 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_6 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ STATE : 54|3@0+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_8 : 62|7@0- (1,0) [0|127] "" XXX + SG_ LONG_DIST : 63|12@1+ (0.05,0) [0|8191] "m" XXX + SG_ LAT_DIST : 76|12@1- (0.05,0) [0|127] "" XXX + SG_ REL_SPEED : 88|14@1- (0.01,0) [0|16383] "" XXX + SG_ NEW_SIGNAL_4 : 103|2@0+ (1,0) [0|3] "" XXX + SG_ LAT_DIST_ACCEL : 104|13@1- (1,0) [0|8191] "" XXX + SG_ REL_ACCEL : 118|10@1- (0.02,0) [0|1023] "" XXX + SG_ NEW_SIGNAL_5 : 133|4@0+ (1,0) [0|15] "" XXX +""" + +RADAR_210_21F_DBC_TEMPLATE = """ +BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 32 RADAR + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ 1_COUNTER_255 : 47|8@0+ (1,0) [0|255] "" XXX + SG_ 1_STATE_ALT : 51|4@0+ (1,0) [0|15] "" XXX + SG_ 1_STATE : 55|4@0+ (1,0) [0|15] "" XXX + SG_ 1_NEW_SIGNAL_3 : 63|8@0- (1,0) [0|255] "" XXX + SG_ 1_LONG_DIST : 64|12@1+ (0.05,0) [0|4095] "" XXX + SG_ 1_LAT_DIST : 76|12@1- (0.05,0) [0|4095] "" XXX + SG_ 1_REL_SPEED : 88|14@1- (0.01,0) [0|16383] "" XXX + SG_ 1_NEW_SIGNAL_1 : 102|2@1+ (1,0) [0|3] "" XXX + SG_ 1_LAT_ACCEL : 104|13@1- (1,0) [0|8191] "" XXX + SG_ 1_REL_ACCEL : 118|10@1- (1,0) [0|1023] "" XXX + SG_ 2_COUNTER_255 : 175|8@0+ (1,0) [0|255] "" XXX + SG_ 2_STATE_ALT : 179|4@0+ (1,0) [0|15] "" XXX + SG_ 2_STATE : 183|4@0+ (1,0) [0|15] "" XXX + SG_ 2_NEW_SIGNAL_3 : 191|8@0- (1,0) [0|255] "" XXX + SG_ 2_LONG_DIST : 192|12@1+ (0.05,0) [0|4095] "" XXX + SG_ 2_LAT_DIST : 204|12@1- (0.05,0) [0|4095] "" XXX + SG_ 2_REL_SPEED : 216|14@1- (0.01,0) [0|65535] "" XXX + SG_ 2_NEW_SIGNAL_1 : 230|2@1+ (1,0) [0|3] "" XXX + SG_ 2_LAT_ACCEL : 232|13@1- (1,0) [0|8191] "" XXX + SG_ 2_REL_ACCEL : 246|10@1- (1,0) [0|1023] "" XXX +""" + +RADAR_602_611_DBC_TEMPLATE = """ +BO_ {addr_dec} RADAR_TRACK_{addr_hex}: 8 RADAR + SG_ 1_DISTANCE : 0|10@1+ (0.25,0) [0|255.75] "" XXX + SG_ 1_LATERAL : 10|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX + SG_ 1_SPEED : 21|10@1+ (0.25,-128) [-128|127.75] "" XXX + SG_ 2_DISTANCE : 31|10@1+ (0.25,0) [0|255.75] "" XXX + SG_ 2_LATERAL : 41|11@1+ (0.03,-30.705) [-30.705|30.705] "" XXX + SG_ 2_SPEED : 52|10@1+ (0.25,-128) [-128|127.75] "" XXX + SG_ COUNTER : 62|2@1+ (1,0) [0|3] "" XXX +""" + + +@dataclass(frozen=True) +class RadarSpec: + name: str + start_addr: int + msg_count: int + dbc_template: str + track_prefixes: tuple[str, ...] + + @property + def end_addr(self) -> int: + return self.start_addr + self.msg_count - 1 + + def contains(self, address: int) -> bool: + return self.start_addr <= address <= self.end_addr + + +RADAR_SPECS = ( + RadarSpec("RADAR_500_51F", 0x500, 32, RADAR_500_51F_DBC_TEMPLATE, ("",)), + RadarSpec("RADAR_210_21F", 0x210, 16, RADAR_210_21F_DBC_TEMPLATE, ("1_", "2_")), + RadarSpec("RADAR_3A5_3C4", 0x3A5, 32, RADAR_3A5_3C4_DBC_TEMPLATE, ("",)), + RadarSpec("RADAR_602_611", 0x602, 16, RADAR_602_611_DBC_TEMPLATE, ("1_", "2_")), +) + + +def build_seen_address_map() -> dict[str, set[int]]: + return {radar_spec.name: set() for radar_spec in RADAR_SPECS} + + +def get_radar_spec(address: int) -> RadarSpec | None: + for radar_spec in RADAR_SPECS: + if radar_spec.contains(address): + return radar_spec + return None + + +def is_exclusive_full_range_match(radar_spec: RadarSpec, seen_addresses: dict[str, set[int]]) -> bool: + expected_addresses = set(range(radar_spec.start_addr, radar_spec.end_addr + 1)) + if seen_addresses[radar_spec.name] != expected_addresses: + return False + + for other_spec in RADAR_SPECS: + if other_spec.name == radar_spec.name: + continue + + other_expected_addresses = set(range(other_spec.start_addr, other_spec.end_addr + 1)) + if seen_addresses[other_spec.name] == other_expected_addresses: + return False + + return True + + +def get_radar_dbc_path(radar_spec: RadarSpec) -> str: + dbc_path = os.path.join(tempfile.gettempdir(), f"{radar_spec.name.lower()}_radar_ui.dbc") + dbc_content = "\n".join( + radar_spec.dbc_template.format(addr_dec=addr, addr_hex=f"{addr:x}") + for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1) + ) + if not os.path.exists(dbc_path) or open(dbc_path).read() != dbc_content: + with open(dbc_path, "w") as f: + f.write(dbc_content) + return dbc_path + + +def get_radar_can_parser(radar_spec: RadarSpec, bus: int) -> CANParser: + messages = [(f"RADAR_TRACK_{addr:x}", 50) for addr in range(radar_spec.start_addr, radar_spec.end_addr + 1)] + return CANParser(get_radar_dbc_path(radar_spec), messages, bus) + + +def get_track_storage_key(radar_spec: RadarSpec, bus: int, addr: int, track_prefix: str) -> tuple[str, int, int]: + if radar_spec.name in ("RADAR_500_51F", "RADAR_3A5_3C4"): + return (radar_spec.name, bus, addr) + + track_index = int(track_prefix[0]) - 1 + return (radar_spec.name, bus, addr * 2 + track_index) + + +def get_track_ts_nanos(parser: CANParser, msg_name: str, radar_spec: RadarSpec, track_prefix: str) -> int: + if radar_spec.name == "RADAR_602_611": + return parser.ts_nanos[msg_name][f"{track_prefix}DISTANCE"] + if radar_spec.name == "RADAR_210_21F": + return parser.ts_nanos[msg_name][f"{track_prefix}LONG_DIST"] + return parser.ts_nanos[msg_name]["LONG_DIST"] + + +def decode_radar_track(radar_spec: RadarSpec, track_msg, track_prefix: str) -> tuple[float, float, float, float]: + if radar_spec.name == "RADAR_602_611": + return ( + track_msg[f"{track_prefix}DISTANCE"], + track_msg[f"{track_prefix}LATERAL"], + track_msg[f"{track_prefix}SPEED"], + float("nan"), + ) + + if radar_spec.name == "RADAR_210_21F": + return ( + track_msg[f"{track_prefix}LONG_DIST"], + track_msg[f"{track_prefix}LAT_DIST"], + track_msg[f"{track_prefix}REL_SPEED"], + float("nan"), + ) + + if radar_spec.name == "RADAR_500_51F": + azimuth = math.radians(track_msg["AZIMUTH"]) + long_dist = track_msg["LONG_DIST"] + return ( + math.cos(azimuth) * long_dist, + 0.5 * -math.sin(azimuth) * long_dist, + track_msg["REL_SPEED"], + track_msg["REL_ACCEL"], + ) + + return ( + track_msg["LONG_DIST"], + track_msg["LAT_DIST"], + track_msg["REL_SPEED"], + track_msg["REL_ACCEL"], + ) + + +def is_radar_track_valid(radar_spec: RadarSpec, track_msg, track_prefix: str) -> bool: + if radar_spec.name == "RADAR_602_611": + return track_msg[f"{track_prefix}DISTANCE"] != 255.75 + + if radar_spec.name == "RADAR_210_21F": + return track_msg[f"{track_prefix}STATE"] in (3, 4) + + return track_msg["STATE"] in (3, 4) diff --git a/tools/replay/ui_radar.py b/tools/replay/ui_radar.py new file mode 100755 index 0000000000..daef943fd0 --- /dev/null +++ b/tools/replay/ui_radar.py @@ -0,0 +1,912 @@ +#!/usr/bin/env python3 +import argparse +import os +import signal +import subprocess +import sys +import time +from dataclasses import dataclass + +import cv2 +import numpy as np +import pyray as rl + +import cereal.messaging as messaging +from opendbc.car.tests.routes import routes +from openpilot.common.basedir import BASEDIR +from openpilot.common.transformations.camera import DEVICE_CAMERAS +from openpilot.tools.replay.custom_routes import CUSTOM_ROUTES +from openpilot.tools.replay.lib.ui_helpers import ( + UP, + BLACK, + GREEN, + Calibration, + get_blank_lid_overlay, + init_plots, + plot_lead, + plot_model, + to_topdown_pt, +) +from openpilot.tools.replay.radar_helpers import ( + RADAR_SPECS, + build_seen_address_map, + decode_radar_track, + get_radar_can_parser, + get_radar_spec, + get_track_storage_key, + get_track_ts_nanos, + is_exclusive_full_range_match, + is_radar_track_valid, +) +from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA +from msgq.visionipc import VisionIpcClient, VisionStreamType + +os.environ['BASEDIR'] = BASEDIR + +ANGLE_SCALE = 5.0 +RADAR_TRACK_TIMEOUT_FRAMES = 10 +RADAR_FORMAT_SWITCH_MISS_FRAMES = 30 +RADAR_TRACK_RADIUS = 4 +CAMERA_RADAR_Y_OFFSET = 25 +RADAR_HEATMAP_DECAY = 0.975 +RADAR_HEATMAP_ALPHA = 0.65 +CAMERA_RADAR_HEATMAP_ALPHA = 0.45 +REPLAY_PATH = os.path.join(os.path.dirname(__file__), "replay") +REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS = 10.0 +REPLAY_SPEEDS = (0.2, 0.5, 1.0, 2.0, 4.0, 8.0) +CAMERA_DRAW_WIDTH = 640 +CAMERA_DRAW_HEIGHT = 480 +TOP_DOWN_DRAW_WIDTH = 384 +PLOT_DRAW_WIDTH = 480 +PLOT_DRAW_HEIGHT = 480 +RADAR_HEATMAP_MODES = ("OFF", "TOP", "CAMERA", "BOTH") +CYAN = (90, 235, 255) +AMBER = (255, 210, 90) +SOFT_WHITE = (235, 235, 235) +SLATE = (140, 180, 210) + + +@dataclass +class RadarTrackPoint: + trackId: int + measured: bool = True + dRel: float = 0.0 + yRel: float = 0.0 + vRel: float = 0.0 + aRel: float = 0.0 + yvRel: float = float("nan") + + +def draw_radar_points(tracks, lid_overlay): + for track in tracks: + px, py = to_topdown_pt(track.dRel, -track.yRel) + if px != -1: + cv2.circle(lid_overlay, (py, px), RADAR_TRACK_RADIUS, 255, thickness=-1, lineType=cv2.LINE_AA) + + +def update_radar_heatmap(tracks, radar_heatmap): + radar_heatmap *= RADAR_HEATMAP_DECAY + for track in tracks: + px, py = to_topdown_pt(track.dRel, -track.yRel) + if px != -1: + cv2.circle(radar_heatmap, (py, px), RADAR_TRACK_RADIUS + 2, 255.0, thickness=-1, lineType=cv2.LINE_AA) + + +def update_radar_camera_heatmap(tracks, radar_heatmap, calibration, shape): + radar_heatmap *= RADAR_HEATMAP_DECAY + if calibration is None: + return + + height, width = shape[:2] + for track in tracks: + if track.dRel <= 0.0: + continue + + pt = calibration.car_space_to_bb( + np.asarray([track.dRel - RADAR_TO_CAMERA]), + np.asarray([-track.yRel]), + np.asarray([1.0]), + ) + x, y = np.round(pt[0]).astype(int) + y += CAMERA_RADAR_Y_OFFSET + if 0 <= x < width and 0 <= y < height: + cv2.circle(radar_heatmap, (x, y), RADAR_TRACK_RADIUS + 3, 255.0, thickness=-1, lineType=cv2.LINE_AA) + + +def overlay_heatmap(img, radar_heatmap, alpha_scale): + radar_heat_uint8 = np.ascontiguousarray(np.clip(radar_heatmap, 0, 255).astype(np.uint8)) + radar_heat_mask = radar_heat_uint8 > 0 + if not np.any(radar_heat_mask): + return + + heat_rgb = cv2.cvtColor(cv2.applyColorMap(radar_heat_uint8, cv2.COLORMAP_TURBO), cv2.COLOR_BGR2RGB).astype(np.float32) + img_rgb = img.astype(np.float32) + alpha = ((radar_heat_uint8.astype(np.float32) / 255.0) * alpha_scale)[..., None] + img[:] = np.where( + radar_heat_mask[..., None], + np.clip(img_rgb * (1.0 - alpha) + heat_rgb * alpha, 0, 255).astype(np.uint8), + img, + ) + + +def draw_radar_points_camera(tracks, img, calibration): + if calibration is None: + return + + for track in tracks: + if track.dRel <= 0.0: + continue + + # Match the road-space projection convention used by other UI overlays. + pt = calibration.car_space_to_bb( + np.asarray([track.dRel - RADAR_TO_CAMERA]), + np.asarray([-track.yRel]), + np.asarray([1.0]), + ) + x, y = np.round(pt[0]).astype(int) + y += CAMERA_RADAR_Y_OFFSET + if 0 <= x < img.shape[1] and 0 <= y < img.shape[0]: + cv2.circle(img, (x, y), RADAR_TRACK_RADIUS, (255, 255, 255), thickness=-1, lineType=cv2.LINE_AA) + + +def draw_loading_overlay(font, lines, camera_texture, top_down_texture, hor_mode, panel_x, panel_y): + rl.draw_texture_pro( + camera_texture, + rl.Rectangle(0, 0, camera_texture.width, camera_texture.height), + rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT), + rl.Vector2(0, 0), + 0.0, + rl.WHITE, + ) + rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251 + + if hor_mode: + panel_width = 620 + panel_height = 300 + else: + panel_width = 700 + panel_height = 320 + + rl.draw_rectangle(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(0, 0, 0, 140)) + rl.draw_rectangle_lines(panel_x - 20, panel_y - 30, panel_width, panel_height, rl.Color(255, 255, 255, 90)) + + for i, line in enumerate(lines): + if line: + rl.draw_text_ex(font, line, rl.Vector2(panel_x, panel_y + i * 40), 28 if i == 0 else 20, 0, rl.WHITE) + + +def start_replay(route: str, prefix: str, playback: str, data_dir: str | None, start_seconds: int) -> subprocess.Popen: + cmd = [ + REPLAY_PATH, + "--playback", playback, + "--prefix", prefix, + ] + if start_seconds > 0: + cmd.extend(["--start", str(start_seconds)]) + if data_dir: + cmd.extend(["--data_dir", data_dir]) + cmd.append(route) + + env = os.environ.copy() + env["OPENPILOT_PREFIX"] = prefix + return subprocess.Popen( + cmd, + cwd=os.path.dirname(REPLAY_PATH), + env=env, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + preexec_fn=os.setsid if os.name != "nt" else None, + ) + + +def stop_replay(proc: subprocess.Popen | None) -> None: + if proc is not None and proc.poll() is None: + if os.name != "nt": + try: + os.killpg(proc.pid, signal.SIGTERM) + except ProcessLookupError: + pass + else: + proc.terminate() + + try: + proc.wait(timeout=5) + except subprocess.TimeoutExpired: + if os.name != "nt": + try: + os.killpg(proc.pid, signal.SIGKILL) + except ProcessLookupError: + pass + else: + proc.kill() + proc.wait(timeout=5) + + # `replay` can leave helper processes like `replayd` behind, so do one + # targeted cleanup pass on shutdown as well. + for pattern in ("/tools/replay/replay", "replayd"): + try: + subprocess.run( + ["pkill", "-f", pattern], + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + check=False, + ) + except FileNotFoundError: + pass + + +def wait_for_can_socket(prefix: str, timeout: float) -> None: + socket_path = os.path.join("/tmp", f"msgq_{prefix}", "can") + started = time.monotonic() + while not os.path.exists(socket_path): + if time.monotonic() - started > timeout: + raise TimeoutError(f"Timed out waiting for CAN socket at {socket_path}") + time.sleep(0.1) + + +def get_hkg_routes() -> list[tuple[str, str]]: + hkg_prefixes = ("HYUNDAI_", "KIA_", "GENESIS_") + return [ + (route.route, getattr(route.car_model, "name", str(route.car_model))) + for route in routes + if route.car_model is not None and getattr(route.car_model, "name", str(route.car_model)).startswith(hkg_prefixes) + ] + + +def get_custom_routes() -> list[tuple[str, str]]: + return [(route.route, route.car_model) for route in CUSTOM_ROUTES] + + +def make_submaster(addr): + return messaging.SubMaster( + [ + 'carState', + 'longitudinalPlan', + 'carControl', + 'radarState', + 'liveCalibration', + 'controlsState', + 'selfdriveState', + 'liveTracks', + 'modelV2', + 'liveParameters', + 'roadCameraState', + ], + addr=addr, + ) + + +def reset_radar_state(): + return ( + 0, + None, + 0, + {radar_spec.name: 0 for radar_spec in RADAR_SPECS}, + build_seen_address_map(), + {}, + 0, + {}, + {}, + {}, + ) + + +def ui_thread(addr, route_entries=None, playback="1.0", data_dir=None, prefix="ui-replay", start_route_idx=0): + cv2.setNumThreads(1) + + # 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 + + if hor_mode: + size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH + PLOT_DRAW_WIDTH, 960) + write_x = 5 + write_y = 480 + else: + size = (CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 960 + 300) + write_x = CAMERA_DRAW_WIDTH + 5 + write_y = 970 + + 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) + + # Load font + font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf") + font = rl.load_font_ex(font_path, 32, None, 0) + + # 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) + + # 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) + + current_route_idx = start_route_idx + current_route_name = None + current_route_model = None + replay_proc = None + current_start_seconds = 0 + paused = False + state_checks_enabled = False + radar_heatmap_mode_idx = 0 + current_playback = min(REPLAY_SPEEDS, key=lambda speed: abs(speed - float(playback))) + last_replay_started_at = time.monotonic() + playback_ready = False + loading_status = "Initializing UI" + + def current_offset_seconds() -> int: + if paused or not playback_ready or replay_proc is None or replay_proc.poll() is not None: + return current_start_seconds + return max(0, int(current_start_seconds + (time.monotonic() - last_replay_started_at) * current_playback)) + + def connect_streams(): + nonlocal replay_proc, current_route_name, current_route_model, current_route_idx, current_start_seconds, loading_status, paused, \ + last_replay_started_at, current_playback, playback_ready + + if route_entries: + current_route_name, current_route_model = route_entries[current_route_idx] + loading_status = f"Starting replay for route {current_route_idx + 1}/{len(route_entries)} at {current_start_seconds}s ({current_playback:.1f}x)" + stop_replay(replay_proc) + os.environ["OPENPILOT_PREFIX"] = prefix + messaging.reset_context() + replay_proc = start_replay(current_route_name, prefix, f"{current_playback:.1f}", data_dir, current_start_seconds) + paused = False + playback_ready = False + loading_status = "Waiting for CAN socket" + wait_for_can_socket(prefix, REPLAY_SOCKET_WAIT_TIMEOUT_SECONDS) + + loading_status = "Connecting to messaging streams" + sm_local = make_submaster(addr) + logcan_local = messaging.sub_sock("can", addr=addr, conflate=False, timeout=100) + loading_status = "Waiting for road camera frames" + return sm_local, logcan_local + + sm, logcan = connect_streams() + + img = np.zeros((480, 640, 3), dtype='uint8') + imgff = None + num_px = 0 + calibration = None + (can_range_msg_count, + active_radar_format_name, + active_radar_format_miss_count, + radar_format_total_counts, + radar_format_seen_addresses, + radar_track_ids, + next_radar_track_id, + radar_tracks, + radar_track_last_seen, + radar_parsers) = reset_radar_state() + + lid_overlay_blank = get_blank_lid_overlay(UP) + radar_heatmap = np.zeros_like(lid_overlay_blank, dtype=np.float32) + camera_radar_heatmap = np.zeros(img.shape[:2], dtype=np.float32) + + # 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, + } + + 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.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 not rl.window_should_close(): + # ***** frame ***** + if not vipc_client.is_connected(): + vipc_client.connect(False) + + rl.begin_drawing() + rl.clear_background(rl.Color(64, 64, 64, 255)) + + if rl.is_key_released(rl.KeyboardKey.KEY_Q): + rl.end_drawing() + stop_replay(replay_proc) + replay_proc = None + break + + shift_down = rl.is_key_down(rl.KeyboardKey.KEY_LEFT_SHIFT) or rl.is_key_down(rl.KeyboardKey.KEY_RIGHT_SHIFT) + + if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_SPACE): + if paused: + sm, logcan = connect_streams() + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) + else: + current_start_seconds = current_offset_seconds() + paused = True + loading_status = "Paused" + stop_replay(replay_proc) + replay_proc = None + + if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_RIGHT): + current_route_idx = (current_route_idx + 1) % len(route_entries) + current_start_seconds = 0 + paused = False + (can_range_msg_count, + active_radar_format_name, + active_radar_format_miss_count, + radar_format_total_counts, + radar_format_seen_addresses, + radar_track_ids, + next_radar_track_id, + radar_tracks, + radar_track_last_seen, + radar_parsers) = reset_radar_state() + radar_heatmap.fill(0) + camera_radar_heatmap.fill(0) + sm, logcan = connect_streams() + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) + + if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_LEFT): + current_route_idx = (current_route_idx - 1) % len(route_entries) + current_start_seconds = 0 + paused = False + (can_range_msg_count, + active_radar_format_name, + active_radar_format_miss_count, + radar_format_total_counts, + radar_format_seen_addresses, + radar_track_ids, + next_radar_track_id, + radar_tracks, + radar_track_last_seen, + radar_parsers) = reset_radar_state() + radar_heatmap.fill(0) + camera_radar_heatmap.fill(0) + sm, logcan = connect_streams() + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) + + if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_M): + current_start_seconds = max(0, current_offset_seconds() + (-60 if shift_down else 60)) + paused = False + (can_range_msg_count, + active_radar_format_name, + active_radar_format_miss_count, + radar_format_total_counts, + radar_format_seen_addresses, + radar_track_ids, + next_radar_track_id, + radar_tracks, + radar_track_last_seen, + radar_parsers) = reset_radar_state() + radar_heatmap.fill(0) + camera_radar_heatmap.fill(0) + sm, logcan = connect_streams() + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) + + if route_entries and rl.is_key_released(rl.KeyboardKey.KEY_S): + current_start_seconds = max(0, current_offset_seconds() + (-10 if shift_down else 10)) + paused = False + (can_range_msg_count, + active_radar_format_name, + active_radar_format_miss_count, + radar_format_total_counts, + radar_format_seen_addresses, + radar_track_ids, + next_radar_track_id, + radar_tracks, + radar_track_last_seen, + radar_parsers) = reset_radar_state() + sm, logcan = connect_streams() + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) + + if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_EQUAL) or rl.is_key_released(rl.KeyboardKey.KEY_KP_ADD)): + for speed in REPLAY_SPEEDS: + if speed > current_playback: + current_playback = speed + break + if not paused: + current_start_seconds = current_offset_seconds() + (can_range_msg_count, + active_radar_format_name, + active_radar_format_miss_count, + radar_format_total_counts, + radar_format_seen_addresses, + radar_track_ids, + next_radar_track_id, + radar_tracks, + radar_track_last_seen, + radar_parsers) = reset_radar_state() + radar_heatmap.fill(0) + camera_radar_heatmap.fill(0) + sm, logcan = connect_streams() + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) + + if route_entries and (rl.is_key_released(rl.KeyboardKey.KEY_MINUS) or rl.is_key_released(rl.KeyboardKey.KEY_KP_SUBTRACT)): + for speed in reversed(REPLAY_SPEEDS): + if speed < current_playback: + current_playback = speed + break + if not paused: + current_start_seconds = current_offset_seconds() + (can_range_msg_count, + active_radar_format_name, + active_radar_format_miss_count, + radar_format_total_counts, + radar_format_seen_addresses, + radar_track_ids, + next_radar_track_id, + radar_tracks, + radar_track_last_seen, + radar_parsers) = reset_radar_state() + radar_heatmap.fill(0) + camera_radar_heatmap.fill(0) + sm, logcan = connect_streams() + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True) + + if rl.is_key_released(rl.KeyboardKey.KEY_C): + state_checks_enabled = not state_checks_enabled + + if rl.is_key_released(rl.KeyboardKey.KEY_H): + radar_heatmap_mode_idx = (radar_heatmap_mode_idx + 1) % len(RADAR_HEATMAP_MODES) + if RADAR_HEATMAP_MODES[radar_heatmap_mode_idx] == "OFF": + radar_heatmap.fill(0) + camera_radar_heatmap.fill(0) + + yuv_img_raw = vipc_client.recv() + if yuv_img_raw is None or not yuv_img_raw.data.any(): + if replay_proc is not None and replay_proc.poll() is not None: + loading_status = f"Replay exited with code {replay_proc.poll()}" + + loading_lines = [ + f"{loading_status}{' (paused)' if paused else ''}", + f"Route {current_route_idx + 1}/{len(route_entries)}" if route_entries else "Connected to external replay", + f"Platform: {current_route_model}" if current_route_model is not None else "", + f"Route: {current_route_name}" if current_route_name is not None else "", + f"Offset: {current_offset_seconds()}s" if route_entries else "", + f"Playback: {current_playback:.1f}x" if route_entries else "", + f"Radar state checks: {'ON' if state_checks_enabled else 'OFF'}", + f"Radar heatmap: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}", + "Keys: SPACE play/pause, RIGHT next, LEFT prev, M +/-60s, S +/-10s, +/- speed, C checks, H heatmap, Q quit" \ + if route_entries else "Keys: C checks, H heatmap, Q quit", + ] + draw_loading_overlay(font, loading_lines, camera_texture, top_down_texture, hor_mode, 80, 160) + rl.end_drawing() + continue + + if not playback_ready: + playback_ready = True + last_replay_started_at = time.monotonic() + loading_status = "Streaming" + + lid_overlay = lid_overlay_blank.copy() + top_down = top_down_texture, lid_overlay + + sm.update(0) + + camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))] + + # Use received buffer dimensions (full HEVC can have stride != buffer_len/rows due to VENUS padding) + h, w, stride = yuv_img_raw.height, yuv_img_raw.width, yuv_img_raw.stride + nv12_size = h * 3 // 2 * stride + imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8, count=nv12_size).reshape((h * 3 // 2, stride)) + num_px = w * h + rgb = cv2.cvtColor(imgff[: h * 3 // 2, : w], cv2.COLOR_YUV2RGB_NV12) + + qcam = "QCAM" in os.environ + bb_scale = 0.825 if qcam else 0.8 + 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 + + w = sm['controlsState'].lateralControlState.which() + if w == 'lqrStateDEPRECATED': + angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg + elif w == 'indiState': + angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg + else: + angle_steers_k = np.inf + + plot_arr[:-1] = plot_arr[1:] + plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg + plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg + 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['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['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 + + if len(sm['longitudinalPlan'].accels): + plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0] + + if sm.recv_frame['modelV2']: + plot_model(sm['modelV2'], img, calibration, top_down) + + if sm.recv_frame['radarState']: + plot_lead(sm['radarState'], top_down) + + if sm.updated['liveCalibration'] and num_px: + rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib) + calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale) + + can_packets = messaging.drain_sock(logcan) + if can_packets: + can_strings = [ + (can_packet.logMonoTime, [(msg.address, msg.dat, msg.src) for msg in can_packet.can]) + for can_packet in can_packets + ] + detected_format_counts = {radar_spec.name: 0 for radar_spec in RADAR_SPECS} + + for can_packet in can_packets: + for msg in can_packet.can: + radar_spec = get_radar_spec(msg.address) + if radar_spec is not None: + can_range_msg_count += 1 + detected_format_counts[radar_spec.name] += 1 + radar_format_total_counts[radar_spec.name] += 1 + radar_format_seen_addresses[radar_spec.name].add(msg.address) + if radar_spec.name not in radar_parsers: + radar_parsers[radar_spec.name] = {} + if msg.src not in radar_parsers[radar_spec.name]: + radar_parsers[radar_spec.name][msg.src] = get_radar_can_parser(radar_spec, msg.src) + + matching_formats = [ + radar_spec.name + for radar_spec in RADAR_SPECS + if is_exclusive_full_range_match(radar_spec, radar_format_seen_addresses) + ] + if len(matching_formats) == 1: + if active_radar_format_name == matching_formats[0]: + active_radar_format_miss_count = 0 + elif active_radar_format_name is None: + active_radar_format_name = matching_formats[0] + active_radar_format_miss_count = 0 + else: + active_radar_format_miss_count += 1 + if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES: + active_radar_format_name = matching_formats[0] + active_radar_format_miss_count = 0 + elif len(matching_formats) == 0 and active_radar_format_name is not None: + active_radar_format_miss_count += 1 + if active_radar_format_miss_count >= RADAR_FORMAT_SWITCH_MISS_FRAMES: + active_radar_format_name = None + active_radar_format_miss_count = 0 + + active_radar_spec = next((spec for spec in RADAR_SPECS if spec.name == active_radar_format_name), None) + if active_radar_spec is not None: + for bus, parser in radar_parsers.get(active_radar_spec.name, {}).items(): + updated_addrs = parser.update(can_strings) + relevant_updated_addrs = { + track_addr for track_addr in updated_addrs + if active_radar_spec.start_addr <= track_addr <= active_radar_spec.end_addr + } + if not relevant_updated_addrs: + continue + + for track_addr in relevant_updated_addrs: + msg_name = f"RADAR_TRACK_{track_addr:x}" + track_msg = parser.vl[msg_name] + for track_prefix in active_radar_spec.track_prefixes: + track_key = get_track_storage_key(active_radar_spec, bus, track_addr, track_prefix) + ts_nanos = get_track_ts_nanos(parser, msg_name, active_radar_spec, track_prefix) + if ts_nanos == 0: + continue + + if state_checks_enabled and not is_radar_track_valid(active_radar_spec, track_msg, track_prefix): + radar_tracks.pop(track_key, None) + radar_track_last_seen.pop(track_key, None) + continue + + d_rel, y_rel, v_rel, a_rel = decode_radar_track(active_radar_spec, track_msg, track_prefix) + + if track_key not in radar_track_ids: + radar_track_ids[track_key] = next_radar_track_id + next_radar_track_id += 1 + + radar_tracks[track_key] = RadarTrackPoint( + trackId=radar_track_ids[track_key], + dRel=d_rel, + yRel=y_rel, + vRel=v_rel, + aRel=a_rel, + ) + radar_track_last_seen[track_key] = sm.frame + + stale_tracks = [ + track_key for track_key, last_seen in radar_track_last_seen.items() + if (sm.frame - last_seen) > RADAR_TRACK_TIMEOUT_FRAMES + ] + for track_key in stale_tracks: + radar_track_last_seen.pop(track_key, None) + radar_tracks.pop(track_key, None) + + active_radar_tracks = [ + track for track_key, track in radar_tracks.items() + if active_radar_format_name is not None and track_key[0] == active_radar_format_name + ] + active_radar_buses = sorted({ + track_key[1] for track_key in radar_tracks + if active_radar_format_name is not None and track_key[0] == active_radar_format_name + }) + if len(active_radar_tracks) == 0: + active_radar_tracks = sm['liveTracks'].points + active_radar_buses = [] + + radar_heatmap_mode = RADAR_HEATMAP_MODES[radar_heatmap_mode_idx] + if radar_heatmap_mode in ("TOP", "BOTH"): + update_radar_heatmap(active_radar_tracks, radar_heatmap) + if radar_heatmap_mode in ("CAMERA", "BOTH"): + update_radar_camera_heatmap(active_radar_tracks, camera_radar_heatmap, calibration, img.shape) + overlay_heatmap(img, camera_radar_heatmap, CAMERA_RADAR_HEATMAP_ALPHA) + + # draw decoded radar tracks when present, otherwise fall back to liveTracks + draw_radar_points(active_radar_tracks, top_down[1]) + draw_radar_points_camera(active_radar_tracks, img, calibration) + + # *** blits *** + # 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_pro( + camera_texture, + rl.Rectangle(0, 0, camera_texture.width, camera_texture.height), + rl.Rectangle(0, 0, CAMERA_DRAW_WIDTH, CAMERA_DRAW_HEIGHT), + rl.Vector2(0, 0), + 0.0, + rl.WHITE, + ) + + # display alerts + 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: + rl.draw_texture_pro( + plot_texture, + rl.Rectangle(0, 0, plot_texture.width, plot_texture.height), + rl.Rectangle(CAMERA_DRAW_WIDTH + TOP_DOWN_DRAW_WIDTH, 0, PLOT_DRAW_WIDTH, PLOT_DRAW_HEIGHT), + rl.Vector2(0, 0), + 0.0, + rl.WHITE, + ) + else: + rl.draw_texture(plot_texture, 0, 300, rl.WHITE) # noqa: TID251 + + # 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] + if radar_heatmap_mode in ("TOP", "BOTH"): + overlay_heatmap(lid_rgba[..., :3], radar_heatmap.T, RADAR_HEATMAP_ALPHA) + rl.update_texture(top_down_texture, rl.ffi.cast("void *", np.ascontiguousarray(lid_rgba).ctypes.data)) + rl.draw_texture(top_down_texture, CAMERA_DRAW_WIDTH, 0, rl.WHITE) # noqa: TID251 + + SPACING = 20 + lines = [ + ("ENABLED", GREEN if sm['selfdriveState'].enabled else BLACK), + ("SPEED: " + str(round(sm['carState'].vEgo, 1)) + " m/s", CYAN), + ("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), CYAN), + ("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), CYAN), + (f"RADAR FORMAT: {active_radar_format_name or 'NONE'}", AMBER), + (f"RADAR CAN MSGS: {can_range_msg_count}", AMBER), + (f"RADAR TRACKS: {len(active_radar_tracks)}" + + (f" (BUS {','.join(str(bus) for bus in active_radar_buses)})" if active_radar_buses else ""), + AMBER), + (f"RADAR STATE CHECKS: {'ON' if state_checks_enabled else 'OFF'}", AMBER), + (f"RADAR HEATMAP: {RADAR_HEATMAP_MODES[radar_heatmap_mode_idx]}", AMBER), + (f"ROUTE: {current_route_name}" if current_route_name is not None else "", SLATE), + (f"PLATFORM: {current_route_model}" if current_route_model is not None else "", SLATE), + (f"OFFSET: {current_offset_seconds()}s" if route_entries else "", SOFT_WHITE), + (f"PLAYBACK: {current_playback:.1f}x" if route_entries else "", SOFT_WHITE), + (f"STATUS: {'PAUSED' if paused else 'PLAYING'}" if route_entries else "", SOFT_WHITE), + ("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", SOFT_WHITE), + ("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffsetDeg, 2)) + " deg", SOFT_WHITE), + ("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100.0, 2)) + " %", SOFT_WHITE), + ("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), SOFT_WHITE), + ] + + hud_height = len(lines) * SPACING + 18 + rl.draw_rectangle(write_x - 10, write_y - 12, 560, hud_height, rl.Color(8, 12, 18, 155)) + rl.draw_rectangle(write_x - 10, write_y - 12, 4, hud_height, rl.Color(90, 235, 255, 255)) + rl.draw_rectangle_lines(write_x - 10, write_y - 12, 560, hud_height, rl.Color(90, 235, 255, 80)) + + for i, line in enumerate(lines): + if line is not None: + 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() + stop_replay(replay_proc) + + +def get_arg_parser(): + 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("--route", default=None, help="Route to replay locally before opening the UI.") + parser.add_argument("--routes", action="store_true", help="Cycle Hyundai/Kia/Genesis routes from opendbc/car/tests/routes.py.") + parser.add_argument("--custom-routes", nargs="?", type=int, const=1, default=None, + help="Cycle routes from tools/replay/custom_routes.py, optionally starting from a 1-based route index.") + parser.add_argument("--data-dir", default=None, help="Optional local route data directory to pass to replay.") + parser.add_argument("--playback", default="1.0", help="Replay playback speed when using --route.") + parser.add_argument("--prefix", default="ui-replay", help="OPENPILOT_PREFIX to use when launching replay from the UI.") + 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:]) + + selected_sources = int(args.route is not None) + int(args.routes) + int(args.custom_routes is not None) + if selected_sources > 1: + raise SystemExit("Use only one of --route, --routes, or --custom-routes.") + + route_entries = None + start_route_idx = 0 + if args.route is not None: + route_entries = [(args.route, "MANUAL_ROUTE")] + elif args.routes: + route_entries = get_hkg_routes() + elif args.custom_routes is not None: + route_entries = get_custom_routes() + start_route_idx = max(0, min(len(route_entries) - 1, args.custom_routes - 1)) + + if route_entries: + os.environ["OPENPILOT_PREFIX"] = args.prefix + messaging.reset_context() + elif args.ip_address != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.reset_context() + + ui_thread(args.ip_address, route_entries=route_entries, playback=args.playback, data_dir=args.data_dir, prefix=args.prefix, start_route_idx=start_route_idx)