IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Prebuilt Release @ 27f668a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-03 18:23:24 -05:00
commit b073c5182b
2554 changed files with 679696 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
iqpilot/tools/Brewfile Normal file
View File

@@ -0,0 +1,14 @@
brew "git-lfs"
brew "capnp"
brew "coreutils"
brew "eigen"
brew "ffmpeg"
brew "glfw"
brew "libusb"
brew "libtool"
brew "llvm"
brew "openssl@3.0"
brew "zeromq"
cask "gcc-arm-embedded"
brew "portaudio"
brew "gcc@13"

89
iqpilot/tools/README.md Normal file
View File

@@ -0,0 +1,89 @@
# IQ.Pilot tools
## System Requirements
IQ.Pilot is developed and tested on **Apple macOS**, which is the primary development target aside from supported vehicle hardware (3, 3x, 4).
Most of IQ.Pilot should work natively on macOS. On Windows you can use WSL for a nearly native Ubuntu experience. Running natively on any other system is not currently recommended and will likely require modifications.
## Native setup on Ubuntu 24.04 and macOS
Follow these instructions for a fully managed setup experience. If you'd like to manage the dependencies yourself, just read the setup scripts in this directory.
**1. Clone IQ.Pilot**
``` bash
git clone https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
```
**2. Run the setup script**
``` bash
cd IQ.Pilot
iqpilot/tools/iq.sh setup
```
**3. Activate a Python shell**
Activate a shell with the Python dependencies installed:
``` bash
source .venv/bin/activate
```
**4. Build IQ.Pilot**
``` bash
scons -u -j$(nproc)
```
# Using IQ.Pilot tools with Konn3kt:
This guide explains how to use IQ.Pilot tools to view and analyze routes from Konn3kt.
## Overview
All you need to do is authenticate with Konn3kt so you can access your routes.
## Quick Start
### 1. Authenticate with Konn3kt
Run the authentication helper:
```bash
cd IQ.Pilot
python3 iqpilot/tools/lib/auth.py
```
This will:
- Open your browser to log in via GitHub OAuth
- Save your authentication token to `~/.comma/auth.json`
- Allow access to your Konn3kt routes
If browser auto-open is unavailable (headless/WSL), copy the printed URL into any browser — the local callback listens on port 3000.
## How It Works
OP Tools reads your Konn3kt JWT token from `~/.comma/auth.json`.
You can always view public routes!
## WSL on Windows
[Windows Subsystem for Linux (WSL)](https://docs.microsoft.com/en-us/windows/wsl/about) should provide a similar experience to native Ubuntu. [WSL 2](https://docs.microsoft.com/en-us/windows/wsl/compare-versions) specifically has been reported by several users to be a seamless experience.
Follow [these instructions](https://docs.microsoft.com/en-us/windows/wsl/install) to setup the WSL and install the `Ubuntu-24.04` distribution. Once your Ubuntu WSL environment is setup, follow the Linux setup instructions to finish setting up your environment. See [these instructions](https://learn.microsoft.com/en-us/windows/wsl/tutorials/gui-apps) for running GUI apps.
**NOTE**: If you are running WSL and any GUIs are failing (segfaulting or other strange issues) even after following the steps above, you may need to enable software rendering with `LIBGL_ALWAYS_SOFTWARE=1`, e.g. `LIBGL_ALWAYS_SOFTWARE=1 selfdrive/ui/ui`.
## CTF
Learn about the IQ.Pilot ecosystem and tools by playing our [CTF](/tools/CTF.md).
## Directory Structure
```
├── cabana/ # View and plot CAN messages from drives or in realtime
├── joystick/ # Control your car with a joystick
├── jotpluggler/ # View and plot IQ.Pilot logs
├── lib/ # Libraries to support the tools and reading IQ.Pilot logs
├── maneuvers/ # Lateral and longitudinal maneuver testing
├── replay/ # Replay drives and mock IQ.Pilot services
├── scripts/ # Miscellaneous scripts
└── serial/ # Tools for using the comma serial
```

3
iqpilot/tools/__init__.py Normal file
View File

@@ -0,0 +1,3 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""

17
iqpilot/tools/auto_source.py Executable file
View File

@@ -0,0 +1,17 @@
#!/usr/bin/env python3
import sys
from iqpilot.tools.lib.logreader import LogReader, ReadMode
def main():
if len(sys.argv) != 2:
print("Usage: python auto_source.py <log_path>")
sys.exit(1)
log_path = sys.argv[1]
lr = LogReader(log_path, default_mode=ReadMode.AUTO, sort_by_time=True)
print("\n".join(lr.logreader_identifiers))
if __name__ == "__main__":
main()

457
iqpilot/tools/clip/run.py Executable file
View File

@@ -0,0 +1,457 @@
#!/usr/bin/env python3
import os
import sys
import time
import logging
import subprocess
import threading
import queue
import multiprocessing
import itertools
import numpy as np
import tqdm
from argparse import ArgumentParser
from pathlib import Path
from concurrent.futures import ThreadPoolExecutor, as_completed
from iqpilot.tools.lib.route import Route
from iqpilot.tools.lib.logreader import LogReader
from iqpilot.tools.lib.filereader import FileReader
from iqpilot.tools.lib.framereader import FrameReader, ffprobe
from iqpilot.selfdrive.test.process_replay.migration import migrate_all
from iqpilot.common.prefix import OpenpilotPrefix
from iqpilot.common.utils import Timer
from iqpilot.cereal.visionipc import VisionStreamType
from msgq.visionipc import VisionIpcServer
FRAMERATE = 20
DEMO_ROUTE, DEMO_START, DEMO_END = 'a2a0ccea32023010/2023-07-27--13-01-19', 90, 105
logger = logging.getLogger('clip')
def parse_args():
parser = ArgumentParser(description="Direct clip renderer")
parser.add_argument("route", nargs="?", help="Route ID (dongle/route or dongle/route/start/end)")
parser.add_argument("-s", "--start", type=int, help="Start time in seconds")
parser.add_argument("-e", "--end", type=int, help="End time in seconds")
parser.add_argument("-o", "--output", default="output.mp4", help="Output file path")
parser.add_argument("-d", "--data-dir", help="Local directory with route data")
parser.add_argument("-t", "--title", help="Title overlay text")
parser.add_argument("-f", "--file-size", type=float, default=9.0, help="Target file size in MB")
parser.add_argument("-x", "--speed", type=int, default=1, help="Speed multiplier")
parser.add_argument("--demo", action="store_true", help="Use demo route with default timing")
ui_group = parser.add_mutually_exclusive_group()
ui_group.add_argument("--big", dest="big", action="store_true", default=None, help="Force big UI (2160x1080)")
ui_group.add_argument("--mici", dest="big", action="store_false", help="Force mici UI (536x240)")
parser.add_argument("--qcam", action="store_true", help="Use qcamera instead of fcamera")
parser.add_argument("--windowed", action="store_true", help="Show window")
parser.add_argument("--no-metadata", action="store_true", help="Disable metadata overlay")
parser.add_argument("--no-time-overlay", action="store_true", help="Disable time overlay")
args = parser.parse_args()
if args.demo:
args.route, args.start, args.end = args.route or DEMO_ROUTE, args.start or DEMO_START, args.end or DEMO_END
elif not args.route:
parser.error("route is required (or use --demo)")
if args.route and args.route.count('/') == 3:
parts = args.route.split('/')
args.route, args.start, args.end = '/'.join(parts[:2]), args.start or int(parts[2]), args.end or int(parts[3])
if args.start is None or args.end is None:
parser.error("--start and --end are required")
if args.end <= args.start:
parser.error(f"end ({args.end}) must be greater than start ({args.start})")
return args
def setup_env(output_path: str, big: bool = False, speed: int = 1, target_mb: float = 0, duration: int = 0):
os.environ.update({"RECORD": "1", "OFFSCREEN": "1", "RECORD_OUTPUT": str(Path(output_path).with_suffix(".mp4"))})
if speed > 1:
os.environ["RECORD_SPEED"] = str(speed)
if target_mb > 0 and duration > 0:
os.environ["RECORD_BITRATE"] = f"{int(target_mb * 8 * 1024 / (duration / speed))}k"
if big:
os.environ["BIG"] = "1"
else:
os.environ["BIG"] = "0"
def _download_segment(path: str) -> bytes:
with FileReader(path) as f:
return bytes(f.read())
def _parse_and_chunk_segment(args: tuple) -> list[dict]:
raw_data, fps = args
from iqpilot.tools.lib.logreader import _LogFileReader
messages = migrate_all(list(_LogFileReader("", dat=raw_data, sort_by_time=True)))
if not messages:
return []
dt_ns, chunks, current, next_time = 1e9 / fps, [], {}, messages[0].logMonoTime + 1e9 / fps # type: ignore[var-annotated]
for msg in messages:
if msg.logMonoTime >= next_time:
chunks.append(current)
current, next_time = {}, next_time + dt_ns * ((msg.logMonoTime - next_time) // dt_ns + 1)
current[msg.which()] = msg
return chunks + [current] if current else chunks
def load_logs_parallel(log_paths: list[str], fps: int = 20) -> list[dict]:
num_workers = min(16, len(log_paths), (multiprocessing.cpu_count() or 1))
logger.info(f"Downloading {len(log_paths)} segments with {num_workers} workers...")
with ThreadPoolExecutor(max_workers=num_workers) as pool:
futures = {pool.submit(_download_segment, path): idx for idx, path in enumerate(log_paths)}
raw_data = {futures[f]: f.result() for f in as_completed(futures)}
logger.info("Parsing and chunking segments...")
with multiprocessing.Pool(num_workers) as pool:
return list(itertools.chain.from_iterable(pool.map(_parse_and_chunk_segment, [(raw_data[i], fps) for i in range(len(log_paths))])))
def patch_submaster(message_chunks, ui_state):
# Reset started_frame so alerts render correctly (recv_frame must be >= started_frame)
ui_state.started_frame = 0
ui_state.started_time = time.monotonic()
def mock_update(timeout=None):
sm, t = ui_state.sm, time.monotonic()
sm.updated = dict.fromkeys(sm.services, False)
if sm.frame < len(message_chunks):
for svc, msg in message_chunks[sm.frame].items():
if svc in sm.data:
sm.seen[svc] = sm.updated[svc] = sm.alive[svc] = sm.valid[svc] = True
sm.data[svc] = getattr(msg.as_builder(), svc)
sm.logMonoTime[svc], sm.recv_time[svc], sm.recv_frame[svc] = msg.logMonoTime, t, sm.frame
sm.frame += 1
ui_state.sm.update = mock_update
def get_frame_dimensions(camera_path: str) -> tuple[int, int]:
"""Get frame dimensions from a video file using ffprobe."""
probe = ffprobe(camera_path)
stream = probe["streams"][0]
return stream["width"], stream["height"]
def iter_segment_frames(camera_paths, start_time, end_time, fps=20, use_qcam=False,
frame_size: tuple[int, int] | None = None, on_segment_open=None):
frames_per_seg = fps * 60
start_frame, end_frame = int(start_time * fps), int(end_time * fps)
current_seg: int = -1
seg_frames: FrameReader | np.ndarray | None = None
for global_idx in range(start_frame, end_frame):
seg_idx, local_idx = global_idx // frames_per_seg, global_idx % frames_per_seg
if seg_idx != current_seg:
current_seg = seg_idx
path = camera_paths[seg_idx] if seg_idx < len(camera_paths) else None
if not path:
raise RuntimeError(f"No camera file for segment {seg_idx}")
if on_segment_open is not None:
on_segment_open(seg_idx, path)
if use_qcam:
w, h = frame_size or get_frame_dimensions(path)
with FileReader(path) as f:
result = subprocess.run(["ffmpeg", "-v", "quiet", "-i", "-", "-f", "rawvideo", "-pix_fmt", "nv12", "-"],
input=f.read(), capture_output=True)
if result.returncode != 0:
raise RuntimeError(f"ffmpeg failed: {result.stderr.decode()}")
seg_frames = np.frombuffer(result.stdout, dtype=np.uint8).reshape(-1, w * h * 3 // 2)
else:
seg_frames = FrameReader(path, pix_fmt="nv12")
assert seg_frames is not None
frame = seg_frames[local_idx] if use_qcam else seg_frames.get(local_idx) # type: ignore[index, union-attr]
yield global_idx, frame
class FrameQueue:
def __init__(self, camera_paths, start_time, end_time, fps=20, prefetch_count=60, use_qcam=False):
# Probe first valid camera file for dimensions
first_path = next((p for p in camera_paths if p), None)
if not first_path:
raise RuntimeError("No valid camera paths")
self.frame_w, self.frame_h = get_frame_dimensions(first_path)
self._queue, self._stop, self._error = queue.Queue(maxsize=prefetch_count), threading.Event(), None
self._use_qcam = use_qcam
self._current_seg_idx = None
self._current_path = first_path
self._state_lock = threading.Lock()
self._thread = threading.Thread(target=self._worker,
args=(camera_paths, start_time, end_time, fps, use_qcam, (self.frame_w, self.frame_h)), daemon=True)
self._thread.start()
def _set_current_source(self, seg_idx, path):
with self._state_lock:
self._current_seg_idx = seg_idx
self._current_path = path
def _worker(self, camera_paths, start_time, end_time, fps, use_qcam, frame_size):
try:
for idx, data in iter_segment_frames(camera_paths, start_time, end_time, fps, use_qcam, frame_size, self._set_current_source):
if self._stop.is_set():
break
self._queue.put((idx, data.tobytes()))
except Exception as e:
logger.exception("Decode error")
self._error = e
finally:
self._queue.put(None)
def get(self, timeout=60.0):
deadline = time.monotonic() + timeout
while True:
if self._error:
raise self._error
remaining = max(0.0, deadline - time.monotonic())
if remaining == 0.0:
break
try:
result = self._queue.get(timeout=min(0.5, remaining))
except queue.Empty:
continue
if result is None:
if self._error:
raise self._error
raise StopIteration("No more frames")
return result
if self._error:
raise self._error
with self._state_lock:
seg_idx = self._current_seg_idx
path = self._current_path
camera_kind = "qcamera" if self._use_qcam else "fcamera"
source = f"segment {seg_idx}" if seg_idx is not None else "the initial segment"
if path:
source = f"{source} ({path})"
hint = ""
if path and path.startswith(("http://", "https://")):
hint = " Try downloading the route locally with --data-dir or verify the remote camera endpoint supports timely range reads."
raise TimeoutError(f"Timed out after {timeout:.0f}s waiting for {camera_kind} frames from {source}; camera fetch or decode is stalled.{hint}")
def stop(self):
self._stop.set()
while not self._queue.empty():
try:
self._queue.get_nowait()
except queue.Empty:
break
self._thread.join(timeout=2.0)
def load_route_metadata(route):
from iqpilot.common.params import Params, UnknownKeyName
lr = LogReader(route.log_paths()[0])
init_data, car_params = lr.first('initData'), lr.first('carParams')
params = Params()
for entry in init_data.params.entries:
try:
value = params.cpp2python(entry.key, entry.value)
if value is None:
logger.warning("Skipping malformed route param %s while loading clip metadata", entry.key)
continue
params.put(entry.key, value)
except UnknownKeyName:
pass
except TypeError:
logger.warning("Skipping route param %s due to type mismatch while loading clip metadata", entry.key)
origin = init_data.gitRemote.split('/')[3] if len(init_data.gitRemote.split('/')) > 3 else 'unknown'
return {
'version': init_data.version, 'route': route.name.canonical_name,
'car': car_params.carFingerprint if car_params else 'unknown', 'origin': origin,
'commit': init_data.gitCommit[:7],
}
def detect_big_ui(route: Route) -> bool:
try:
init_data = LogReader(route.log_paths()[0]).first('initData')
git_branch = (init_data.gitBranch or "").lower()
device_type = str(init_data.deviceType).lower()
if device_type in ("mici", "tizi", "tici"):
big = device_type != "mici"
reason = f"route device type {device_type}"
elif "mici" in git_branch:
big = False
reason = f"route branch {git_branch}"
else:
big = True
reason = f"route branch {git_branch or 'unknown'}"
logger.info("Auto-detected %s UI from %s", "big" if big else "mici", reason)
return big
except Exception:
logger.warning("Falling back to big UI; failed to auto-detect UI mode", exc_info=True)
return True
def draw_text_box(rl, text, x, y, size, gui_app, font, font_scale, color=None, center=False):
box_color, text_color = rl.Color(0, 0, 0, 85), color or rl.WHITE
# measure_text_ex is NOT auto-scaled, so multiply by font_scale
# draw_text_ex IS auto-scaled, so pass size directly
text_size = rl.measure_text_ex(font, text, size * font_scale, 0)
text_width, text_height = int(text_size.x), int(text_size.y)
if center:
x = (gui_app.width - text_width) // 2
rl.draw_rectangle(x - 8, y - 4, text_width + 16, text_height + 8, box_color)
rl.draw_text_ex(font, text, rl.Vector2(x, y), size, 0, text_color)
def render_overlays(rl, gui_app, font, font_scale, metadata, title, start_time, frame_idx, show_metadata, show_time):
if show_metadata and metadata and frame_idx < FRAMERATE * 5:
m = metadata
text = ", ".join([f"IQ.Pilot v{m['version']}", f"route: {m['route']}", f"car: {m['car']}", f"origin: {m['origin']}",
f"commit: {m['commit']}"])
# Truncate if too wide (leave 20px margin on each side)
max_width = gui_app.width - 40
while rl.measure_text_ex(font, text, 15 * font_scale, 0).x > max_width and len(text) > 20:
text = text[:-4] + "..."
draw_text_box(rl, text, 0, 8, 15, gui_app, font, font_scale, center=True)
if title:
draw_text_box(rl, title, 0, 60, 32, gui_app, font, font_scale, center=True)
if show_time:
t = start_time + frame_idx / FRAMERATE
time_text = f"{int(t)//60:02d}:{int(t)%60:02d}"
time_width = int(rl.measure_text_ex(font, time_text, 24 * font_scale, 0).x)
draw_text_box(rl, time_text, gui_app.width - time_width - 45, 45, 24, gui_app, font, font_scale)
def prefetch_nav_tiles(road_view, ui_state, message_chunks) -> None:
"""If the route drove with on-screen maps enabled, feed the panel the first messages and block
until the opening viewport's tiles are fetched, so the clip doesn't start on the placeholder grid."""
nav_panel = getattr(getattr(road_view, "_hud_renderer", None), "nav_map_panel", None)
if nav_panel is None or not getattr(nav_panel, "maps_enabled", lambda: False)():
return
logger.info("Route has on-screen maps enabled, prefetching map tiles...")
for _ in range(min(len(message_chunks), FRAMERATE * 2)):
ui_state.sm.update()
nav_panel.update()
if nav_panel.active:
break
if nav_panel.active:
if not nav_panel.warm_up_tiles():
logger.warning("Map tiles incomplete after warmup (missing MapboxToken or slow network); map may render partially")
else:
logger.warning("No nav position in the first seconds of the clip; map tiles will load mid-clip")
ui_state.sm.frame = 0
def clip(route: Route, output: str, start: int, end: int, headless: bool = True, big: bool = False,
title: str | None = None, show_metadata: bool = True, show_time: bool = True, use_qcam: bool = False):
timer, duration = Timer(), end - start
# The prefix must wrap the UI imports: ui_state and the nav map panel bind Params() to the
# params path active when they're constructed, and load_route_metadata below seeds the route's
# params (on-screen maps gate, units, Mapbox token) into the prefixed dir.
with OpenpilotPrefix(shared_download_cache=True):
import pyray as rl
if big:
from iqpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView
else:
from iqpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView # type: ignore[assignment]
from iqpilot.selfdrive.ui.ui_state import ui_state
from iqpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE
timer.lap("import")
logger.info(f"Clipping {route.name.canonical_name}, {start}s-{end}s ({duration}s)")
seg_start, seg_end = start // 60, (end - 1) // 60 + 1
all_chunks = load_logs_parallel(route.log_paths()[seg_start:seg_end], fps=FRAMERATE)
timer.lap("logs")
frame_start = (start - seg_start * 60) * FRAMERATE
message_chunks = all_chunks[frame_start:frame_start + duration * FRAMERATE]
if not message_chunks:
logger.error("No messages to render")
sys.exit(1)
metadata = load_route_metadata(route)
if not show_metadata:
metadata = None
if headless:
rl.set_config_flags(rl.ConfigFlags.FLAG_WINDOW_HIDDEN)
camera_paths = route.qcamera_paths() if use_qcam else route.camera_paths()
frame_queue = FrameQueue(camera_paths, start, end, fps=FRAMERATE, use_qcam=use_qcam)
ecamera_paths = route.ecamera_paths() if not use_qcam else []
wide_frame_queue: FrameQueue | None = None
if any(p for p in ecamera_paths[seg_start:seg_end] if p):
wide_frame_queue = FrameQueue(ecamera_paths, start, end, fps=FRAMERATE)
vipc = VisionIpcServer("camerad")
vipc.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 4, frame_queue.frame_w, frame_queue.frame_h)
if wide_frame_queue:
vipc.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 4, wide_frame_queue.frame_w, wide_frame_queue.frame_h)
vipc.start_listener()
patch_submaster(message_chunks, ui_state)
gui_app.init_window("clip", fps=FRAMERATE)
road_view = AugmentedRoadView()
road_view.set_rect(rl.Rectangle(0, 0, gui_app.width, gui_app.height))
font = gui_app.font(FontWeight.NORMAL)
prefetch_nav_tiles(road_view, ui_state, message_chunks)
timer.lap("setup")
frame_idx = 0
with tqdm.tqdm(total=len(message_chunks), desc="Rendering", unit="frame") as pbar:
for should_render in gui_app.render():
if frame_idx >= len(message_chunks):
break
_, frame_bytes = frame_queue.get()
vipc.send(VisionStreamType.VISION_STREAM_ROAD, frame_bytes, frame_idx, int(frame_idx * 5e7), int(frame_idx * 5e7))
if wide_frame_queue:
_, wide_bytes = wide_frame_queue.get()
vipc.send(VisionStreamType.VISION_STREAM_WIDE_ROAD, wide_bytes, frame_idx, int(frame_idx * 5e7), int(frame_idx * 5e7))
ui_state.update()
if should_render:
road_view.render()
render_overlays(rl, gui_app, font, FONT_SCALE, metadata, title, start, frame_idx, show_metadata, show_time)
frame_idx += 1
pbar.update(1)
timer.lap("render")
frame_queue.stop()
if wide_frame_queue:
wide_frame_queue.stop()
gui_app.close()
timer.lap("ffmpeg")
logger.info(f"Clip saved to: {Path(output).resolve()}")
logger.info(f"Generated {timer.fmt(duration)}")
def main():
logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s\t%(message)s")
args = parse_args()
route = Route(args.route, data_dir=args.data_dir)
big = args.big if args.big is not None else detect_big_ui(route)
setup_env(args.output, big=big, speed=args.speed, target_mb=args.file_size, duration=args.end - args.start)
try:
clip(route, args.output, args.start, args.end, not args.windowed,
big, args.title, not args.no_metadata, not args.no_time_overlay, args.qcam)
except TimeoutError as e:
logger.error("%s", e)
sys.exit(1)
if __name__ == "__main__":
main()

43
iqpilot/tools/install_python_dependencies.sh Executable file
View File

@@ -0,0 +1,43 @@
#!/usr/bin/env bash
set -euo pipefail
# Increase the pip timeout to handle TimeoutError
export PIP_DEFAULT_TIMEOUT=200
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
ROOT="$DIR"/../../
cd "$ROOT"
if ! command -v "uv" > /dev/null 2>&1; then
echo "installing uv..."
curl -LsSf --retry 5 --retry-delay 5 --retry-all-errors https://astral.sh/uv/install.sh | sh
UV_BIN="$HOME/.local/bin"
PATH="$UV_BIN:$PATH"
fi
echo "updating uv..."
# ok to fail, can also fail due to installing with brew
uv self update || true
echo "installing python packages..."
UV_SYNC_ARGS=(--frozen)
if [[ "${IQPILOT_RUNTIME_DEPENDENCIES_ONLY:-0}" != "1" ]]; then
UV_SYNC_ARGS+=(--all-extras)
fi
UV_SYNC_OK=0
for attempt in 1 2 3; do
if uv sync "${UV_SYNC_ARGS[@]}"; then
UV_SYNC_OK=1
break
fi
[[ "${attempt}" -lt 3 ]] && sleep "$((attempt * 5))"
done
if [[ "${UV_SYNC_OK}" -ne 1 ]]; then
exit 1
fi
source .venv/bin/activate
if [[ "$(uname)" == 'Darwin' ]]; then
touch "$ROOT"/.env
echo "export OBJC_DISABLE_INITIALIZE_FORK_SAFETY=YES" >> "$ROOT"/.env
fi

127
iqpilot/tools/install_ubuntu_dependencies.sh Executable file
View File

@@ -0,0 +1,127 @@
#!/usr/bin/env bash
set -e
SUDO=""
# Use sudo if not root
if [[ ! $(id -u) -eq 0 ]]; then
if [[ -z $(which sudo) ]]; then
echo "Please install sudo or run as root"
exit 1
fi
SUDO="sudo"
fi
# Check if stdin is open
if [ -t 0 ]; then
INTERACTIVE=1
fi
# Install common packages
function install_ubuntu_common_requirements() {
$SUDO apt-get update
# normal stuff, mostly for the bare docker image
$SUDO apt-get install -y --no-install-recommends \
ca-certificates \
clang \
build-essential \
curl \
libssl-dev \
libcurl4-openssl-dev \
locales \
git \
git-lfs \
xvfb
# TODO: vendor the rest of these in third_party/
$SUDO apt-get install -y --no-install-recommends \
gcc-arm-none-eabi \
capnproto \
libcapnp-dev \
libdrm-dev \
ffmpeg \
libavformat-dev \
libavcodec-dev \
libavdevice-dev \
libavutil-dev \
libavfilter-dev \
libbz2-dev \
libeigen3-dev \
libffi-dev \
libgles2-mesa-dev \
libglfw3-dev \
libglib2.0-0 \
libjpeg-dev \
libncurses5-dev \
libusb-1.0-0-dev \
libva-dev \
libx264-dev \
libzmq3-dev \
libzstd-dev \
libsqlite3-dev \
opencl-headers \
ocl-icd-libopencl1 \
ocl-icd-opencl-dev \
portaudio19-dev \
gettext
}
# Install Ubuntu 24.04 LTS packages
function install_ubuntu_lts_latest_requirements() {
install_ubuntu_common_requirements
$SUDO apt-get install -y --no-install-recommends \
g++-12 \
python3-dev \
python3-venv
}
# Detect OS using /etc/os-release file
if [ -f "/etc/os-release" ]; then
source /etc/os-release
case "$VERSION_CODENAME" in
"jammy" | "kinetic" | "noble")
install_ubuntu_lts_latest_requirements
;;
*)
echo "$ID $VERSION_ID is unsupported. This setup script is written for Ubuntu 24.04."
read -p "Would you like to attempt installation anyway? " -n 1 -r
echo ""
if [[ ! $REPLY =~ ^[Yy]$ ]]; then
exit 1
fi
install_ubuntu_lts_latest_requirements
esac
if [[ -d "/etc/udev/rules.d/" ]]; then
# Setup jungle udev rules
$SUDO tee /etc/udev/rules.d/12-panda_jungle.rules > /dev/null <<EOF
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcf", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddef", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcf", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddef", MODE="0666"
EOF
# Setup panda udev rules
$SUDO tee /etc/udev/rules.d/11-panda.rules > /dev/null <<EOF
SUBSYSTEM=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="df11", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddcc", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="3801", ATTRS{idProduct}=="ddee", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddcc", MODE="0666"
SUBSYSTEM=="usb", ATTRS{idVendor}=="bbaa", ATTRS{idProduct}=="ddee", MODE="0666"
EOF
# Setup adb udev rules
$SUDO tee /etc/udev/rules.d/50-comma-adb.rules > /dev/null <<EOF
SUBSYSTEM=="usb", ATTR{idVendor}=="04d8", ATTR{idProduct}=="1234", ENV{adb_user}="yes"
EOF
$SUDO udevadm control --reload-rules && $SUDO udevadm trigger || true
fi
else
echo "No /etc/os-release in the system. Make sure you're running on Ubuntu, or similar."
exit 1
fi

328
iqpilot/tools/iq.sh Executable file
View File

@@ -0,0 +1,328 @@
#!/usr/bin/env bash
# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
set -euo pipefail
IQ_RESET='\033[0m'
IQ_BOLD='\033[1m'
IQ_MUTED='\033[38;5;245m'
IQ_RED='\033[38;5;204m'
IQ_GREEN='\033[38;5;120m'
IQ_CYAN='\033[38;2;38;145;184m'
IQ_BLUE='\033[38;2;51;112;176m'
IQ_PURPLE='\033[38;5;141m'
IQ_PINK='\033[38;5;211m'
IQ_ROOT=''
IQ_DRY=0
IQ_NO_VERIFY=0
IQ_LOG_FILE=''
IQ_REBOOT=0
if [[ ! -t 1 || "${NO_COLOR:-}" != '' ]]; then
IQ_RESET='' IQ_BOLD='' IQ_MUTED='' IQ_RED='' IQ_GREEN='' IQ_CYAN='' IQ_BLUE='' IQ_PURPLE='' IQ_PINK=''
fi
iq_line() {
printf '%b%s%b\n' "$IQ_CYAN" '━━━━━━━━━━━━' "$IQ_RESET"
}
iq_title() {
printf '%bI%b%bQ%b%b.%b%bP%b%bi%b%bl%b%bo%b%bt%b %b%s%b\n' "$IQ_CYAN" "$IQ_RESET" "$IQ_BLUE" "$IQ_RESET" "$IQ_PURPLE" "$IQ_RESET" "$IQ_PINK" "$IQ_RESET" "$IQ_PURPLE" "$IQ_RESET" "$IQ_BLUE" "$IQ_RESET" "$IQ_CYAN" "$IQ_RESET" "$IQ_CYAN" "$IQ_RESET" "$IQ_MUTED" "$1" "$IQ_RESET"
}
iq_ok() {
printf ' %b●%b %s\n' "$IQ_GREEN" "$IQ_RESET" "$1"
}
iq_fail() {
printf ' %b●%b %s\n' "$IQ_RED" "$IQ_RESET" "$1" >&2
}
iq_note() {
printf ' %b●%b %s\n' "$IQ_CYAN" "$IQ_RESET" "$1"
}
iq_find_root() {
local candidate="${IQ_ROOT:-$PWD}"
while [[ "$candidate" != / ]]; do
if [[ ( -f "$candidate/launch_iqpilot.sh" || -f "$candidate/launch_openpilot.sh" ) && -d "$candidate/iqpilot" ]]; then
IQ_ROOT="$candidate"
return 0
fi
candidate="$(cd "$candidate/.." && pwd)"
done
for candidate in "$HOME/iqpilot" "$HOME/openpilot" /data/iqpilot /data/openpilot; do
if [[ ( -f "$candidate/launch_iqpilot.sh" || -f "$candidate/launch_openpilot.sh" ) && -d "$candidate/iqpilot" ]]; then
IQ_ROOT="$candidate"
return 0
fi
done
return 1
}
iq_require_root() {
if ! iq_find_root; then
iq_fail 'IQ.Pilot checkout not found. Run this inside the checkout or use --dir PATH.'
return 1
fi
}
iq_run() {
local rendered
printf -v rendered '%q ' "$@"
rendered="${rendered% }"
printf '%b›%b %b%s%b\n' "$IQ_PINK" "$IQ_RESET" "$IQ_MUTED" "$rendered" "$IQ_RESET"
[[ "$IQ_DRY" = 1 ]] || "$@"
}
iq_check() {
iq_title 'environment check'
iq_require_root
iq_ok "checkout $IQ_ROOT"
command -v git >/dev/null 2>&1 || { iq_fail 'git is not installed'; return 1; }
iq_ok "git $(git --version | sed 's/git version //')"
command -v python3 >/dev/null 2>&1 || { iq_fail 'python3 is not installed'; return 1; }
iq_ok "python $(python3 --version | sed 's/Python //')"
if [[ -x "$IQ_ROOT/.venv/bin/python3" ]]; then
iq_ok 'venv ready'
else
iq_note 'venv not created yet — run iq setup'
fi
}
iq_install() {
local shell_name rc_file iq_script command
shell_name="$(basename "${SHELL:-bash}")"
rc_file="$HOME/.${shell_name}rc"
[[ "$(uname)" = Darwin && "$shell_name" = bash ]] && rc_file="$HOME/.bash_profile"
iq_script="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/iq.sh"
command="alias iq='${iq_script} \"\$@\"'"
touch "$rc_file"
grep -Fqx "$command" "$rc_file" 2>/dev/null || printf '\n%s\n' "$command" >> "$rc_file"
iq_title 'command installed'
iq_ok "restart your shell, then use iq ${rc_file}"
}
iq_setup() {
local script
iq_require_root
if [[ -f /AGNOS ]]; then
iq_title 'setup'
iq_note 'IQ.OS manages system dependencies and the base Python runtime'
iq_note 'use iq pkg to synchronize private packages when needed'
iq_check
return 0
fi
case "$OSTYPE" in
linux-gnu*) script="$IQ_ROOT/iqpilot/tools/ubuntu_setup.sh" ;;
darwin*) script="$IQ_ROOT/iqpilot/tools/mac_setup.sh" ;;
*) iq_fail "unsupported platform: $OSTYPE"; return 1 ;;
esac
iq_title 'setup'
iq_run "$script"
if command -v git-lfs >/dev/null 2>&1; then
iq_run git -C "$IQ_ROOT" lfs pull
fi
iq_ok 'setup complete'
}
iq_venv() {
iq_require_root
[[ -f "$IQ_ROOT/.venv/bin/activate" ]] || { iq_fail 'venv not found — run iq setup first'; return 1; }
case "$(basename "${SHELL:-bash}")" in
zsh) ZDOTDIR="$(mktemp -d)"; printf 'source %q\nsource %q\n' "$HOME/.zshrc" "$IQ_ROOT/.venv/bin/activate" > "$ZDOTDIR/.zshrc"; zsh ;;
*) bash --rcfile <(printf 'source %q\nsource %q\n' "$HOME/.bashrc" "$IQ_ROOT/.venv/bin/activate") ;;
esac
}
iq_build() {
iq_require_root
if [[ -f /AGNOS ]]; then
iq_run "$IQ_ROOT/iqpilot/system/manager/build.py"
else
(cd "$IQ_ROOT" && iq_run scons "$@")
fi
}
iq_quality() {
iq_require_root
(cd "$IQ_ROOT" && iq_run scripts/lint/lint.sh "$@")
}
iq_desktop_tool() {
local name="$1" launcher="$2"
shift 2
iq_require_root
if [[ ! -x "$IQ_ROOT/$launcher" ]]; then
if [[ -f /AGNOS ]]; then
iq_note "$name is not included in IQ.OS checkouts"
return 0
fi
iq_fail "$name launcher is missing: $IQ_ROOT/$launcher"
return 1
fi
iq_title "opening $name"
(cd "$IQ_ROOT" && iq_run "$IQ_ROOT/$launcher" "$@")
}
iq_cabana() {
iq_desktop_tool 'Cabana' 'iqpilot/tools/cabana/cabana' "$@"
}
iq_juggle() {
iq_desktop_tool 'Jotpluggler' 'iqpilot/tools/jotpluggler/pluggle.py' "$@"
}
iq_pkg() {
iq_require_root
# the venv python has the component packages (iqdbc etc.); bare python3 does not
local py=python3
[[ -x "$IQ_ROOT/.venv/bin/python3" ]] && py="$IQ_ROOT/.venv/bin/python3"
if [[ -f "$IQ_ROOT/iqpilot/tools/scripts/setup_private_packages.py" ]]; then
(cd "$IQ_ROOT" && iq_run "$py" iqpilot/tools/scripts/setup_private_packages.py "$@")
else
iq_note 'private package sources are not present in this checkout'
fi
if [[ -f "$IQ_ROOT/artifacts/runtime/ensure_private_installed.sh" ]]; then
(cd "$IQ_ROOT" && iq_run bash artifacts/runtime/ensure_private_installed.sh)
fi
}
iq_update() {
local fast=0
iq_require_root
if [[ $# -gt 0 ]]; then
[[ $# = 1 && "$1" = f ]] || { iq_fail 'usage: iq update [f] [r]'; return 1; }
fast=1
fi
iq_title 'updating IQ.Pilot'
iq_run git -C "$IQ_ROOT" pull
iq_pkg
[[ "$fast" = 1 ]] && iq_fast_restart
}
iq_status() {
local branch commit tree
iq_require_root
branch="$(git -C "$IQ_ROOT" branch --show-current 2>/dev/null || printf detached)"
commit="$(git -C "$IQ_ROOT" rev-parse --short HEAD 2>/dev/null || printf unknown)"
tree=clean
[[ -n "$(git -C "$IQ_ROOT" status --porcelain 2>/dev/null)" ]] && tree=modified
iq_title 'status'
iq_note "root $IQ_ROOT"
iq_note "branch $branch"
iq_note "commit $commit"
[[ "$tree" = clean ]] && iq_ok "tree $tree" || iq_note "tree $tree"
}
iq_switch() {
local remote=origin branch
iq_require_root
[[ $# -ge 1 ]] || { iq_fail 'usage: iq switch [REMOTE] BRANCH'; return 1; }
[[ $# -ge 2 ]] && { remote="$1"; shift; }
branch="$1"
iq_title "switching to $remote/$branch"
iq_note 'this discards uncommitted changes and untracked files'
iq_run git -C "$IQ_ROOT" fetch "$remote" "$branch:refs/remotes/$remote/$branch"
iq_run git -C "$IQ_ROOT" checkout -B "$branch" --track "$remote/$branch"
iq_run git -C "$IQ_ROOT" reset --hard "$remote/$branch"
iq_run git -C "$IQ_ROOT" clean -df
}
iq_service() {
local action="$1"
[[ -f /AGNOS ]] || { iq_note "${action} is available on IQ.OS devices only"; return 0; }
iq_run sudo systemctl "$action" iq
}
iq_wait_for_tmux() {
local expected="$1" attempt active
[[ "$IQ_DRY" = 1 ]] && { iq_note "would verify IQ tmux session ${expected}"; return 0; }
for ((attempt = 0; attempt < 15; attempt++)); do
if tmux has-session -t iq 2>/dev/null; then active=running; else active=stopped; fi
[[ "$active" = "$expected" ]] && return 0
sleep 1
done
return 1
}
iq_fast_restart() {
[[ -f /AGNOS || "$IQ_DRY" = 1 ]] || { iq_fail 'fast update restart is available on IQ.OS devices only'; return 1; }
iq_title 'fast restarting IQ.Pilot'
iq_run sudo systemctl stop iq
if ! iq_wait_for_tmux stopped; then
iq_fail 'IQ tmux session did not stop; attempt a reboot.'
return 1
fi
iq_run sudo systemctl start iq
if ! iq_wait_for_tmux running; then
iq_fail 'IQ tmux session did not start; attempt a reboot.'
return 1
fi
iq_ok 'IQ.Pilot restarted'
}
iq_help() {
iq_title 'command center'
printf '%bUsage%b iq [--dir PATH] [--dry] COMMAND [ARGS] [r]\n\n' "$IQ_BOLD" "$IQ_RESET"
printf '%bCOMMAND%b\n' "$IQ_PURPLE" "$IQ_RESET"
printf ' %bsetup%b install development dependencies\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bcheck%b verify checkout, Git, Python, and venv\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bbuild%b build IQ.Pilot\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bquality%b run code-quality checks\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bcabana%b open the CAN analysis tool\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bjuggle%b open the log plotting tool\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bpkg%b authenticate and synchronize private packages\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bupdate%b pull IQ.Pilot, synchronize packages, optionally fast restart\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bstatus%b show checkout, branch, commit, and tree state\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bswitch%b replace the checkout with another branch\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bvenv%b open a shell with the project venv active\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bstart%b start IQ.Pilot on IQ.OS\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %bstop%b stop IQ.Pilot on IQ.OS\n' "$IQ_CYAN" "$IQ_RESET"
printf ' %binstall%b install the iq shell command\n' "$IQ_CYAN" "$IQ_RESET"
printf '\n %br%b reboot after a successful command\n' "$IQ_CYAN" "$IQ_RESET"
printf '\n%bExamples%b\n' "$IQ_PURPLE" "$IQ_RESET"
printf ' iq setup iq update iq update f iq update f r\n'
iq_line
}
while [[ $# -gt 0 ]]; do
case "$1" in
-d|--dir) [[ $# -ge 2 ]] || { iq_fail 'missing path after --dir'; exit 2; }; IQ_ROOT="$2"; shift 2 ;;
--dry) IQ_DRY=1; shift ;;
-n|--no-verify) IQ_NO_VERIFY=1; shift ;;
-l|--log) [[ $# -ge 2 ]] || { iq_fail 'missing file after --log'; exit 2; }; IQ_LOG_FILE="$2"; shift 2 ;;
-h|--help|help) iq_help; exit 0 ;;
*) break ;;
esac
done
command="${1:-help}"
[[ $# -gt 0 ]] && shift
if [[ $# -gt 0 && "${!#}" = r ]]; then
IQ_REBOOT=1
set -- "${@:1:$#-1}"
fi
case "$command" in
help) iq_help ;;
setup) iq_setup "$@" ;;
check) iq_check "$@" ;;
build) iq_build "$@" ;;
quality) iq_quality "$@" ;;
cabana) iq_cabana "$@" ;;
juggle) iq_juggle "$@" ;;
pkg) iq_pkg "$@" ;;
update) iq_update "$@" ;;
status) iq_status "$@" ;;
switch) iq_switch "$@" ;;
venv) iq_venv "$@" ;;
start) iq_service start "$@" ;;
stop) iq_service stop "$@" ;;
install) iq_install "$@" ;;
*) iq_fail "unknown command: $command"; iq_help; exit 2 ;;
esac
if [[ "$IQ_REBOOT" = 1 ]]; then
iq_title 'restarting IQ.OS'
iq_run sudo reboot
fi

1
iqpilot/tools/iqperf/__init__.py Executable file
View File

@@ -0,0 +1 @@

82
iqpilot/tools/iqperf/analyze_msg_size.py Executable file
View File

@@ -0,0 +1,82 @@
#!/usr/bin/env python3
import argparse
from tqdm import tqdm
from iqpilot.cereal.services import SERVICE_LIST, QueueSize
from iqpilot.tools.lib.logreader import LogReader
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Analyze message sizes from a log route")
parser.add_argument("route", nargs="?", default="98395b7c5b27882e/000000a8--f87e7cd255",
help="Log route to analyze (default: 98395b7c5b27882e/000000a8--f87e7cd255)")
args = parser.parse_args()
lr = LogReader(args.route)
szs = {}
for msg in tqdm(lr):
sz = len(msg.as_builder().to_bytes())
msg_type = msg.which()
if msg_type not in szs:
szs[msg_type] = {'min': sz, 'max': sz, 'sum': sz, 'count': 1}
else:
szs[msg_type]['min'] = min(szs[msg_type]['min'], sz)
szs[msg_type]['max'] = max(szs[msg_type]['max'], sz)
szs[msg_type]['sum'] += sz
szs[msg_type]['count'] += 1
print()
print(f"{'Service':<36} {'Min (KB)':>12} {'Max (KB)':>12} {'Avg (KB)':>12} {'KB/min':>12} {'KB/sec':>12} {'Minutes in 10MB':>18} {'Seconds in Queue':>18}")
print("-" * 132)
def sort_key(x):
k, v = x
avg = v['sum'] / v['count']
freq = SERVICE_LIST.get(k, None)
freq_val = freq.frequency if freq else 0.0
kb_per_min = (avg * freq_val * 60) / 1024 if freq_val > 0 else 0.0
return kb_per_min
total_kb_per_min = 0.0
RINGBUFFER_SIZE_KB = 10 * 1024 # 10MB old default
for k, v in sorted(szs.items(), key=sort_key, reverse=True):
avg = v['sum'] / v['count']
service = SERVICE_LIST.get(k, None)
freq_val = service.frequency if service else 0.0
queue_size_kb = (service.queue_size / 1024) if service else 250 # default to SMALL
kb_per_min = (avg * freq_val * 60) / 1024 if freq_val > 0 else 0.0
kb_per_sec = kb_per_min / 60
minutes_in_buffer = RINGBUFFER_SIZE_KB / kb_per_min if kb_per_min > 0 else float('inf')
seconds_in_queue = (queue_size_kb / kb_per_sec) if kb_per_sec > 0 else float('inf')
total_kb_per_min += kb_per_min
min_str = f"{minutes_in_buffer:.2f}" if minutes_in_buffer != float('inf') else "inf"
sec_queue_str = f"{seconds_in_queue:.2f}" if seconds_in_queue != float('inf') else "inf"
print(f"{k:<36} {v['min']/1024:>12.2f} {v['max']/1024:>12.2f} {avg/1024:>12.2f} {kb_per_min:>12.2f} {kb_per_sec:>12.2f} {min_str:>18} {sec_queue_str:>18}")
# Summary section
print()
print(f"Total usage: {total_kb_per_min / 1024:.2f} MB/min")
# Calculate memory usage: old (10MB for all) vs new (from services.py)
OLD_SIZE = 10 * 1024 * 1024 # 10MB was the old default
old_total = len(SERVICE_LIST) * OLD_SIZE
new_total = sum(s.queue_size for s in SERVICE_LIST.values())
# Count by queue size
size_counts = {QueueSize.BIG: 0, QueueSize.MEDIUM: 0, QueueSize.SMALL: 0}
for s in SERVICE_LIST.values():
size_counts[s.queue_size] += 1
savings_pct = (1 - new_total / old_total) * 100
print()
print(f"{'Queue Size Comparison':<40}")
print("-" * 60)
print(f"{'Old (10MB default):':<30} {old_total / 1024 / 1024:>10.2f} MB")
print(f"{'New (from services.py):':<30} {new_total / 1024 / 1024:>10.2f} MB")
print(f"{'Savings:':<30} {savings_pct:>10.1f}%")
print()
print(f"{'Breakdown:':<30}")
print(f" BIG (10MB): {size_counts[QueueSize.BIG]:>3} services")
print(f" MEDIUM (2MB): {size_counts[QueueSize.MEDIUM]:>3} services")
print(f" SMALL (250KB): {size_counts[QueueSize.SMALL]:>3} services")

35
iqpilot/tools/iqperf/check_can_parser_performance.py Executable file
View File

@@ -0,0 +1,35 @@
#!/usr/bin/env python3
import numpy as np
import time
from tqdm import tqdm
from iqpilot.cereal import car
from iqdbc.car.tests.routes import CarTestRoute
from iqpilot.selfdrive.car.tests.test_models import TestCarModelBase
N_RUNS = 10
DEMO_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19"
class CarModelTestCase(TestCarModelBase):
test_route = CarTestRoute(DEMO_ROUTE, None)
if __name__ == '__main__':
# Get CAN messages and parsers
tm = CarModelTestCase()
tm.setUpClass()
tm.setUp()
CC = car.CarControl.new_message()
ets = []
for _ in tqdm(range(N_RUNS)):
start_t = time.process_time_ns()
for msg in tm.can_msgs:
for cp in tm.CI.can_parsers.values():
if cp is not None:
cp.update_strings(msg)
ets.append((time.process_time_ns() - start_t) * 1e-6)
print(f'{len(tm.can_msgs)} CAN packets, {N_RUNS} runs')
print(f'{np.mean(ets):.2f} mean ms, {max(ets):.2f} max ms, {min(ets):.2f} min ms, {np.std(ets):.2f} std ms')
print(f'{np.mean(ets) / len(tm.can_msgs):.4f} mean ms / CAN packet')

50
iqpilot/tools/iqperf/check_freq.py Executable file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python3
import argparse
import numpy as np
import time
from collections import defaultdict, deque
from collections.abc import MutableSequence
import iqpilot.cereal.messaging as messaging
if __name__ == "__main__":
context = messaging.Context()
poller = messaging.Poller()
parser = argparse.ArgumentParser()
parser.add_argument("socket", type=str, nargs='*', help="socket name")
args = parser.parse_args()
socket_names = args.socket
sockets = {}
rcv_times: defaultdict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100))
valids: defaultdict[str, deque[bool]] = defaultdict(lambda: deque(maxlen=100))
t = time.monotonic()
for name in socket_names:
sock = messaging.sub_sock(name, poller=poller)
sockets[sock] = name
prev_print = t
while True:
for socket in poller.poll(100):
msg = messaging.recv_one(socket)
if msg is None:
continue
name = msg.which()
t = time.monotonic()
rcv_times[name].append(msg.logMonoTime / 1e9)
valids[name].append(msg.valid)
if t - prev_print > 1:
print()
for name in socket_names:
dts = np.diff(rcv_times[name])
mean = np.mean(dts)
print(f"{name}: Freq {1.0 / mean:.2f} Hz, Min {np.min(dts) / mean * 100:.2f}%, Max {np.max(dts) / mean * 100:.2f}%, valid ", all(valids[name]))
prev_print = t

27
iqpilot/tools/iqperf/check_lag.py Executable file
View File

@@ -0,0 +1,27 @@
#!/usr/bin/env python3
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal.services import SERVICE_LIST
TO_CHECK = ['carState']
if __name__ == "__main__":
sm = messaging.SubMaster(TO_CHECK)
prev_t: dict[str, float] = {}
while True:
sm.update()
for s in TO_CHECK:
if sm.updated[s]:
t = sm.logMonoTime[s] / 1e9
if s in prev_t:
expected = 1.0 / (SERVICE_LIST[s].frequency)
dt = t - prev_t[s]
if dt > 10 * expected:
print(t, s, dt)
prev_t[s] = t

36
iqpilot/tools/iqperf/check_timings.py Executable file
View File

@@ -0,0 +1,36 @@
#!/usr/bin/env python3
import sys
import time
import numpy as np
import datetime
from collections.abc import MutableSequence
from collections import defaultdict
import iqpilot.cereal.messaging as messaging
if __name__ == "__main__":
ts: defaultdict[str, MutableSequence[float]] = defaultdict(list)
socks = {s: messaging.sub_sock(s, conflate=False) for s in sys.argv[1:]}
try:
st = time.monotonic()
while True:
print()
for s, sock in socks.items():
msgs = messaging.drain_sock(sock)
for m in msgs:
ts[s].append(m.logMonoTime / 1e6)
if len(ts[s]) > 2:
d = np.diff(ts[s])[-100:]
print(f"{s:25} {np.mean(d):7.2f} {np.std(d):7.2f} {np.max(d):7.2f} {np.min(d):7.2f}")
time.sleep(1)
except KeyboardInterrupt:
print("\n")
print("="*5, "timing summary", "="*5)
for s, sock in socks.items():
msgs = messaging.drain_sock(sock)
if len(ts[s]) > 2:
d = np.diff(ts[s])
print(f"{s:25} {np.mean(d):7.2f} {np.std(d):7.2f} {np.max(d):7.2f} {np.min(d):7.2f}")
print("="*5, datetime.timedelta(seconds=time.monotonic()-st), "="*5)

120
iqpilot/tools/iqperf/cpu_usage_stat.py Executable file
View File

@@ -0,0 +1,120 @@
#!/usr/bin/env python3
'''
System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs.
Features:
Use psutil library to sample cpu usage(avergage for all cores) of openpilot processes, at a rate of 5 samples/sec.
Do cpu usage statistics periodically, 5 seconds as a cycle.
Calculate the average cpu usage within this cycle.
Calculate minumium/maximum/accumulated_average cpu usage as long term inspections.
Monitor multiple processes simuteneously.
Sample usage:
root@localhost:/data/openpilot$ python iqpilot/tools/iqperf/cpu_usage_stat.py pandad,ubloxd
('Add monitored proc:', './pandad')
('Add monitored proc:', 'python locationd/ubloxd.py')
pandad: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96%
ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39%
'''
import psutil
import time
import os
import sys
import numpy as np
import argparse
import re
from collections import defaultdict
from iqpilot.system.manager.process_config import managed_processes
# Do statistics every 5 seconds
PRINT_INTERVAL = 5
SLEEP_INTERVAL = 0.2
monitored_proc_names = [
# android procs
'SurfaceFlinger', 'sensors.qcom'
] + list(managed_processes.keys())
cpu_time_names = ['user', 'system', 'children_user', 'children_system']
def get_arg_parser():
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("proc_names", nargs="?", default='',
help="Process names to be monitored, comma separated")
parser.add_argument("--list_all", action='store_true',
help="Show all running processes' cmdline")
parser.add_argument("--detailed_times", action='store_true',
help="show cpu time details (split by user, system, child user, child system)")
return parser
if __name__ == "__main__":
args = get_arg_parser().parse_args(sys.argv[1:])
if args.list_all:
for p in psutil.process_iter():
print('cmdline', p.cmdline(), 'name', p.name())
sys.exit(0)
if len(args.proc_names) > 0:
monitored_proc_names = args.proc_names.split(',')
monitored_procs = []
stats = {}
for p in psutil.process_iter():
if p == psutil.Process():
continue
matched = any(l for l in p.cmdline() if any(pn for pn in monitored_proc_names if re.match(fr'.*{pn}.*', l, re.M | re.I)))
if matched:
k = ' '.join(p.cmdline())
print('Add monitored proc:', k)
stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None),
'avg': defaultdict(float), 'last_cpu_times': None, 'last_sys_time': None}
stats[k]['last_sys_time'] = time.monotonic()
stats[k]['last_cpu_times'] = p.cpu_times()
monitored_procs.append(p)
i = 0
interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL)
while True:
for p in monitored_procs:
k = ' '.join(p.cmdline())
cur_sys_time = time.monotonic()
cur_cpu_times = p.cpu_times()
cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time'])
stats[k]['last_sys_time'] = cur_sys_time
stats[k]['last_cpu_times'] = cur_cpu_times
cpu_percent = 0
for num, name in enumerate(cpu_time_names):
stats[k]['cpu_samples'][name].append(cpu_times[num])
cpu_percent += cpu_times[num]
stats[k]['cpu_samples']['total'].append(cpu_percent)
time.sleep(SLEEP_INTERVAL)
i += 1
if i % interval_int == 0:
l = []
for k, stat in stats.items():
if len(stat['cpu_samples']) <= 0:
continue
for name, samples in stat['cpu_samples'].items():
samples = np.array(samples)
avg = samples.mean()
c = samples.size
min_cpu = np.amin(samples)
max_cpu = np.amax(samples)
if stat['min'][name] is None or min_cpu < stat['min'][name]:
stat['min'][name] = min_cpu
if stat['max'][name] is None or max_cpu > stat['max'][name]:
stat['max'][name] = max_cpu
stat['avg'][name] = (stat['avg'][name] * (i - c) + avg * c) / (i)
stat['cpu_samples'][name] = []
msg = f"avg: {stat['avg']['total']:.2%}, min: {stat['min']['total']:.2%}, max: {stat['max']['total']:.2%} {os.path.basename(k)}"
if args.detailed_times:
for stat_type in ['avg', 'min', 'max']:
msg += f"\n {stat_type}: {[(name + ':' + str(round(stat[stat_type][name] * 100, 2))) for name in cpu_time_names]}"
l.append((os.path.basename(k), stat['avg']['total'], msg))
l.sort(key=lambda x: -x[1])
for x in l:
print(x[2])
print('avg sum: {:.2%} over {} samples {} seconds\n'.format(
sum(stat['avg']['total'] for k, stat in stats.items()), i, i * SLEEP_INTERVAL
))

223
iqpilot/tools/iqperf/io_stall_repro.py Executable file
View File

@@ -0,0 +1,223 @@
#!/usr/bin/env python3
"""
io_stall_repro.py — bench reproduction + fix-validation rig for the VW PQ EPS
HCA fault (control loop stalling on a /data read under eMMC write saturation).
The real fault chain (proven from rlog b29ee8c5a0a735d1/000000e4--8a8ba97b54):
loggerd buffered writes saturate eMMC -> ext4 jbd2 journal commits ->
controlsd's inline Params.get (util::read_file on /data) blocks ~300-630ms ->
controlsd stops publishing carControl -> card's all_alive guard withholds
HCA_1 -> EPS LH2_Sta_HCA 7->2.
This reproduces the *proximate* cause WITHOUT driving, WITHOUT the EPS, and
WITHOUT touching the real control stack. Two roles:
--writer : emulate loggerd. Buffered (no-fsync) writes to /data at a target
MB/s, with an optional periodic big flush to mimic 60s segment
rotation. This is what saturates the eMMC.
--probe : emulate controlsd's I/O exposure. A 100Hz loop that every
--param-period seconds reads a real param (util::read_file on
/data). It records per-iteration loop gaps and param-read
durations. A 300ms gap here == the stall that drops HCA.
--threaded moves the param read to a background thread (the
proposed fix, mirroring card.py's params_thread) so you can A/B it.
USAGE (run parked, ignition on, on the device):
# 1) baseline: probe alone -> gaps should be tiny
python3 io_stall_repro.py --probe --secs 120
# 2) reproduce: writer in one shell, probe in another
python3 io_stall_repro.py --writer --mbps 25 --rotate 60
python3 io_stall_repro.py --probe --secs 180 # expect big gaps
# 3) validate fix A (params off control thread):
python3 io_stall_repro.py --probe --secs 180 --threaded # gaps should vanish
# 4) validate fix B (smoother writeback) — set before step 2, as root:
# echo 5 > /proc/sys/vm/dirty_background_ratio
# echo 10 > /proc/sys/vm/dirty_ratio
# then re-run step 2 inline probe and compare gap distribution.
Cleanup: writer deletes its scratch files on exit. Read-only wrt openpilot.
"""
import argparse
import os
import sys
import time
import threading
import signal
SCRATCH_DEFAULT = "/data/media/0/io_repro_scratch"
# ----------------------------------------------------------------------------- writer
def run_writer(args):
os.makedirs(args.scratch, exist_ok=True)
chunk = os.urandom(1 << 20) # 1 MiB
bytes_per_s = int(args.mbps * (1 << 20))
print(f"[writer] buffered no-fsync writes to {args.scratch} at ~{args.mbps} MB/s, rotate every {args.rotate}s (big flush). Ctrl-C to stop.", file=sys.stderr)
stop = {"v": False}
signal.signal(signal.SIGINT, lambda *_: stop.update(v=True))
signal.signal(signal.SIGTERM, lambda *_: stop.update(v=True))
files = []
seg = 0
try:
while not stop["v"]:
seg_start = time.monotonic()
path = os.path.join(args.scratch, f"seg_{seg}.bin")
f = open(path, "wb", buffering=1 << 20)
files.append(path)
written = 0
# write at target rate using buffered fwrite, NO fsync (exactly loggerd)
while not stop["v"] and (time.monotonic() - seg_start) < args.rotate:
f.write(chunk)
written += len(chunk)
# pace to target MB/s
target_t = written / bytes_per_s
elapsed = time.monotonic() - seg_start
if target_t > elapsed:
time.sleep(min(0.1, target_t - elapsed))
# "segment rotation": flush+close a big buffered file at once -> writeback burst
f.flush()
f.close()
seg += 1
# keep only a few recent files so we don't fill the disk
while len(files) > 3:
old = files.pop(0)
try:
os.remove(old)
except OSError:
pass
finally:
for p in files:
try:
os.remove(p)
except OSError:
pass
print("[writer] stopped, scratch cleaned.", file=sys.stderr)
# ----------------------------------------------------------------------------- probe
def _get_param(key):
# real /data read, same syscall path as controlsd's get_params_iq
try:
from iqpilot.common.params import Params
return Params().get_bool(key)
except Exception:
# fallback: plain file read of a param file if openpilot import unavailable
p = os.path.join(os.getenv("PARAMS_ROOT", "/data/params"), "d", key)
try:
with open(p, "rb") as fh:
return fh.read()
except OSError:
return None
class ThreadedParam:
"""Mirror card.py params_thread: refresh the param on a bg thread, control
loop reads the cached value (non-blocking)."""
def __init__(self, key, period):
self.key = key
self.period = period
self.val = None
self.stop = False
self.t = threading.Thread(target=self._loop, daemon=True)
self.t.start()
def _loop(self):
while not self.stop:
self.val = _get_param(self.key)
time.sleep(self.period)
def read(self): # O(1), no I/O on the control thread
return self.val
def run_probe(args):
# pin like controlsd (core 4) so we share the same iowait domain if possible
try:
os.sched_setaffinity(0, {args.core})
except (OSError, AttributeError):
pass
interval = 0.01 # 100Hz, like the control loop
gaps = [] # ms, per-iteration loop overrun beyond 10ms
read_ms = [] # ms, time spent in the param read on the control thread
worst = 0.0
threaded = ThreadedParam(args.param_key, args.param_period) if args.threaded else None
print(
f"[probe] 100Hz loop for {args.secs}s, param '{args.param_key}' every {args.param_period}s, threaded={args.threaded}, core={args.core}", file=sys.stderr
)
t_end = time.monotonic() + args.secs
next_t = time.monotonic()
last_param = 0.0
while time.monotonic() < t_end:
loop_start = time.monotonic()
# the I/O exposure: read param on the control thread (inline) every period
if loop_start - last_param >= args.param_period:
r0 = time.monotonic()
if threaded is not None:
_ = threaded.read() # cached, no I/O on this thread (the FIX)
else:
_ = _get_param(args.param_key) # inline /data read (current behavior)
dr = (time.monotonic() - r0) * 1000
read_ms.append(dr)
last_param = loop_start
# measure scheduling/lag: how late did this iteration actually fire?
next_t += interval
lag = (time.monotonic() - next_t) * 1000 # ms behind schedule
if lag > 5:
gaps.append(lag)
worst = max(worst, lag)
sleep = next_t - time.monotonic()
if sleep > 0:
time.sleep(sleep)
else:
next_t = time.monotonic() # don't spiral after a big stall
if threaded:
threaded.stop = True
def pct(xs, p):
return sorted(xs)[int(p / 100 * (len(xs) - 1))] if xs else 0.0
print("\n================ PROBE RESULT ================")
print(f"loop-lag events >5ms : {len(gaps)}")
print(f"loop-lag p50/p99/max : {pct(gaps, 50):.0f} / {pct(gaps, 99):.0f} / {worst:.0f} ms")
print(f"param-read p50/p99/max: {pct(read_ms, 50):.1f} / {pct(read_ms, 99):.1f} / {max(read_ms + [0]):.1f} ms (n={len(read_ms)})")
hca_class = max(gaps + [0])
verdict = "FAULT-CLASS STALL REPRODUCED (>250ms -> would drop HCA)" if hca_class > 250 else "marginal (100-250ms)" if hca_class > 100 else "clean (<100ms)"
print(f"VERDICT: {verdict}")
print("=============================================")
# ----------------------------------------------------------------------------- main
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--writer", action="store_true", help="emulate loggerd eMMC saturation")
ap.add_argument("--probe", action="store_true", help="emulate controlsd I/O exposure")
ap.add_argument("--mbps", type=float, default=25.0, help="writer target MB/s (loggerd ~10-30)")
ap.add_argument("--rotate", type=float, default=60.0, help="writer segment/flush period s")
ap.add_argument("--scratch", default=SCRATCH_DEFAULT)
ap.add_argument("--secs", type=float, default=180.0, help="probe duration s")
ap.add_argument("--param-key", default="IsMetric", help="a real param key to read")
ap.add_argument("--param-period", type=float, default=3.0, help="controlsd reads every 3s")
ap.add_argument("--threaded", action="store_true", help="probe: read param off control thread (the FIX)")
ap.add_argument("--core", type=int, default=4, help="probe cpu affinity (control core)")
args = ap.parse_args()
if args.writer == args.probe:
ap.error("pick exactly one of --writer / --probe (run them in separate shells)")
run_writer(args) if args.writer else run_probe(args)
if __name__ == "__main__":
main()

268
iqpilot/tools/iqperf/io_stall_tracer.py Executable file
View File

@@ -0,0 +1,268 @@
#!/usr/bin/env python3
"""
io_stall_tracer.py — continuous low-overhead per-process disk-I/O tracer.
WHY: the VW PQ EPS HCA faults are caused by a control process (controlsd/card)
blocking ~300-630ms in disk I/O (iowait) on /data, which stops HCA_1 TX. The
stall is far too short to catch with a manual `iostat`/`iotop` run. This sampler
runs for the whole drive at 100ms cadence and records, per process:
- write_bytes (/proc/<pid>/io) -> identifies the WRITER saturating eMMC
- delayacct_blkio (/proc/<pid>/stat) -> per-process cumulative block-I/O wait
- state (/proc/<pid>/stat) -> catches 'D' (uninterruptible disk wait)
plus whole-device /proc/diskstats. After a fault, find the wall-clock time of the
LH2_Sta_HCA->2 event (from the rlog) and look at the rows around it: the process
whose write_bytes delta spikes is the bully; the control process whose blkio
delta jumps / state == 'D' is the victim.
Deploy: copy to the device, run alongside openpilot during a drive:
python3 io_stall_tracer.py --out /data/media/0/io_trace.csv
Overhead: reading /proc for ~40 procs every 100ms is well under 1% of one core,
and it pins itself to CPU 0 (away from the control cores 4/5) at low priority.
Read-only. Writes a single CSV. No openpilot deps.
"""
import argparse
import os
import time
import glob
import sys
CLK_TCK = os.sysconf("SC_CLK_TCK") # usually 100 -> blkio ticks are 10ms each
def read_proc_io(pid):
# wchar/rchar = bytes moved via read()/write() syscalls (catches BUFFERED writers
# like loggerd, which never appear in write_bytes because the kernel flushes their
# page-cache dirty pages asynchronously via kworker). write_bytes = bytes actually
# sent to the block device. Track both.
try:
with open(f"/proc/{pid}/io") as f:
d = {}
for line in f:
k, _, v = line.partition(":")
d[k] = int(v)
return (d.get("wchar", 0), d.get("rchar", 0), d.get("write_bytes", 0), d.get("read_bytes", 0))
except (OSError, ValueError):
return None
def read_proc_stat(pid):
# state is field 3; delayacct_blkio_ticks is field 42 (1-indexed). comm may
# contain spaces/parens, so split on the last ')'.
try:
with open(f"/proc/{pid}/stat") as f:
data = f.read()
rparen = data.rfind(")")
comm = data[data.find("(") + 1 : rparen]
rest = data[rparen + 2 :].split()
state = rest[0] # field 3
blkio_ticks = int(rest[39]) if len(rest) > 39 else 0 # field 42
return comm, state, blkio_ticks
except (OSError, ValueError, IndexError):
return None
def read_diskstats():
# returns {dev: (sectors_written, ms_doing_io)} for whole-disk devices
out = {}
try:
with open("/proc/diskstats") as f:
for line in f:
p = line.split()
if len(p) < 14:
continue
dev = p[2]
# field 10 (idx 9) = sectors written; field 13 (idx 12) = ms doing I/O
out[dev] = (int(p[9]), int(p[12]))
except (OSError, AttributeError):
pass
return out
# These counters tell us WHICH kernel mechanism caused a stall, which decides
# the fix: compact_stall jumping -> memory compaction (texture-pool fix);
# allocstall/pgsteal jumping -> direct reclaim; high nr_dirty/nr_writeback ->
# loggerd writeback bomb (loggerd sync_file_range fix). meminfo Dirty/Writeback
# are absolute kB; vmstat ones are cumulative event counts (we delta them).
VMSTAT_KEYS = (
"compact_stall",
"compact_fail",
"allocstall_normal",
"allocstall_movable",
"pgsteal_direct",
"pgscan_direct",
"pgmajfault",
"nr_dirty",
"nr_writeback",
)
MEMINFO_KEYS = ("MemFree", "MemAvailable", "Dirty", "Writeback")
def read_vmstat():
out = {}
try:
with open("/proc/vmstat") as f:
for line in f:
k, _, v = line.partition(" ")
if k in VMSTAT_KEYS:
out[k] = int(v)
except OSError:
pass
return out
def read_meminfo():
out = {}
try:
with open("/proc/meminfo") as f:
for line in f:
k, _, v = line.partition(":")
if k in MEMINFO_KEYS:
out[k] = int(v.split()[0]) # kB
except (OSError, IndexError):
pass
return out
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--out", default="/data/media/0/io_trace.csv")
ap.add_argument("--hz", type=float, default=10.0, help="sample rate (default 10Hz/100ms)")
ap.add_argument("--disk", default="sda", help="comma-separated disk devices to track (default sda)")
ap.add_argument(
"--names",
default="controlsd,car.c,selfd,ui,loggerd,encoderd,modeld,camerad,locationd,estimatord,navd,mapd",
help="substring match of process comm to record (others summed as 'other')",
)
args = ap.parse_args()
# be a good citizen: low priority, off the control cores
try:
os.nice(10)
os.sched_setaffinity(0, {0})
except OSError:
pass
watch = [n.strip() for n in args.names.split(",") if n.strip()]
disks = [d.strip() for d in args.disk.split(",") if d.strip()]
interval = 1.0 / args.hz
prev_io = {} # pid -> (wchar, rchar, write_bytes, read_bytes)
prev_blkio = {} # pid -> blkio_ticks
prev_disk = read_diskstats()
prev_vm = read_vmstat()
f = open(args.out, "w", buffering=1)
# per-proc rows fill the first block; one SYS row per tick fills the trailing
# mechanism columns (deltas for the vmstat counts, absolute kB for meminfo).
columns = [
"wall",
"mono",
"proc",
"pid",
"state",
"d_wchar_kB",
"d_wbytes_kB",
"d_blkio_ms",
"disk_d_write_kB",
"disk_d_busy_ms",
"compact_stall",
"allocstall",
"pgmajfault",
"dirty_kB",
"writeback_kB",
"memfree_kB",
"memavail_kB",
]
f.write(",".join(columns) + "\n")
print(f"[io_stall_tracer] writing {args.out} at {args.hz}Hz, tracking {watch}", file=sys.stderr)
while True:
t_wall = time.time()
t_mono = time.monotonic()
# whole-disk delta (write kB + busy ms) for the named disks
disk = read_diskstats()
disk_dw = disk_db = 0
for dev in disks:
if dev in disk and dev in prev_disk:
disk_dw += (disk[dev][0] - prev_disk[dev][0]) * 512 / 1024.0 # sectors->kB
disk_db += disk[dev][1] - prev_disk[dev][1]
prev_disk = disk
seen = set()
rows = []
for path in glob.glob("/proc/[0-9]*"):
pid = path.rsplit("/", 1)[1]
st = read_proc_stat(pid)
if st is None:
continue
comm, state, blkio = st
label = next((w for w in watch if w in comm), None)
if label is None:
# still track D-state of anything to catch surprise writers/blockers
if state != "D":
continue
label = comm
io = read_proc_io(pid)
if io is None:
continue
wchar, rchar, wbytes, rbytes = io
pw = prev_io.get(pid, io)
pblk = prev_blkio.get(pid, blkio)
d_wchar = (wchar - pw[0]) / 1024.0 # syscall write volume (catches loggerd)
d_wbytes = (wbytes - pw[2]) / 1024.0 # bytes hitting the block device
d_blk = (blkio - pblk) * (1000.0 / CLK_TCK) # ticks -> ms blocked on block I/O
prev_io[pid] = io
prev_blkio[pid] = blkio
seen.add(pid)
# only emit rows that carry signal (writing, blocked, or in D) to keep file small
if d_wchar > 4 or d_wbytes > 4 or d_blk > 5 or state == "D":
rows.append((label, pid, state, d_wchar, d_wbytes, d_blk))
# drop dead pids from prev maps occasionally
if len(prev_io) > 4000:
prev_io = {p: v for p, v in prev_io.items() if p in seen}
prev_blkio = {p: v for p, v in prev_blkio.items() if p in seen}
for label, pid, state, d_wchar, d_wbytes, d_blk in rows:
f.write(f"{t_wall:.3f},{t_mono:.3f},{label},{pid},{state},{d_wchar:.0f},{d_wbytes:.0f},{d_blk:.0f},{disk_dw:.0f},{disk_db:.0f},,,,,,,\n")
# one SYS row per tick: the kernel-mechanism counters (compaction vs reclaim
# vs writeback). Compare these against the stall's wall-clock to see which
# one spiked.
vm = read_vmstat()
mi = read_meminfo()
d_compact = vm.get("compact_stall", 0) - prev_vm.get("compact_stall", 0)
d_alloc = (vm.get("allocstall_normal", 0) + vm.get("allocstall_movable", 0)) - (prev_vm.get("allocstall_normal", 0) + prev_vm.get("allocstall_movable", 0))
d_majflt = vm.get("pgmajfault", 0) - prev_vm.get("pgmajfault", 0)
prev_vm = vm
values = [
f"{t_wall:.3f}",
f"{t_mono:.3f}",
"SYS",
"0",
"-",
"",
"",
"",
"",
"",
str(d_compact),
str(d_alloc),
str(d_majflt),
str(mi.get("Dirty", 0)),
str(mi.get("Writeback", 0)),
str(mi.get("MemFree", 0)),
str(mi.get("MemAvailable", 0)),
]
f.write(",".join(values) + "\n")
time.sleep(max(0.0, interval - (time.monotonic() - t_mono)))
if __name__ == "__main__":
main()

106
iqpilot/tools/iqperf/live_cpu_and_temp.py Executable file
View File

@@ -0,0 +1,106 @@
#!/usr/bin/env python3
import argparse
import numpy as np
import capnp
from collections import defaultdict
from iqpilot.cereal.messaging import SubMaster
def cputime_total(ct):
return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
def cputime_busy(ct):
return ct.user + ct.nice + ct.system + ct.irq + ct.softirq
def proc_cputime_total(ct):
return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem
def proc_name(proc):
name = proc.name
if len(proc.cmdline):
name = proc.cmdline[0]
if len(proc.exe):
name = proc.exe + " - " + name
return name
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--mem', action='store_true')
parser.add_argument('--cpu', action='store_true')
args = parser.parse_args()
sm = SubMaster(['deviceState', 'procLog'])
last_temp = 0.0
last_mem = 0.0
total_times = [0.]*8
busy_times = [0.]*8
prev_proclog: capnp._DynamicStructReader | None = None
prev_proclog_t: int | None = None
while True:
sm.update()
if sm.updated['deviceState']:
t = sm['deviceState']
last_temp = np.mean(t.cpuTempC)
last_mem = t.memoryUsagePercent
if sm.updated['procLog']:
m = sm['procLog']
cores = [0.]*8
total_times_new = [0.]*8
busy_times_new = [0.]*8
for c in m.cpuTimes:
n = c.cpuNum
total_times_new[n] = cputime_total(c)
busy_times_new[n] = cputime_busy(c)
for n in range(8):
t_busy = busy_times_new[n] - busy_times[n]
t_total = total_times_new[n] - total_times[n]
cores[n] = t_busy / t_total
total_times = total_times_new[:]
busy_times = busy_times_new[:]
print(f"CPU {100.0 * np.mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
if args.cpu and prev_proclog is not None and prev_proclog_t is not None:
procs: dict[str, float] = defaultdict(float)
dt = (sm.logMonoTime['procLog'] - prev_proclog_t) / 1e9
for proc in m.procs:
try:
name = proc_name(proc)
prev_proc = [p for p in prev_proclog.procs if proc.pid == p.pid][0]
cpu_time = proc_cputime_total(proc) - proc_cputime_total(prev_proc)
cpu_usage = cpu_time / dt * 100.
procs[name] += cpu_usage
except IndexError:
pass
print("Top CPU usage:")
for k, v in sorted(procs.items(), key=lambda item: item[1], reverse=True)[:10]:
print(f"{k.rjust(70)} {v:.2f} %")
print()
if args.mem:
mems = {}
for proc in m.procs:
name = proc_name(proc)
mems[name] = float(proc.memRss) / 1e6
print("Top memory usage:")
for k, v in sorted(mems.items(), key=lambda item: item[1], reverse=True)[:10]:
print(f"{k.rjust(70)} {v:.2f} MB")
print()
prev_proclog = m
prev_proclog_t = sm.logMonoTime['procLog']

131
iqpilot/tools/iqperf/max_lat_accel.py Executable file
View File

@@ -0,0 +1,131 @@
#!/usr/bin/env python3
import argparse
import numpy as np
import matplotlib.pyplot as plt
from functools import partial
from tqdm import tqdm
from typing import NamedTuple
from iqpilot.tools.lib.logreader import LogReader
from iqpilot.selfdrive.locationd.models.pose_kf import EARTH_G
RLOG_MIN_LAT_ACTIVE = 50
RLOG_MIN_STEERING_UNPRESSED = 50
RLOG_MIN_REQUESTING_MAX = 25 # sample many times after reaching max torque
QLOG_DECIMATION = 10
class Event(NamedTuple):
lateral_accel: float
speed: float
roll: float
timestamp: float # relative to start of route (s)
def find_events(lr: LogReader, extrapolate: bool = False, qlog: bool = False) -> list[Event]:
min_lat_active = RLOG_MIN_LAT_ACTIVE // QLOG_DECIMATION if qlog else RLOG_MIN_LAT_ACTIVE
min_steering_unpressed = RLOG_MIN_STEERING_UNPRESSED // QLOG_DECIMATION if qlog else RLOG_MIN_STEERING_UNPRESSED
min_requesting_max = RLOG_MIN_REQUESTING_MAX // QLOG_DECIMATION if qlog else RLOG_MIN_REQUESTING_MAX
# if we test with driver torque safety, max torque can be slightly noisy
steer_threshold = 0.7 if extrapolate else 0.95
events = []
# state tracking
steering_unpressed = 0 # frames
requesting_max = 0 # frames
lat_active = 0 # frames
# current state
curvature = 0
v_ego = 0
roll = 0
out_torque = 0
start_ts = 0
for msg in lr:
if msg.which() == 'carControl':
if start_ts == 0:
start_ts = msg.logMonoTime
lat_active = lat_active + 1 if msg.carControl.latActive else 0
elif msg.which() == 'carOutput':
out_torque = msg.carOutput.actuatorsOutput.torque
requesting_max = requesting_max + 1 if abs(out_torque) > steer_threshold else 0
elif msg.which() == 'carState':
steering_unpressed = steering_unpressed + 1 if not msg.carState.steeringPressed else 0
v_ego = msg.carState.vEgo
elif msg.which() == 'controlsState':
curvature = msg.controlsState.curvature
elif msg.which() == 'vehicleParameters':
roll = msg.vehicleParameters.roll
if lat_active > min_lat_active and steering_unpressed > min_steering_unpressed and requesting_max > min_requesting_max:
# TODO: record max lat accel at the end of the event, need to use the past lat accel as overriding can happen before we detect it
requesting_max = 0
factor = 1 / abs(out_torque)
current_lateral_accel = (curvature * v_ego ** 2 * factor) - roll * EARTH_G
events.append(Event(current_lateral_accel, v_ego, roll, round((msg.logMonoTime - start_ts) * 1e-9, 2)))
print(events[-1])
return events
if __name__ == '__main__':
parser = argparse.ArgumentParser(description="Find max lateral acceleration events",
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("route", nargs='+')
parser.add_argument("-e", "--extrapolate", action="store_true", help="Extrapolates max lateral acceleration events linearly. " +
"This option can be far less accurate.")
args = parser.parse_args()
events = []
for route in tqdm(args.route):
try:
lr = LogReader(route, sort_by_time=True)
except Exception:
print(f'Skipping {route}')
continue
qlog = route.endswith('/q')
if qlog:
print('WARNING: Treating route as qlog!')
print('Finding events...')
events += lr.run_across_segments(8, partial(find_events, extrapolate=args.extrapolate, qlog=qlog), disable_tqdm=True)
print()
print(f'Found {len(events)} events')
perc_left_accel = -np.percentile([-ev.lateral_accel for ev in events if ev.lateral_accel < 0] or [0], 90)
perc_right_accel = np.percentile([ev.lateral_accel for ev in events if ev.lateral_accel > 0] or [0], 90)
CP = lr.first('carParams')
plt.ion()
plt.clf()
plt.suptitle(f'{CP.carFingerprint} - Max lateral acceleration events')
plt.title(', '.join(args.route))
plt.scatter([ev.speed for ev in events], [ev.lateral_accel for ev in events], label='max lateral accel events')
plt.plot([0, 35], [3, 3], c='r', label='ISO 11270 - 3 m/s^2')
plt.plot([0, 35], [-3, -3], c='r')
plt.plot([0, 35], [perc_left_accel, perc_left_accel], c='g', linestyle='--', label='90th percentile left lateral accel')
plt.plot([0, 35], [perc_right_accel, perc_right_accel], c='#ff7f0e', linestyle='--', label='90th percentile right lateral accel')
plt.text(0.4, float(perc_left_accel + 0.4), f'{perc_left_accel:.2f} m/s^2', verticalalignment='center', fontsize=12)
plt.text(0.4, float(perc_right_accel - 0.4), f'{perc_right_accel:.2f} m/s^2', verticalalignment='center', fontsize=12)
plt.xlim(0, 35)
plt.ylim(-5, 5)
plt.xlabel('speed (m/s)')
plt.ylabel('lateral acceleration (m/s^2)')
plt.legend()
plt.show(block=True)

58
iqpilot/tools/iqperf/measure_torque_time_to_max.py Executable file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python3
import os
import argparse
import struct
from collections import deque
from statistics import mean
from iqpilot.cereal import log
import iqpilot.cereal.messaging as messaging
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Sniff a communication socket')
parser.add_argument('--addr', default='127.0.0.1')
args = parser.parse_args()
if args.addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.reset_context()
poller = messaging.Poller()
messaging.sub_sock('can', poller, addr=args.addr)
active = 0
start_t = 0
start_v = 0
max_v = 0
max_t = 0
window = deque(maxlen=10)
avg = 0
while 1:
polld = poller.poll(1000)
for sock in polld:
msg = sock.receive()
with log.Event.from_bytes(msg) as log_evt:
evt = log_evt
for item in evt.can:
if item.address == 0xe4 and item.src == 128:
torque_req = struct.unpack('!h', item.dat[0:2])[0]
# print(torque_req)
active = abs(torque_req) > 0
if abs(torque_req) < 100:
if max_v > 5:
print(f'{start_v} -> {max_v} = {round(max_v - start_v, 2)} over {round(max_t - start_t, 2)}s')
start_t = evt.logMonoTime / 1e9
start_v = avg
max_t = 0
max_v = 0
if item.address == 0x1ab and item.src == 0:
motor_torque = ((item.dat[0] & 0x3) << 8) + item.dat[1]
window.append(motor_torque)
avg = mean(window)
#print(f'{evt.logMonoTime}: {avg}')
if active and avg > max_v + 0.5:
max_v = avg
max_t = evt.logMonoTime / 1e9

78
iqpilot/tools/iqperf/qlog_size.py Executable file
View File

@@ -0,0 +1,78 @@
#!/usr/bin/env python3
import argparse
import zstandard as zstd
from collections import defaultdict
import matplotlib.pyplot as plt
from iqpilot.cereal.services import SERVICE_LIST
from iqpilot.common.utils import LOG_COMPRESSION_LEVEL
from iqpilot.tools.lib.logreader import LogReader
from tqdm import tqdm
MIN_SIZE = 0.5 # Percent size of total to show as separate entry
def make_pie(msgs, typ):
msgs_by_type = defaultdict(list)
for m in msgs:
msgs_by_type[m.which()].append(m.as_builder().to_bytes())
total = len(zstd.compress(b"".join([m.as_builder().to_bytes() for m in msgs]), LOG_COMPRESSION_LEVEL))
uncompressed_total = len(b"".join([m.as_builder().to_bytes() for m in msgs]))
length_by_type = {k: len(b"".join(v)) for k, v in msgs_by_type.items()}
# calculate compressed size by calculating diff when removed from the segment
compressed_length_by_type = {}
for k in tqdm(msgs_by_type.keys(), desc="Compressing"):
compressed_length_by_type[k] = total - len(zstd.compress(b"".join([m.as_builder().to_bytes() for m in msgs if m.which() != k]), LOG_COMPRESSION_LEVEL))
sizes = sorted(compressed_length_by_type.items(), key=lambda kv: kv[1])
print("name - comp. size (uncomp. size)")
for (name, sz) in sizes:
print(f"{name:<22} - {sz / 1024:.2f} kB ({length_by_type[name] / 1024:.2f} kB)")
print()
print(f"{typ} - Real total {total / 1024:.2f} kB")
print(f"{typ} - Breakdown total {sum(compressed_length_by_type.values()) / 1024:.2f} kB")
print(f"{typ} - Uncompressed total {uncompressed_total / 1024 / 1024:.2f} MB")
sizes_large = [(k, sz) for (k, sz) in sizes if sz >= total * MIN_SIZE / 100]
sizes_large += [('other', sum(sz for (_, sz) in sizes if sz < total * MIN_SIZE / 100))]
labels, sizes = zip(*sizes_large, strict=True)
plt.figure()
plt.title(f"{typ}")
plt.pie(sizes, labels=labels, autopct='%1.1f%%')
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='View log size breakdown by message type')
parser.add_argument('route', help='route to use')
parser.add_argument('--as-qlog', action='store_true', help='decimate rlog using latest decimation factors')
args = parser.parse_args()
msgs = list(LogReader(args.route))
if args.as_qlog:
new_msgs = []
msg_cnts: dict[str, int] = defaultdict(int)
for msg in msgs:
msg_which = msg.which()
if msg.which() in ("initData", "sentinel"):
new_msgs.append(msg)
continue
if msg_which not in SERVICE_LIST:
continue
decimation = SERVICE_LIST[msg_which].decimation
if decimation is not None and msg_cnts[msg_which] % decimation == 0:
new_msgs.append(msg)
msg_cnts[msg_which] += 1
msgs = new_msgs
make_pie(msgs, 'qlog')
plt.show()

78
iqpilot/tools/iqperf/sendcan_gap_audit.py Executable file
View File

@@ -0,0 +1,78 @@
#!/usr/bin/env python3
"""
sendcan_gap_audit.py — measure the openpilot->car actuator TX cadence from an rlog.
WHY: VW PQ random long/lat disengages are caused by control-loop STALLS that
freeze the `sendcan` publish for >100ms. The car's ECU runs a counter/checksum
watchdog on the actuator messages (ACC_System ADR, HCA_1) and FAULTS when frames
arrive late/missing — it does NOT care about the payload value. So the single
metric that predicts the fault is the inter-frame GAP in sendcan, not anything
about accel/torque. (Reference: route 20e3cd4f0d5f39d1|00000038--0f69286335 had a
103ms gap at ~373s -> engine MO2_Sta_GRA->0 -> main switch off -> disengage. A
separate 60ms gap did NOT disengage: the ECU timeout sits ~60-100ms.)
This is the pass/fail metric for the mlockall / loggerd-writeback fix (5324c46)
and, later, the decoupled in-card heartbeat TX. Run it on a BASELINE route to see
the offending gaps, then on POST-FIX drives to confirm they're gone.
python3 iqpilot/tools/iqperf/sendcan_gap_audit.py <route_or_segment> [--warn-ms 30] [--fault-ms 100]
Exit code 0 if no gap >= --fault-ms, else 1 (so it can gate CI / a smoke test).
Read-only; pulls rlogs via the normal LogReader (konn3kt for IQ.Pilot routes).
"""
import argparse
import sys
from iqpilot.tools.lib.logreader import LogReader
def audit(route: str, warn_ms: float, fault_ms: float) -> int:
lr = LogReader(route, sort_by_time=True)
last = None
gaps = [] # (t_end, dt_ms) for every gap >= warn_ms
n = 0
worst = 0.0
for m in lr:
if m.which() != "sendcan":
continue
t = m.logMonoTime / 1e9
n += 1
if last is not None:
dt = (t - last) * 1000.0
worst = max(worst, dt)
if dt >= warn_ms:
gaps.append((t, dt))
last = t
faults = [(t, dt) for t, dt in gaps if dt >= fault_ms]
print(f"route : {route}")
print(f"sendcan frames : {n}")
print(f"worst gap : {worst:.1f} ms")
print(f"gaps >= {warn_ms:.0f}ms : {len(gaps)}")
print(f"gaps >= {fault_ms:.0f}ms (FAULT-RISK): {len(faults)}")
if gaps:
print("\n t(s) gap(ms) risk")
for t, dt in gaps:
print(f" {t:10.3f} {dt:7.1f} {'<-- FAULT RISK' if dt >= fault_ms else ''}")
if faults:
print(f"\nFAIL: {len(faults)} gap(s) >= {fault_ms:.0f}ms can trip the car's actuator counter watchdog (late/missing frames).")
return 1
print(f"\nPASS: no sendcan gap >= {fault_ms:.0f}ms.")
return 0
def main() -> int:
p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
p.add_argument("route", help="route name, segment, or URL (e.g. dongle|time--hash or .../5:8)")
p.add_argument("--warn-ms", type=float, default=30.0, help="list gaps >= this (default 30)")
p.add_argument("--fault-ms", type=float, default=100.0, help="fail on gaps >= this (default 100)")
args = p.parse_args()
return audit(args.route, args.warn_ms, args.fault_ms)
if __name__ == "__main__":
sys.exit(main())

102
iqpilot/tools/iqpilot/console_pretty.py Normal file
View File

@@ -0,0 +1,102 @@
#!/usr/bin/env python3
# console path for the boot/manager tmux: must never die or block; on any error, forward raw.
import os
import re
import signal
import sys
try:
signal.signal(signal.SIGPIPE, signal.SIG_IGN)
except Exception:
pass
_COLOR = sys.stdout.isatty() and os.environ.get("NO_COLOR") is None
_DROP = re.compile(
r"(\x1b\[\?\d+[a-z])"
r"|^Last login:"
r"|gbm_create_device\(\d+\): Info:"
r"|^pid \d+'s (current|new) affinity list:"
r"|kj/filesystem-disk-unix\.c\+\+:\d+: warning: PWD"
)
_CLOUDLOG = re.compile(r"^([\w./+-]+\.(?:cc|cpp|c|h|py)): (.*)$")
_ALREADY = re.compile(r"^\s*(?:\x1b\[[0-9;]*m)?\s*(CRIT|ERR|WARN|info|dbg)\b")
_ERRISH = re.compile(r"not supported|fail|error|invalid|cannot|timed out|timeout|unable", re.I)
def _sev(msg):
return ("\033[1;38;5;203m", " ERR", "\033[1;38;5;210m") if _ERRISH.search(msg) \
else ("\033[38;5;110m", "info", "")
def _restyle(line):
if _DROP.search(line):
return None
if _ALREADY.match(line):
return line
m = _CLOUDLOG.match(re.sub(r"\x1b\[[0-9;]*m", "", line))
if m and _COLOR:
src, msg = m.group(1), m.group(2)
lc, ln, mc = _sev(msg)
body = f"{mc}{msg}\033[0m" if mc else msg
return f"{lc}{ln}\033[0m \033[2m{src}\033[0m {body}"
return line
def _emit(out, buf):
styled = _restyle(buf)
if styled is not None:
out.write(styled + "\r\n"); out.flush()
def main():
out = sys.stdout
buf = ""
pending_cr = False
read = sys.stdin.buffer.read
while True:
try:
ch = read(1)
except Exception:
break
if not ch:
break
try:
c = ch.decode("utf-8", "replace")
if pending_cr:
pending_cr = False
if c == "\n":
_emit(out, buf); buf = ""; continue
out.write(buf + "\r"); out.flush(); buf = "" # bare \r: progress
if c == "\r":
pending_cr = True
elif c == "\n":
_emit(out, buf); buf = ""
else:
buf += c
except Exception:
try:
out.write(buf); out.flush()
except Exception:
pass
buf = ""; pending_cr = False
if pending_cr:
out.write(buf + "\r"); out.flush()
elif buf:
try:
styled = _restyle(buf)
if styled is not None:
out.write(styled); out.flush()
except Exception:
pass
if __name__ == "__main__":
try:
main()
except Exception:
try:
import shutil
shutil.copyfileobj(sys.stdin.buffer, sys.stdout.buffer)
except Exception:
pass

32
iqpilot/tools/iqpilot/git-pretty.sh Normal file
View File

@@ -0,0 +1,32 @@
# IQ.Pilot pretty git wrappers (gpull/gfetch/gsync/greset/gbv). bash + zsh.
# Source from your shell rc: source .../tools/iqpilot/git-pretty.sh
if [ -n "${BASH_SOURCE:-}" ]; then
_iq_gp_src="${BASH_SOURCE[0]}"
elif [ -n "${ZSH_VERSION:-}" ]; then
_iq_gp_src="${(%):-%x}"
else
_iq_gp_src="$0"
fi
_IQ_GIT_PRETTY="$(cd "$(dirname "$_iq_gp_src")" 2>/dev/null && pwd)/git_pretty.py"
unset _iq_gp_src
if [ -z "${_IQ_GP_PY:-}" ]; then
if command -v python3 >/dev/null 2>&1; then _IQ_GP_PY=python3
elif [ -x /usr/bin/python3 ]; then _IQ_GP_PY=/usr/bin/python3
else _IQ_GP_PY=python; fi
fi
_iq_git_pretty() {
command git "$@" 2>&1 | "$_IQ_GP_PY" "$_IQ_GIT_PRETTY"
if [ -n "${ZSH_VERSION:-}" ]; then
return ${pipestatus[1]}
else
return ${PIPESTATUS[0]}
fi
}
gpull() { _iq_git_pretty -c color.ui=always pull --progress "$@"; }
gfetch() { _iq_git_pretty -c color.ui=always fetch --progress "$@"; }
gsync() { _iq_git_pretty -c color.ui=always submodule update --init --recursive --progress "$@"; }
greset() { _iq_git_pretty -c color.ui=always reset "$@"; }
gbv() { _iq_git_pretty -c color.branch=always branch -v "$@"; }

123
iqpilot/tools/iqpilot/git_pretty.py Normal file
View File

@@ -0,0 +1,123 @@
#!/usr/bin/env python3
import os
import re
import sys
_GRAD = {
"PULL": ((95, 240, 150), (40, 200, 120)),
"FETCH": ((95, 205, 255), (130, 110, 250)),
"SUBMOD": ((200, 160, 255), (150, 110, 250)),
"RESET": ((255, 190, 90), (230, 80, 70)),
"BRANCH": ((95, 215, 255), (70, 130, 245)),
}
_TARGET_RGB = (150, 152, 178)
_GREEN = (120, 210, 130)
_DIM = "\033[2;38;5;246m"
_RST = "\033[0m"
def _mode():
if not sys.stdout.isatty() or os.environ.get("NO_COLOR"):
return None
return "true" if os.environ.get("COLORTERM", "").lower() in ("truecolor", "24bit") else "256"
def _fg(rgb, mode):
r, g, b = rgb
if mode == "true":
return f"\033[38;2;{r};{g};{b}m"
if abs(r - g) < 12 and abs(g - b) < 12 and abs(r - b) < 12:
idx = 232 + min(23, round((r + g + b) / 3 / 255 * 23))
else:
idx = 16 + 36 * round(r / 255 * 5) + 6 * round(g / 255 * 5) + round(b / 255 * 5)
return f"\033[38;5;{idx}m"
def _grad(word, label, mode):
start, end = _GRAD.get(label, _GRAD["FETCH"])
n = max(1, len(word) - 1)
out = [f"\033[1m{_fg(tuple(int(s + (e - s) * i / n) for s, e in zip(start, end)), mode)}{ch}"
for i, ch in enumerate(word)]
return "".join(out) + _RST
def _label(label, body, mode):
pad = " " * max(0, 8 - len(label))
return f"{pad}{_grad(label, label, mode)} {_fg(_TARGET_RGB, mode)}{body}{_RST}"
def _restyle(line, mode):
s = line.rstrip("\n")
raw = re.sub(r"\033\[[0-9;]*m", "", s) # match against de-colored text
if raw == "Already up to date.":
return f"{_fg(_GREEN, mode)}✓ already up to date{_RST}"
m = re.match(r"Updating ([0-9a-f]+\.\.[0-9a-f]+)$", raw)
if m:
return _label("PULL", m.group(1), mode)
if raw == "Fast-forward":
return f"{_DIM}fast-forward{_RST}"
m = re.match(r"HEAD is now at ([0-9a-f]+) (.*)$", raw)
if m:
return _label("RESET", f"{m.group(1)} {m.group(2)}", mode)
m = re.match(r"Submodule path '(.+)': checked out '([0-9a-f]+)'$", raw)
if m:
return _label("SUBMOD", f"{m.group(1)} @ {m.group(2)[:9]}", mode)
m = re.match(r"Submodule '(.+)' \((.+)\) registered for path '(.+)'$", raw)
if m:
return _label("SUBMOD", f"{m.group(3)} (registered)", mode)
m = re.match(r"From (.+)$", raw)
if m:
return _label("FETCH", m.group(1), mode)
m = re.match(r"\s*\*?\s*\[new (?:branch|tag)\]\s+(\S+)\s+->\s+(\S+)$", raw)
if m:
return _label("FETCH", f"new {m.group(1)} → {m.group(2)}", mode)
m = re.match(r"\s*\*\s+(?:branch|tag)\s+(\S+)\s+->\s+(\S+)$", raw)
if m:
return _label("FETCH", f"{m.group(1)} → {m.group(2)}", mode)
m = re.match(r"\s*([0-9a-f]+\.\.[0-9a-f]+)\s+(\S+)\s+->\s+(\S+)$", raw)
if m:
return _label("FETCH", f"{m.group(1)} {m.group(2)} → {m.group(3)}", mode)
m = re.match(r"([* ]) +(\S+) +([0-9a-f]{7,})( .*)?$", raw)
if m:
cur, name, sha, msg = m.groups()
star = f"{_fg(_GREEN, mode)}●{_RST} " if cur == "*" else " "
return f"{star}{_grad(name, 'BRANCH', mode)} {_fg(_TARGET_RGB, mode)}{sha}{(msg or '')}{_RST}"
return s
def main():
mode = _mode()
out = sys.stdout
if mode is None: # not a tty / NO_COLOR: passthrough
for chunk in iter(lambda: sys.stdin.buffer.read(4096), b""):
out.buffer.write(chunk)
out.buffer.flush()
return
buf = ""
stream = sys.stdin
while True:
ch = stream.read(1)
if not ch:
break
if ch == "\r": # progress fragment: emit live, untouched
out.write(buf + "\r")
out.flush()
buf = ""
elif ch == "\n":
out.write(_restyle(buf, mode) + "\n")
out.flush()
buf = ""
else:
buf += ch
if buf:
out.write(_restyle(buf, mode))
out.flush()
if __name__ == "__main__":
try:
main()
except (BrokenPipeError, KeyboardInterrupt):
pass

775
iqpilot/tools/iqpilot/mici_preview.py Executable file
View File

@@ -0,0 +1,775 @@
#!/usr/bin/env python3
"""
╔══════════════════════════════════════════════════════════════════════════╗
║ IQ.Pilot MICI UI Preview Tool ║
║ ────────────────────────────────────────────────────────────────────── ║
║ Renders any MICI layout/widget at 536×240 on your Mac desktop so you ║
║ can visually inspect and iterate without a physical comma 4. ║
║ ║
║ MODES ║
║ screenshot — render N frames, save PNG(s), open in Preview ║
║ video — render N seconds to MP4 via ffmpeg, open in QuickTime ║
║ live — interactive window, hot-reload on file-save ║
║ ║
║ USAGE ║
║ python tools/iqpilot/mici_preview.py [OPTIONS] ║
║ ║
║ OPTIONS ║
║ --panel PANEL Panel to render: steering, visuals, display, ║
║ software, cruise, trips, osm, models, toggles, ║
║ device, developer, home, settings (default: steering)║
║ --mode MODE screenshot | video | live (default: screenshot) ║
║ --frames N Frames to settle before screenshot (default: 90) ║
║ --shots N Number of screenshots to take (default: 1) ║
║ --duration S Video duration in seconds (default: 4) ║
║ --fps N Render FPS (default: 60) ║
║ --scale F Window scale multiplier (2.0 = 1072×480 window) ║
║ (default: 2.5) ║
║ --out PATH Output file/dir (default: /tmp/mici_preview/) ║
║ --open Open output file(s) after capture (default: True) ║
║ --mock Use mock UI state (no real params needed) ║
║ ║
║ EXAMPLES ║
║ # Screenshot the steering panel (scaled 2.5×, opens in Preview) ║
║ python tools/iqpilot/mici_preview.py --panel steering ║
║ ║
║ # 4-second video of the neon glow animation ║
║ python tools/iqpilot/mici_preview.py --panel steering --mode video ║
║ ║
║ # Live interactive window with hot-reload ║
║ python tools/iqpilot/mici_preview.py --panel visuals --mode live ║
║ ║
║ # Multiple panels in one go (screenshots) ║
║ python tools/iqpilot/mici_preview.py --panel all ║
╚══════════════════════════════════════════════════════════════════════════╝
"""
import argparse
import importlib
import os
import subprocess
import sys
import time
import queue
import threading
from pathlib import Path
# ── Project root on sys.path ──────────────────────────────────────────────
ROOT = Path(__file__).resolve().parents[3]
sys.path.insert(0, str(ROOT))
# Force MICI mode (no BIG UI)
os.environ.setdefault("BIG", "0")
os.environ.setdefault("IQPILOT_UI", "1")
# Use offscreen mode for screenshot/video (no FPS cap → fast capture)
# Live mode overrides this below.
import pyray as rl # noqa: E402 — must come after env setup
# ── MICI canvas dimensions ─────────────────────────────────────────────────
MICI_W = 536
MICI_H = 240
# ── Output directory ───────────────────────────────────────────────────────
DEFAULT_OUT = Path("/tmp/mici_preview")
# ── Color palette for the preview chrome ──────────────────────────────────
CHROME_BG = rl.Color(18, 18, 22, 255) # dark bg behind the MICI canvas
LABEL_COLOR = rl.Color(160, 160, 160, 200) # panel label text
# ── All available panels ───────────────────────────────────────────────────
ALL_PANELS = [
"steering", "visuals", "display", "software", "cruise",
"trips", "osm", "models", "toggles", "device", "developer", "home",
]
# ─────────────────────────────────────────────────────────────────────────────
# Mock UI state so we can run without a live comma process
# ─────────────────────────────────────────────────────────────────────────────
def _patch_mock_state():
"""Replace real ui_state and Params with lightweight mocks."""
import types
# ── Shared in-memory store ────────────────────────────────────────────────
_store: dict = {}
class MockParams:
"""
Drop-in Params mock that also supports `Params | None` type expressions
at module import time by implementing __or__ / __ror__ on the class itself.
"""
# Allow `Params | None` as a runtime type-union expression
def __class_getitem__(cls, item):
return cls
def __or__(cls, other):
import types as _t
return _t.UnionType if hasattr(_t, 'UnionType') else object
__ror__ = __or__
def __init__(self, _=None, **__):
pass # ignore any constructor args (real Params accepts path kwarg)
def get(self, key, default=None, return_default=False):
val = _store.get(key, default)
# Real Params.get() returns str or None; convert bools → str
if isinstance(val, bool):
return str(int(val))
return val
def get_bool(self, key, default=False):
val = _store.get(key, default)
if isinstance(val, str):
return val not in ('', '0', 'false', 'False', 'None')
return bool(val)
def put(self, key, val):
_store[key] = val
def put_bool(self, key, val):
_store[key] = bool(val)
def put_nonblocking(self, key, val):
_store[key] = val
# Seed some sensible defaults so widgets render realistically
mp = MockParams()
# ── Steering ────────────────────────────────────────────────────────────
mp.put("AolEnabled", False)
mp.put("AolSteeringMode", 0) # 0=remain active, 1=pause, 2=disengage
mp.put("AolMainCruiseAllowed", True)
mp.put("AolUnifiedEngagementMode", False)
mp.put("AolPauseOnSteeringOverride", False)
mp.put("NeuralNetworkFeedForward", False)
mp.put("IQLaneChangeTimer", 0) # nudge
mp.put("IQLaneChangeBsmDelay", False)
mp.put("IQEdgeGuard", False)
# ── Visuals (correct param keys matching visuals.py) ─────────────────────
mp.put("IQBlindSpotAlerts", True)
mp.put("IQSteerEffortArc", True)
mp.put("IQRoadNameOverlay", True)
mp.put("IQBlinkerIndicators", True)
mp.put("IQAccelMeter", False)
mp.put("IQLeadReadouts", 0) # 0=off
mp.put("IQDevUIInfo", 0) # 0=off
mp.put("AlphaLongitudinalEnabled", False) # real param; gates ChevronInfo
# ── Display ───────────────────────────────────────────────────────────────
mp.put("OnroadScreenOffBrightness", 0) # 0=auto
mp.put("OnroadScreenOffTimer", 60) # 1m
mp.put("InteractivityTimeout", 0) # default
# ── Software ──────────────────────────────────────────────────────────────
mp.put("DisableUpdates", False)
mp.put("GitBranch", "master-mici")
mp.put("Version", "IQ.Pilot 0.9.5-mici")
# ── Models ────────────────────────────────────────────────────────────────
mp.put("IQLiveSteerDelay", False)
mp.put("IQLaneTurnDesire", False)
mp.put("IQLaneTurnValue", "19.0")
# ── Cruise ────────────────────────────────────────────────────────────────
mp.put("ExperimentalMode", False)
mp.put("IQDynamicMode", False)
mp.put("LongitudinalPersonality", 1)
mp.put("IQSpeedAssistMode", 0) # 0=off
# ── Misc / system ─────────────────────────────────────────────────────────
mp.put("IsMetric", False)
mp.put("UIAccentColor", "#00FFF5") # default neon cyan
# Inject mock into common.params
# IMPORTANT: inject the CLASS (not an instance) so that `Params | None`
# type-union expressions in downstream modules work at import time.
mock_module = types.ModuleType("iqpilot.common.params")
mock_module.Params = MockParams
sys.modules["iqpilot.common.params"] = mock_module
# ── Mock ui_state (both the iqpilot layer and the top-level selfdrive layer) ─
class MockCP:
enableBsm = True
openpilotLongitudinalControl = True
alphaLongitudinalAvailable = False
# Minimal mock for ui_state.sm — returns empty/default objects for any key
class _MockBundle:
internalName = "mock-model"
displayName = "Mock Model"
index = 0
status = None # not downloading
models = []
overrides = []
class _MockModelManager:
availableBundles = []
activeBundle = _MockBundle()
selectedBundle = None # None = not downloading
from iqpilot.cereal import log as _log
class _MockDeviceState:
networkType = _log.DeviceState.NetworkType.wifi
networkStrength = type("NS", (), {"raw": 3})()
egpuDockPresent = True
started = False
freeSpacePercent = 50.0
memoryUsagePercent = 40
class _MockSM:
"""Minimal SubMaster-like dict that returns sensible defaults."""
_data = {
"iqModelManager": _MockModelManager(),
"deviceState": _MockDeviceState(),
}
def __getitem__(self, key):
return self._data.get(key, type("Empty", (), {"enabled": False})())
@property
def updated(self):
return type("U", (), {"__getitem__": lambda s, k: False})()
@property
def alive(self):
return type("A", (), {"__getitem__": lambda s, k: True})()
@property
def valid(self):
return type("V", (), {"__getitem__": lambda s, k: True})()
@property
def frame(self): return 0
def __contains__(self, key): return True
class MockUIState:
CP = MockCP()
params = mp
sm = _MockSM()
started = False
ignition = False
is_metric = False
has_longitudinal_control = True
always_on_dm = False
recording_audio = False
personality = 1 # standard
custom_interactive_timeout = 0
light_sensor = -1.0
is_release = False
# IQ-specific extras
aol_enabled = False
aol_state = 0
def is_offroad(self): return True
def is_onroad(self): return False
def add_offroad_transition_callback(self, cb): pass
def add_engaged_transition_callback(self, cb): pass
def update_params(self): pass
@property
def engaged(self): return False
_mock_ui_state = MockUIState()
# iqpilot.selfdrive.ui.ui_state (base openpilot layer — hosts the IQ UI state classes/enums)
from enum import Enum, IntEnum
class _UIStatus(Enum):
DISENGAGED = "disengaged"
ENGAGED = "engaged"
OVERRIDE = "override"
LAT_ONLY = "lat_only"
LONG_ONLY = "long_only"
class _OnroadTimerStatus(Enum):
NONE = 0
PAUSE = 1
RESUME = 2
class _OnroadBrightness(IntEnum):
AUTO = 0
AUTO_DARK = 1
mock_base_ui_mod = types.ModuleType("iqpilot.selfdrive.ui.ui_state")
mock_base_ui_mod.ui_state = _mock_ui_state
mock_base_ui_mod.UIStatus = _UIStatus
mock_base_ui_mod.OnroadTimerStatus = _OnroadTimerStatus
mock_base_ui_mod.OnroadBrightness = _OnroadBrightness
mock_base_ui_mod.device = type("MockDevice", (), {"awake": True})()
sys.modules["iqpilot.selfdrive.ui.ui_state"] = mock_base_ui_mod
return mp
# ─────────────────────────────────────────────────────────────────────────────
# Panel loader
# ─────────────────────────────────────────────────────────────────────────────
def load_panel(name: str):
"""
Instantiate a MICI panel/layout by name.
Returns a Widget instance with set_rect() already called.
"""
rect = rl.Rectangle(0, 0, MICI_W, MICI_H)
layouts = {
"steering": ("iqpilot.ui.mici.layouts.steering", "SteeringLayoutMici"),
"visuals": ("iqpilot.ui.mici.layouts.visuals", "VisualsLayoutMici"),
"display": ("iqpilot.ui.mici.layouts.display", "DisplayLayoutMici"),
"software": ("iqpilot.ui.mici.layouts.software", "SoftwareLayoutMici"),
"cruise": ("iqpilot.ui.mici.layouts.cruise", "CruiseLayoutMici"),
"trips": ("iqpilot.ui.mici.layouts.trips", "TripsLayoutMici"),
"osm": ("iqpilot.ui.mici.layouts.osm", "OSMLayoutMici"),
"models": ("iqpilot.ui.mici.layouts.models", "ModelsLayoutMici"),
"toggles": ("iqpilot.selfdrive.ui.mici.layouts.settings.toggles", "TogglesLayoutMici"),
"device": ("iqpilot.selfdrive.ui.mici.layouts.settings.device", "DeviceLayoutMici"),
"developer":("iqpilot.selfdrive.ui.mici.layouts.settings.developer","DeveloperLayoutMici"),
"home": ("iqpilot.selfdrive.ui.mici.layouts.home", "MiciHomeLayout"),
"settings": ("iqpilot.ui.mici.layouts.settings", "IQMiciSettingsLayout"),
}
if name not in layouts:
raise ValueError(f"Unknown panel '{name}'. Choose from: {', '.join(layouts)}")
# All IQ.Pilot MICI layout panels accept an optional back_callback
NEEDS_BACK_CB = {
"steering", "visuals", "display", "software", "cruise",
"trips", "osm", "models", "toggles", "device", "developer",
}
mod_path, cls_name = layouts[name]
mod = importlib.import_module(mod_path)
cls = getattr(mod, cls_name)
if name in NEEDS_BACK_CB:
widget = cls(back_callback=lambda: None)
else:
widget = cls()
widget.set_rect(rect)
widget.show_event()
eg = os.environ.get("IQ_EGPU_STATE")
mc = os.environ.get("IQ_MAC_STATE")
if (eg or mc) and hasattr(widget, "_egpu_state"):
widget._egpu_state = eg or None
widget._mac_state = mc or None
widget._egpu_progress = float(os.environ.get("IQ_EGPU_PROGRESS", "0") or 0)
widget._mac_progress = float(os.environ.get("IQ_MAC_PROGRESS", "0") or 0)
widget._update_dock_status = lambda: None
return widget
# ─────────────────────────────────────────────────────────────────────────────
# Rendering helpers
# ─────────────────────────────────────────────────────────────────────────────
def _draw_chrome(panel_name: str, scale: float, canvas_x: int, canvas_y: int):
"""Draw the preview window chrome: background, label, dimension hint."""
# Nothing to draw outside the canvas — the window IS the canvas (+ padding)
pass
def _render_frame(widget, render_tex: rl.RenderTexture, canvas_x: int, canvas_y: int, scale: float, panel_name: str):
"""
Render one frame:
1. Draw the MICI widget into render_tex (536×240 offscreen)
2. Blit the texture into the window at the correct position + scale
3. Draw chrome overlays (panel label, grid, etc.)
"""
# Draw into the MICI-sized render texture
rl.begin_texture_mode(render_tex)
rl.clear_background(rl.Color(0, 0, 0, 255))
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
rl.end_texture_mode()
# Blit to screen (flip Y because OpenGL textures are upside-down)
src = rl.Rectangle(0, 0, MICI_W, -MICI_H) # negative H = flip
dst = rl.Rectangle(canvas_x, canvas_y, MICI_W * scale, MICI_H * scale)
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
# Panel name label bottom-left
rl.draw_text(panel_name.upper(), canvas_x + 6, canvas_y + int(MICI_H * scale) + 6,
14, rl.Color(120, 120, 120, 180))
# Dimension hint bottom-right
hint = f"{MICI_W}×{MICI_H} (×{scale:.1f})"
hint_w = rl.measure_text(hint, 12)
win_w = rl.get_screen_width()
rl.draw_text(hint, win_w - hint_w - 8, canvas_y + int(MICI_H * scale) + 6,
12, rl.Color(80, 80, 80, 160))
# ─────────────────────────────────────────────────────────────────────────────
# Screenshot mode
# ─────────────────────────────────────────────────────────────────────────────
def run_screenshot(panel_name: str, args, out_dir: Path) -> list[Path]:
"""
Render `args.frames` frames (so animations settle), then take `args.shots`
screenshots 0.5s apart. Returns list of saved PNG paths.
Uses gui_app.init_window() so fonts are loaded correctly, and the SCALE
env var (set to args.scale in main()) controls window size.
"""
from iqpilot.system.ui.lib.application import gui_app
gui_app.init_window(f"MICI Preview — {panel_name}", fps=args.fps)
# Offscreen MICI-resolution render texture
render_tex = rl.load_render_texture(MICI_W, MICI_H)
# Load widget AFTER window/fonts are initialized
widget = load_panel(panel_name)
saved: list[Path] = []
frame = 0
shots_taken = 0
next_shot_frame = args.frames # first shot after settle
# Display at native scaled window coords (0,0 → screen size)
win_w = rl.get_screen_width()
win_h = rl.get_screen_height()
while not rl.window_should_close() and shots_taken < args.shots:
rl.begin_drawing()
rl.clear_background(CHROME_BG)
# Render widget into the MICI-sized texture, then blit full-window
rl.begin_texture_mode(render_tex)
rl.clear_background(rl.BLACK)
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
rl.end_texture_mode()
# Blit the render texture to fill the whole window (Y-flipped)
src = rl.Rectangle(0, 0, MICI_W, -MICI_H)
dst = rl.Rectangle(0, 0, win_w, win_h)
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
# Settle progress bar
if frame < args.frames:
pct = frame / args.frames
rl.draw_rectangle(0, win_h - 3, int(win_w * pct), 3, rl.Color(0, 255, 245, 140))
rl.draw_text(f"settling {frame}/{args.frames}", 6, 6, 11, rl.Color(100, 100, 100, 160))
rl.end_drawing()
frame += 1
if frame == next_shot_frame:
# Read from render texture (native MICI res, no padding to crop)
img = rl.load_image_from_texture(render_tex.texture)
rl.image_flip_vertical(img)
# Save at 2× native for clarity on Retina displays
rl.image_resize(img, MICI_W * 2, MICI_H * 2)
out_dir.mkdir(parents=True, exist_ok=True)
ts = int(time.time() * 1000)
path = out_dir / f"mici_{panel_name}_{shots_taken + 1:02d}_{ts}.png"
rl.export_image(img, str(path))
rl.unload_image(img)
saved.append(path)
print(f" 📸 saved: {path}")
shots_taken += 1
next_shot_frame += int(args.fps * 0.5) # 0.5s between shots
rl.unload_render_texture(render_tex)
rl.close_window()
return saved
# ─────────────────────────────────────────────────────────────────────────────
# Video mode
# ─────────────────────────────────────────────────────────────────────────────
def run_video(panel_name: str, args, out_dir: Path) -> Path:
"""
Render args.duration seconds at args.fps, pipe raw RGBA frames to ffmpeg → MP4.
Returns the output MP4 path.
"""
from iqpilot.system.ui.lib.application import gui_app
out_dir.mkdir(parents=True, exist_ok=True)
ts = int(time.time())
mp4_path = out_dir / f"mici_{panel_name}_{ts}.mp4"
# Launch ffmpeg to accept raw RGBA frames at MICI native resolution
ffmpeg = subprocess.Popen([
"ffmpeg", "-v", "warning", "-nostats",
"-f", "rawvideo", "-pix_fmt", "rgba",
"-s", f"{MICI_W}x{MICI_H}",
"-r", str(args.fps),
"-i", "pipe:0",
"-vf", "vflip,format=yuv420p",
"-c:v", "libx264", "-preset", "fast", "-crf", "18",
"-y", str(mp4_path),
], stdin=subprocess.PIPE)
# Write-queue so rendering doesn't block on ffmpeg
frame_queue: queue.Queue = queue.Queue(maxsize=args.fps * 2)
stop_event = threading.Event()
def _writer():
while not stop_event.is_set() or not frame_queue.empty():
try:
data = frame_queue.get(timeout=0.1)
ffmpeg.stdin.write(data)
except queue.Empty:
pass
ffmpeg.stdin.close()
writer_thread = threading.Thread(target=_writer, daemon=True)
writer_thread.start()
gui_app.init_window(f"MICI Preview (recording) — {panel_name}", fps=args.fps)
render_tex = rl.load_render_texture(MICI_W, MICI_H)
widget = load_panel(panel_name)
win_w = rl.get_screen_width()
win_h = rl.get_screen_height()
total_frames = int(args.fps * args.duration)
frame = 0
settle = min(args.frames, total_frames // 4)
print(f" 🎬 recording {args.duration}s at {args.fps}fps → {mp4_path.name}")
while not rl.window_should_close() and frame < total_frames:
rl.begin_drawing()
rl.clear_background(CHROME_BG)
rl.begin_texture_mode(render_tex)
rl.clear_background(rl.BLACK)
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
rl.end_texture_mode()
src = rl.Rectangle(0, 0, MICI_W, -MICI_H)
dst = rl.Rectangle(0, 0, win_w, win_h)
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
# REC progress bar
pct = frame / total_frames
rl.draw_rectangle(0, win_h - 3, int(win_w * pct), 3, rl.Color(255, 80, 80, 180))
rl.draw_text(f"REC {frame}/{total_frames}", 6, 6, 11, rl.Color(255, 80, 80, 200))
rl.end_drawing()
# Queue raw RGBA from MICI-res texture for ffmpeg
if frame >= settle:
import ctypes
img = rl.load_image_from_texture(render_tex.texture)
colors = rl.load_image_colors(img)
raw = bytes(ctypes.string_at(colors, MICI_W * MICI_H * 4))
rl.unload_image_colors(colors)
rl.unload_image(img)
try:
frame_queue.put(raw, timeout=1.0)
except queue.Full:
pass
frame += 1
rl.unload_render_texture(render_tex)
rl.close_window()
stop_event.set()
writer_thread.join(timeout=10)
ffmpeg.wait(timeout=15)
print(f" ✅ video saved: {mp4_path}")
return mp4_path
# ─────────────────────────────────────────────────────────────────────────────
# Live / hot-reload mode
# ─────────────────────────────────────────────────────────────────────────────
def run_live(panel_name: str, args, mock_params):
"""
Interactive window with hot-reload.
Watches the source file of the selected panel; re-imports it on save.
Press S to take a screenshot, R to force reload, Q/Esc to quit.
"""
from iqpilot.system.ui.lib.application import gui_app
gui_app.init_window(f"MICI Live — {panel_name} [S=shot R=reload Q=quit]", fps=args.fps)
render_tex = rl.load_render_texture(MICI_W, MICI_H)
win_w = rl.get_screen_width()
win_h = rl.get_screen_height()
widget = load_panel(panel_name)
last_mtime: dict[str, float] = {}
def _watch_paths() -> list[Path]:
"""Files to watch for changes (panel module + shared theme/button)."""
mods = [
ROOT / "iqpilot/selfdrive/ui/iqpilot/mici/layouts" / f"{panel_name}.py",
ROOT / "iqpilot/selfdrive/ui/iqpilot/theme.py",
ROOT / "iqpilot/selfdrive/ui/mici/widgets/button.py",
]
return [p for p in mods if p.exists()]
def _needs_reload() -> bool:
for p in _watch_paths():
mtime = p.stat().st_mtime
if last_mtime.get(str(p), 0) != mtime:
last_mtime[str(p)] = mtime
return True
return False
def _reload():
nonlocal widget
print(" 🔄 reloading...")
# Invalidate cached modules so importlib picks up changes
prefix = "iqpilot.ui.mici.layouts"
for key in list(sys.modules.keys()):
if key.startswith(prefix) or key == "iqpilot.ui.theme":
del sys.modules[key]
try:
widget = load_panel(panel_name)
print(" ✅ reloaded OK")
except Exception as e:
print(f" ❌ reload error: {e}")
# Seed mtimes
for p in _watch_paths():
last_mtime[str(p)] = p.stat().st_mtime
out_dir = Path(args.out)
shot_count = 0
while not rl.window_should_close():
# Check for file changes
if _needs_reload():
_reload()
# Key bindings
if rl.is_key_pressed(rl.KeyboardKey.KEY_Q) or rl.is_key_pressed(rl.KeyboardKey.KEY_ESCAPE):
break
if rl.is_key_pressed(rl.KeyboardKey.KEY_R):
_reload()
if rl.is_key_pressed(rl.KeyboardKey.KEY_S):
# Take screenshot from render texture (native MICI res)
img = rl.load_image_from_texture(render_tex.texture)
rl.image_flip_vertical(img)
rl.image_resize(img, MICI_W * 2, MICI_H * 2)
out_dir.mkdir(parents=True, exist_ok=True)
ts = int(time.time() * 1000)
path = out_dir / f"mici_{panel_name}_live_{shot_count:03d}_{ts}.png"
rl.export_image(img, str(path))
rl.unload_image(img)
shot_count += 1
print(f" 📸 screenshot saved: {path}")
if args.open:
subprocess.Popen(["open", str(path)])
rl.begin_drawing()
rl.clear_background(CHROME_BG)
# Render widget into MICI-sized texture, blit to full window
rl.begin_texture_mode(render_tex)
rl.clear_background(rl.BLACK)
widget.render(rl.Rectangle(0, 0, MICI_W, MICI_H))
rl.end_texture_mode()
src = rl.Rectangle(0, 0, MICI_W, -MICI_H)
dst = rl.Rectangle(0, 0, win_w, win_h)
rl.draw_texture_pro(render_tex.texture, src, dst, rl.Vector2(0, 0), 0, rl.WHITE)
# Status hint overlay
watch_files = _watch_paths()
hint = f"watching {len(watch_files)} file(s) | S=shot R=reload Q=quit"
rl.draw_text(hint, 6, win_h - 18, 11, rl.Color(80, 80, 80, 160))
rl.end_drawing()
rl.unload_render_texture(render_tex)
rl.close_window()
# ─────────────────────────────────────────────────────────────────────────────
# Entry point
# ─────────────────────────────────────────────────────────────────────────────
def parse_args():
p = argparse.ArgumentParser(
description="IQ.Pilot MICI UI Preview Tool",
formatter_class=argparse.RawDescriptionHelpFormatter,
epilog=__doc__,
)
p.add_argument("--panel", default="steering",
help="Panel name or 'all'")
p.add_argument("--mode", default="screenshot",
choices=["screenshot", "video", "live"],
help="Capture mode")
p.add_argument("--frames", type=int, default=90,
help="Settle frames before screenshot")
p.add_argument("--shots", type=int, default=1,
help="Number of screenshots")
p.add_argument("--duration", type=float, default=4.0,
help="Video duration in seconds")
p.add_argument("--fps", type=int, default=60,
help="Render FPS")
p.add_argument("--scale", type=float, default=2.5,
help="Window scale multiplier (1.0 = native 536×240)")
p.add_argument("--out", default=str(DEFAULT_OUT),
help="Output directory")
p.add_argument("--open", action="store_true", default=True,
help="Open output after capture (macOS)")
p.add_argument("--no-open", dest="open", action="store_false")
p.add_argument("--mock", action="store_true", default=True,
help="Use mock UI state (no live process needed)")
p.add_argument("--no-mock", dest="mock", action="store_false")
p.add_argument("--accent", default=None,
help="Override neon accent color, e.g. '#FF6B00'")
return p.parse_args()
def main():
args = parse_args()
out_dir = Path(args.out)
out_dir.mkdir(parents=True, exist_ok=True)
# Set SCALE env var BEFORE gui_app module is imported (it reads it at import time)
os.environ["SCALE"] = str(args.scale)
# Apply mock state early (before any widget imports)
mock_params = None
if args.mock:
print(" 🎭 using mock UI state (--no-mock to use live params)")
mock_params = _patch_mock_state()
# Override accent color if requested
if args.accent:
if mock_params:
mock_params.put("UIAccentColor", args.accent)
print(f" 🎨 accent color: {args.accent}")
panels = ALL_PANELS if args.panel == "all" else [args.panel]
mode = args.mode
if mode == "live" and len(panels) > 1:
print(" ⚠️ live mode only supports a single panel. Using first panel.")
panels = panels[:1]
all_outputs: list[Path] = []
for panel_name in panels:
print(f"\n ▶ {mode.upper()} — panel: {panel_name}")
try:
if mode == "live":
run_live(panel_name, args, mock_params)
elif mode == "screenshot":
saved = run_screenshot(panel_name, args, out_dir)
all_outputs.extend(saved)
elif mode == "video":
mp4 = run_video(panel_name, args, out_dir)
all_outputs.append(mp4)
except Exception as e:
print(f" ❌ failed for panel '{panel_name}': {e}")
import traceback; traceback.print_exc()
if rl.is_window_ready():
rl.close_window()
# Open all outputs at once
if args.open and all_outputs:
print(f"\n 📂 opening {len(all_outputs)} output(s)...")
subprocess.Popen(["open"] + [str(p) for p in all_outputs])
print("\n ✨ done\n")
if __name__ == "__main__":
main()

296
iqpilot/tools/joystick/joystick_control.py Executable file
View File

@@ -0,0 +1,296 @@
#!/usr/bin/env python3
import argparse
import atexit
import os
from select import select
import signal
import socket
import struct
import sys
import termios
import threading
import time
import numpy as np
from inputs import UnpluggedError, get_gamepad
from iqpilot.cereal import messaging
from iqpilot.common.params import Params
from iqpilot.common.realtime import Ratekeeper
from iqpilot.system.hardware import HARDWARE
REMOTE_PORT_DEFAULT = 8765
REMOTE_TIMEOUT_S = 0.25
REMOTE_PUBLISH_HZ = 30
LOCAL_PUBLISH_HZ = 100
class KBHit:
def __init__(self) -> None:
self.stdin_fd = sys.stdin.fileno()
self.old_term = termios.tcgetattr(self.stdin_fd)
self.new_term = self.old_term.copy()
self.new_term[3] &= ~(termios.ICANON | termios.ECHO)
termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.new_term)
atexit.register(self.set_normal_term)
def set_normal_term(self) -> None:
termios.tcsetattr(self.stdin_fd, termios.TCSAFLUSH, self.old_term)
@staticmethod
def getch() -> str:
return sys.stdin.read(1)
@staticmethod
def kbhit():
return select([sys.stdin], [], [], 0)[0] != []
class Keyboard:
def __init__(self):
self.kb = KBHit()
self.axis_increment = 0.05 # 5% of full actuation each key press
self.axes_map = {'w': 'gb', 's': 'gb',
'a': 'steer', 'd': 'steer'}
self.axes_values = {'gb': 0., 'steer': 0.}
self.axes_order = ['gb', 'steer']
self.cancel = False
self.idle_sleep_s = 0.0
def update(self):
key = self.kb.getch().lower()
self.cancel = False
if key == 'r':
self.axes_values = dict.fromkeys(self.axes_values, 0.)
elif key == 'c':
self.cancel = True
elif key in self.axes_map:
axis = self.axes_map[key]
incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment
self.axes_values[axis] = float(np.clip(self.axes_values[axis] + incr, -1, 1))
else:
return False
return True
def get_buttons(self):
return [False, self.cancel]
class Joystick:
def __init__(self):
# This class supports a PlayStation 5 DualSense controller on the comma 3X
# Using both analog sticks: left stick Y for gas/brake, right stick X for steering
self.cancel_button = 'BTN_NORTH' # BTN_NORTH=X/triangle
if HARDWARE.get_device_type() == 'pc':
accel_axis = 'ABS_Y' # Left stick Y-axis
steer_axis = 'ABS_RX' # Right stick X-axis
self.flip_map = {} # No flipping needed
else:
accel_axis = 'ABS_Y' # Left stick Y-axis
steer_axis = 'ABS_Z' # Right stick X-axis
self.flip_map = {} # No flipping needed
self.min_axis_value = {accel_axis: 0., steer_axis: 0.}
self.max_axis_value = {accel_axis: 255., steer_axis: 255.}
self.axes_values = {accel_axis: 0., steer_axis: 0.}
self.axes_order = [accel_axis, steer_axis]
self.cancel = False
self.idle_sleep_s = 0.0
def update(self):
try:
joystick_event = get_gamepad()[0]
except (OSError, UnpluggedError):
self.axes_values = dict.fromkeys(self.axes_values, 0.)
return False
event = (joystick_event.code, joystick_event.state)
# flip left trigger to negative accel
if event[0] in self.flip_map:
event = (self.flip_map[event[0]], -event[1])
if event[0] == self.cancel_button:
if event[1] == 1:
self.cancel = True
elif event[1] == 0: # state 0 is falling edge
self.cancel = False
elif event[0] in self.axes_values:
self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
norm = -float(np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]))
norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3%
self.axes_values[event[0]] = norm
else:
return False
return True
def get_buttons(self):
return [False, self.cancel]
class RemoteJoystick:
def __init__(self, host: str, port: int):
self.addr = (host, port)
self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.socket.bind(self.addr)
self.socket.settimeout(0.1)
self.axes_values = {'gb': 0.0, 'steer': 0.0}
self.axes_order = ['gb', 'steer']
self.buttons = [False, False]
self.last_update = 0.0
self.authenticated = False
self.client_addr = None
self.idle_sleep_s = 0.01
def _clamp(self, value: float) -> float:
return float(np.clip(value, -1.0, 1.0))
def _handle_timeout(self, now: float) -> None:
if self.authenticated and (now - self.last_update) > REMOTE_TIMEOUT_S:
self.axes_values = {'gb': 0.0, 'steer': 0.0}
self.buttons = [False, False]
def _send_auth_ok(self, addr) -> None:
try:
self.socket.sendto(bytes([1]), addr)
except OSError:
pass
def update(self):
now = time.monotonic()
try:
data, addr = self.socket.recvfrom(64)
except TimeoutError:
self._handle_timeout(now)
return False
except OSError:
return False
if not data:
return False
msg_type = data[0]
if msg_type == 0:
self.client_addr = addr
self.authenticated = True
self._send_auth_ok(addr)
return True
if msg_type == 2:
try:
payload = data[1:].decode("utf-8", errors="strict").strip()
steer_s, accel_s, engage_s, disengage_s = payload.split(",", 3)
steer = float(steer_s)
accel = float(accel_s)
engage = engage_s == "1"
disengage = disengage_s == "1"
except (UnicodeDecodeError, ValueError):
return False
self.axes_values['steer'] = self._clamp(steer)
self.axes_values['gb'] = self._clamp(accel)
self.buttons = [engage, disengage]
self.last_update = now
return True
if msg_type != 1 or len(data) < 9:
return False
steer, accel = struct.unpack_from("<ff", data, 1)
engage = bool(data[9]) if len(data) > 9 else False
disengage = bool(data[10]) if len(data) > 10 else False
self.axes_values['steer'] = self._clamp(steer)
self.axes_values['gb'] = self._clamp(accel)
self.buttons = [engage, disengage]
self.last_update = now
return True
def get_buttons(self):
return self.buttons
def send_thread(joystick, show_values: bool):
pm = messaging.PubMaster(['testJoystick'])
publish_hz = REMOTE_PUBLISH_HZ if isinstance(joystick, RemoteJoystick) else LOCAL_PUBLISH_HZ
rk = Ratekeeper(publish_hz, print_delay_threshold=None)
while True:
if show_values and rk.frame % 20 == 0:
print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items()))
joystick_msg = messaging.new_message('testJoystick')
joystick_msg.valid = True
joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order]
joystick_msg.testJoystick.buttons = joystick.get_buttons()
pm.send('testJoystick', joystick_msg)
rk.keep_time()
def joystick_control_thread(joystick, show_values: bool):
Params().put_bool('JoystickDebugMode', True)
try:
threading.Thread(target=send_thread, args=(joystick, show_values), daemon=True).start()
while True:
updated = joystick.update()
if not updated and joystick.idle_sleep_s > 0:
time.sleep(joystick.idle_sleep_s)
finally:
Params().put_bool('JoystickDebugMode', False)
def main():
joystick_control_thread(Joystick(), True)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' +
'openpilot must be offroad before starting joystick_control. This tool supports ' +
'a PlayStation 5 DualSense controller on the comma 3X.',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
parser.add_argument('--remote', action='store_true', help='Listen for UDP joystick input')
parser.add_argument('--listen', type=int, default=REMOTE_PORT_DEFAULT, help='UDP port for remote joystick input')
parser.add_argument('--listen-address', default='0.0.0.0', help='UDP address to bind for remote input')
args = parser.parse_args()
if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ:
print("The car must be off before running joystick_control.")
exit()
if args.remote and args.keyboard:
print("Choose only one input mode.")
exit()
print()
if args.remote:
print(f'Listening for remote joystick on {args.listen_address}:{args.listen}')
elif args.keyboard:
print('Gas/brake control: `W` and `S` keys')
print('Steering control: `A` and `D` keys')
print('Buttons')
print('- `R`: Resets axes')
print('- `C`: Cancel cruise control')
else:
print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!')
print('If not running on a comma device, the mapping may need to be adjusted.')
def handle_exit(signum, _frame):
Params().put_bool('JoystickDebugMode', False)
raise SystemExit
signal.signal(signal.SIGINT, handle_exit)
signal.signal(signal.SIGTERM, handle_exit)
if args.remote:
joystick = RemoteJoystick(args.listen_address, int(args.listen))
joystick_control_thread(joystick, False)
else:
joystick = Keyboard() if args.keyboard else Joystick()
joystick_control_thread(joystick, True)

139
iqpilot/tools/joystick/joystickd.py Executable file
View File

@@ -0,0 +1,139 @@
#!/usr/bin/env python3
import math
import numpy as np
from iqpilot.cereal import messaging, car, custom
from iqdbc.car.vehicle_model import VehicleModel
from iqpilot.common.realtime import DT_CTRL, Ratekeeper
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
LongCtrlState = car.CarControl.Actuators.LongControlState
MAX_LAT_ACCEL = 5.0
MAX_STEERING_ANGLE_DEG = 500.0
ACCEL_RELEASE_THRESHOLD = 0.01
DECEL_REQUEST_THRESHOLD = -0.02
STOPPING_HOLD_SPEED_MARGIN = 0.3
STOPPING_SPEED = 0.25
def get_lateral_joystick_outputs(CP: car.CarParams, VM: VehicleModel, v_ego: float, roll: float, steer_axis: float) -> tuple[float, float, float]:
steer_axis = float(np.clip(steer_axis, -1, 1))
steering_angle_deg = steer_axis * MAX_STEERING_ANGLE_DEG
curvature = -VM.calc_curvature(math.radians(steering_angle_deg), v_ego, roll)
if CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
return 0.0, steering_angle_deg, curvature
max_curvature = MAX_LAT_ACCEL / max(v_ego ** 2, 5)
max_angle = min(math.degrees(VM.get_steer_from_curvature(max_curvature, v_ego, roll)), MAX_STEERING_ANGLE_DEG)
return steer_axis, steer_axis * max_angle, steer_axis * -max_curvature
def joystickd_thread():
params = Params()
cloudlog.info("joystickd is waiting for CarParams")
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
CP_IQ = messaging.log_from_bytes(params.get("IQCarParams", block=True), custom.IQCarParams)
VM = VehicleModel(CP)
sm = messaging.SubMaster(['carState', 'onroadEvents', 'vehicleParameters', 'selfdriveState', 'iqState', 'testJoystick'], frequency=1. / DT_CTRL)
pm = messaging.PubMaster(['carControl', 'controlsState'])
# Stop-hold behavior for joystick long control:
# - enter hold only when user requested decel and we are near/at stop
# - neutral input does not request decel while rolling
# - release hold on positive accel request
decel_intent_latched = False
stop_hold_latched = False
rk = Ratekeeper(100, print_delay_threshold=None)
while 1:
sm.update(0)
cc_msg = messaging.new_message('carControl')
cc_msg.valid = True
CC = cc_msg.carControl
ss = sm['selfdriveState']
ss_iq = sm['iqState']
aol_enabled = bool(getattr(ss_iq.aol, 'enabled', False))
aol_active = bool(getattr(ss_iq.aol, 'active', False))
joystick_angle_lat_active = aol_active or (
aol_enabled and CP.steerControlType == car.CarParams.SteerControlType.angle
)
CC.enabled = bool(ss.enabled or aol_enabled)
CC.latActive = bool(ss.active or joystick_angle_lat_active) and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
long_through_override = CP_IQ.longActiveWithGasOverride and CP.openpilotLongitudinalControl
override_longitudinal = any(e.overrideLongitudinal for e in sm['onroadEvents'])
CC.longActive = bool(ss.enabled) and (not override_longitudinal or long_through_override) and CP.openpilotLongitudinalControl
CC.cruiseControl.cancel = sm['carState'].cruiseState.enabled and (not CC.enabled or not CP.pcmCruise)
CC.hudControl.leadDistanceBars = 2
actuators = CC.actuators
# reset joystick if it hasn't been received in a while
should_reset_joystick = sm.recv_frame['testJoystick'] == 0 or (sm.frame - sm.recv_frame['testJoystick'])*DT_CTRL > 0.2
if not should_reset_joystick:
joystick_axes = sm['testJoystick'].axes
else:
joystick_axes = [0.0, 0.0]
if CC.longActive:
accel_cmd = float(np.clip(joystick_axes[0], -1, 1))
actuators.accel = 4.0 * accel_cmd
positive_accel_requested = accel_cmd > ACCEL_RELEASE_THRESHOLD
negative_accel_requested = accel_cmd < DECEL_REQUEST_THRESHOLD
near_stop = sm['carState'].standstill or sm['carState'].vEgo <= (STOPPING_SPEED + STOPPING_HOLD_SPEED_MARGIN)
if positive_accel_requested:
stop_hold_latched = False
decel_intent_latched = False
elif negative_accel_requested:
decel_intent_latched = True
if decel_intent_latched and near_stop and not positive_accel_requested:
stop_hold_latched = True
# If we are moving again and driver is not asking for decel, clear stale hold state.
if stop_hold_latched and sm['carState'].vEgo > (STOPPING_SPEED + STOPPING_HOLD_SPEED_MARGIN) and not negative_accel_requested:
stop_hold_latched = False
decel_intent_latched = False
actuators.longControlState = LongCtrlState.stopping if stop_hold_latched else LongCtrlState.pid
CC.cruiseControl.resume = positive_accel_requested
else:
decel_intent_latched = False
stop_hold_latched = False
if CC.latActive:
torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, VM, sm['carState'].vEgo, sm['vehicleParameters'].roll, joystick_axes[1])
actuators.torque = torque
actuators.steeringAngleDeg = steering_angle_deg
actuators.curvature = curvature
pm.send('carControl', cc_msg)
cs_msg = messaging.new_message('controlsState')
cs_msg.valid = True
controlsState = cs_msg.controlsState
controlsState.lateralControlState.init('debugState')
lp = sm['vehicleParameters']
steer_angle_without_offset = math.radians(sm['carState'].steeringAngleDeg - lp.angleOffsetDeg)
controlsState.curvature = -VM.calc_curvature(steer_angle_without_offset, sm['carState'].vEgo, lp.roll)
pm.send('controlsState', cs_msg)
rk.keep_time()
def main():
joystickd_thread()
if __name__ == "__main__":
main()

33
iqpilot/tools/joystick/tests/test_joystickd.py Normal file
View File

@@ -0,0 +1,33 @@
from iqpilot.cereal import car
from iqpilot.tools.joystick.joystickd import get_lateral_joystick_outputs
class StubVehicleModel:
def calc_curvature(self, steer_angle: float, v_ego: float, roll: float) -> float:
return steer_angle
def get_steer_from_curvature(self, curvature: float, v_ego: float, roll: float) -> float:
return curvature
def test_angle_cars_use_angle_outputs():
CP = car.CarParams.new_message()
CP.steerControlType = car.CarParams.SteerControlType.angle
torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, StubVehicleModel(), 20.0, 0.0, 0.5)
assert torque == 0.0
assert steering_angle_deg != 0.0
assert curvature < 0.0
def test_torque_cars_keep_torque_outputs():
CP = car.CarParams.new_message()
CP.steerControlType = car.CarParams.SteerControlType.torque
torque, steering_angle_deg, curvature = get_lateral_joystick_outputs(CP, StubVehicleModel(), 20.0, 0.0, 0.5)
assert torque == 0.5
assert steering_angle_deg != 0.0
assert curvature < 0.0

0
iqpilot/tools/lib/__init__.py Normal file
View File

62
iqpilot/tools/lib/api.py Normal file
View File

@@ -0,0 +1,62 @@
import os
import json
import requests
from requests.adapters import HTTPAdapter, Retry
from iqpilot.system.hardware.hw import Paths
API_HOST = os.getenv('API_HOST', 'https://api-iqlabs.konn3kt.com')
# TODO: this should be merged into common.api
class CommaApi:
def __init__(self, token=None):
self.session = requests.Session()
self.session.headers['User-agent'] = 'OpenpilotTools'
if token:
self.session.headers['Authorization'] = 'JWT ' + token
retries = Retry(total=5, backoff_factor=1, status_forcelist=[500, 502, 503, 504])
self.session.mount('https://', HTTPAdapter(max_retries=retries))
def request(self, method, endpoint, **kwargs):
with self.session.request(method, API_HOST + '/' + endpoint, **kwargs) as resp:
resp_json = resp.json()
if isinstance(resp_json, dict) and resp_json.get('error'):
if resp.status_code in [401, 403]:
raise UnauthorizedError('Unauthorized. Authenticate with tools/lib/auth.py')
e = APIError(str(resp.status_code) + ":" + resp_json.get('description', str(resp_json['error'])))
e.status_code = resp.status_code
raise e
return resp_json
def get(self, endpoint, **kwargs):
return self.request('GET', endpoint, **kwargs)
def post(self, endpoint, **kwargs):
return self.request('POST', endpoint, **kwargs)
class APIError(Exception):
pass
class UnauthorizedError(Exception):
pass
def get_token():
try:
with open(os.path.join(Paths.config_root(), 'auth.json')) as f:
return json.load(f)['access_token']
except Exception:
return None
def set_token(token):
os.makedirs(Paths.config_root(), exist_ok=True)
with open(os.path.join(Paths.config_root(), 'auth.json'), 'w') as f:
json.dump({'access_token': token}, f)
def clear_token():
try:
os.unlink(os.path.join(Paths.config_root(), 'auth.json'))
except FileNotFoundError:
pass

114
iqpilot/tools/lib/auth.py Executable file
View File

@@ -0,0 +1,114 @@
#!/usr/bin/env python3
"""
Usage::
usage: auth.py [-h] [{github,jwt}] [jwt]
Login to your konn3kt account
positional arguments:
{github,jwt}
jwt
optional arguments:
-h, --help show this help message and exit
Examples::
./auth.py # Log in with GitHub
./auth.py jwt ey..hw # Log in with a pre-issued JWT (for CI)
"""
import argparse
import sys
import pprint
import webbrowser
from http.server import BaseHTTPRequestHandler, HTTPServer
from typing import Any
from urllib.parse import parse_qs, urlencode
from iqpilot.tools.lib.api import APIError, CommaApi, UnauthorizedError, set_token, get_token
PORT = 3000
class ClientRedirectServer(HTTPServer):
query_params: dict[str, Any] = {}
class ClientRedirectHandler(BaseHTTPRequestHandler):
def do_GET(self):
if '?' in self.path:
query_parsed = parse_qs(self.path.split('?', 1)[1], keep_blank_values=True)
if 'code' in query_parsed or 'error' in query_parsed:
self.server.query_params = query_parsed
self.send_response(200)
self.send_header('Content-type', 'text/plain')
self.end_headers()
self.wfile.write(b'Return to the CLI to continue')
def log_message(self, fmt, *fmt_args):
sys.stderr.write(f"[auth callback] {self.address_string()} {fmt % fmt_args}\n")
def auth_redirect_link(method):
if method != 'github':
raise NotImplementedError(f"no redirect implemented for method {method}")
params = {
'client_id': 'Ov23lifjMafxJzFatvuB',
'redirect_uri': 'https://api-iqlabs.konn3kt.com/v2/auth/h/redirect/',
'state': f'service,localhost:{PORT}',
'scope': 'read:user',
}
return 'https://github.com/login/oauth/authorize?' + urlencode(params)
def login(method):
oauth_uri = auth_redirect_link(method)
web_server = ClientRedirectServer(('localhost', PORT), ClientRedirectHandler)
print(f'To sign in, use your browser and navigate to {oauth_uri}')
webbrowser.open(oauth_uri, new=2)
while True:
web_server.handle_request()
if 'code' in web_server.query_params:
break
elif 'error' in web_server.query_params:
print('Authentication Error: "{}". Description: "{}" '.format(
web_server.query_params['error'],
web_server.query_params.get('error_description')), file=sys.stderr)
break
try:
auth_resp = CommaApi().post('v2/auth/', data={'code': web_server.query_params['code'], 'provider': web_server.query_params['provider']})
set_token(auth_resp['access_token'])
except APIError as e:
print(f'Authentication Error: {e}', file=sys.stderr)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Login to your konn3kt account')
parser.add_argument('method', default='github', const='github', nargs='?', choices=['github', 'jwt'])
parser.add_argument('jwt', nargs='?')
args = parser.parse_args()
if args.method == 'jwt':
if args.jwt is None:
print("method JWT selected, but no JWT was provided")
exit(1)
set_token(args.jwt)
else:
login(args.method)
try:
me = CommaApi(token=get_token()).get('/v1/me')
print("Authenticated!")
pprint.pprint(me)
except UnauthorizedError:
print("Got invalid JWT")
exit(1)

58
iqpilot/tools/lib/filereader.py Normal file
View File

@@ -0,0 +1,58 @@
import os
import io
import posixpath
import socket
from functools import cache
from iqpilot.common.utils import retry
from urllib.parse import urlparse
from iqpilot.tools.lib.url_file import URLFile
DATA_ENDPOINT = os.getenv("DATA_ENDPOINT", "http://data-raw.comma.internal/")
@cache
@retry(delay=0.0)
def internal_source_available(url: str) -> bool:
if os.path.isdir(url):
return True
try:
hostname = urlparse(url).hostname
port = urlparse(url).port or 80
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.settimeout(0.5)
s.connect((hostname, port))
return True
except (socket.gaierror, ConnectionRefusedError):
pass
return False
def resolve_name(fn):
if fn.startswith("cd:/"):
return posixpath.join(DATA_ENDPOINT, fn[4:])
return fn
@cache
def file_exists(fn):
fn = resolve_name(fn)
if fn.startswith(("http://", "https://")):
return URLFile(fn).get_length_online() != -1
return os.path.exists(fn)
class DiskFile(io.BufferedReader):
def get_multi_range(self, ranges: list[tuple[int, int]]) -> list[bytes]:
parts = []
for r in ranges:
self.seek(r[0])
parts.append(self.read(r[1] - r[0]))
return parts
def FileReader(fn):
fn = resolve_name(fn)
if fn.startswith(("http://", "https://")):
return URLFile(fn)
else:
return DiskFile(open(fn, "rb"))

176
iqpilot/tools/lib/framereader.py Normal file
View File

@@ -0,0 +1,176 @@
import os
import subprocess
import json
import logging
from collections.abc import Iterator
from collections import OrderedDict
import numpy as np
from iqpilot.tools.lib.filereader import FileReader, resolve_name
from iqpilot.tools.lib.vidindex import hevc_index
class DataUnreadableError(Exception):
pass
logger = logging.getLogger("tools")
HEVC_SLICE_B = 0
HEVC_SLICE_P = 1
HEVC_SLICE_I = 2
class LRUCache:
def __init__(self, capacity: int):
self._cache: OrderedDict = OrderedDict()
self.capacity = capacity
def __getitem__(self, key):
self._cache.move_to_end(key)
return self._cache[key]
def __setitem__(self, key, value):
self._cache[key] = value
if len(self._cache) > self.capacity:
self._cache.popitem(last=False)
def __contains__(self, key):
return key in self._cache
def assert_hvec(fn: str) -> None:
with FileReader(fn) as f:
header = f.read(4)
if len(header) == 0:
raise DataUnreadableError(f"{fn} is empty")
elif header == b"\x00\x00\x00\x01":
if 'hevc' not in fn:
raise NotImplementedError(fn)
def decompress_video_data(rawdat, w, h, pix_fmt="rgb24", vid_fmt='hevc', hwaccel="auto", loglevel="info") -> np.ndarray:
threads = os.getenv("FFMPEG_THREADS", "0")
args = ["ffmpeg", "-v", loglevel,
"-threads", threads,
"-hwaccel", hwaccel,
"-c:v", "hevc",
"-vsync", "0",
"-f", vid_fmt,
"-flags2", "showall",
"-i", "-",
"-f", "rawvideo",
"-pix_fmt", pix_fmt,
"-"]
dat = subprocess.check_output(args, input=rawdat)
ret: np.ndarray
if pix_fmt == "rgb24":
ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, h, w, 3)
elif pix_fmt in ["nv12", "yuv420p"]:
ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, (h*w*3//2))
else:
raise NotImplementedError(f"Unsupported pixel format: {pix_fmt}")
return ret
def ffprobe(fn, fmt=None):
fn = resolve_name(fn)
cmd = ["ffprobe", "-v", "quiet", "-print_format", "json", "-show_format", "-show_streams"]
if fmt:
cmd += ["-f", fmt]
cmd += ["-i", "-"]
try:
with FileReader(fn) as f:
ffprobe_output = subprocess.check_output(cmd, input=f.read(4096))
except subprocess.CalledProcessError as e:
raise DataUnreadableError(fn) from e
return json.loads(ffprobe_output)
def get_index_data(fn: str, index_data: dict|None = None):
if index_data is None:
index_data = get_video_index(fn)
if index_data is None:
raise DataUnreadableError(f"Failed to index {fn!r}")
stream = index_data["probe"]["streams"][0]
return index_data["index"], index_data["global_prefix"], stream["width"], stream["height"]
def get_video_index(fn):
assert_hvec(fn)
frame_types, dat_len, prefix = hevc_index(fn)
index = np.array(frame_types + [(0xFFFFFFFF, dat_len)], dtype=np.uint32)
probe = ffprobe(fn, "hevc")
return {
'index': index,
'global_prefix': prefix,
'probe': probe
}
class FfmpegDecoder:
def __init__(self, fn: str, index_data: dict|None = None,
pix_fmt: str = "rgb24", hwaccel="auto", loglevel="quiet"):
self.fn = fn
self.index, self.prefix, self.w, self.h = get_index_data(fn, index_data)
self.frame_count = len(self.index) - 1 # sentinel row at the end
self.iframes = np.where(self.index[:, 0] == HEVC_SLICE_I)[0]
self.pix_fmt = pix_fmt
self.loglevel, self.hwaccel = loglevel, hwaccel
def _gop_bounds(self, frame_idx: int):
f_b = frame_idx
while f_b > 0 and self.index[f_b, 0] != HEVC_SLICE_I:
f_b -= 1
f_e = frame_idx + 1
while f_e < self.frame_count and self.index[f_e, 0] != HEVC_SLICE_I:
f_e += 1
return f_b, f_e, self.index[f_b, 1], self.index[f_e, 1]
def _decode_gop(self, raw: bytes) -> Iterator[np.ndarray]:
yield from decompress_video_data(raw, self.w, self.h, pix_fmt=self.pix_fmt, hwaccel=self.hwaccel, loglevel=self.loglevel)
def get_gop_start(self, frame_idx: int):
return self.iframes[np.searchsorted(self.iframes, frame_idx, side="right") - 1]
def get_iterator(self, start_fidx: int = 0, end_fidx: int|None = None,
frame_skip: int = 1) -> Iterator[tuple[int, np.ndarray]]:
end_fidx = end_fidx or self.frame_count
fidx = start_fidx
while fidx < end_fidx:
f_b, f_e, off_b, off_e = self._gop_bounds(fidx)
with FileReader(self.fn) as f:
f.seek(off_b)
raw = self.prefix + f.read(off_e - off_b)
# number of frames to discard inside this GOP before the wanted one
for i, frm in enumerate(decompress_video_data(raw, self.w, self.h, self.pix_fmt, hwaccel=self.hwaccel, loglevel=self.loglevel)):
fidx = f_b + i
if fidx >= end_fidx:
return
elif fidx >= start_fidx and (fidx - start_fidx) % frame_skip == 0:
yield fidx, frm
fidx += 1
def FrameIterator(fn: str, index_data: dict|None=None, pix_fmt: str = "rgb24",
start_fidx:int=0, end_fidx=None, frame_skip:int=1, hwaccel="auto", loglevel="quiet") -> Iterator[np.ndarray]:
dec = FfmpegDecoder(fn, pix_fmt=pix_fmt, index_data=index_data, hwaccel=hwaccel, loglevel=loglevel)
for _, frame in dec.get_iterator(start_fidx=start_fidx, end_fidx=end_fidx, frame_skip=frame_skip):
yield frame
class FrameReader:
def __init__(self, fn: str, index_data: dict|None = None, cache_size: int = 30,
pix_fmt: str = "rgb24", hwaccel="auto", loglevel="quiet"):
self.decoder = FfmpegDecoder(fn, index_data=index_data, pix_fmt=pix_fmt, hwaccel=hwaccel, loglevel=loglevel)
self.iframes = self.decoder.iframes
self._cache: LRUCache = LRUCache(cache_size)
self.w, self.h, self.frame_count, = self.decoder.w, self.decoder.h, self.decoder.frame_count
self.pix_fmt = pix_fmt
self.it: Iterator[tuple[int, np.ndarray]] | None = None
self.fidx = -1
def get(self, fidx:int):
if fidx in self._cache: # If frame is cached, return it
return self._cache[fidx]
read_start = self.decoder.get_gop_start(fidx)
if not self.it or fidx < self.fidx or read_start != self.decoder.get_gop_start(self.fidx): # If the frame is in a different GOP, reset the iterator
self.it = self.decoder.get_iterator(read_start)
self.fidx = -1
while self.fidx < fidx:
self.fidx, frame = next(self.it)
self._cache[self.fidx] = frame
return self._cache[fidx]

436
iqpilot/tools/lib/logreader.py Executable file
View File

@@ -0,0 +1,436 @@
#!/usr/bin/env python3
import bz2
from functools import partial
import multiprocessing
import capnp
import enum
import os
import pathlib
import sys
import tqdm
import urllib.parse
import warnings
import zstandard as zstd
import numpy as np
from collections.abc import Callable, Iterable, Iterator
from pathlib import Path
from urllib.parse import parse_qs, urlparse
from iqpilot.cereal import log as capnp_log, messaging
from iqpilot.cereal.services import SERVICE_LIST
from iqpilot.common.swaglog import cloudlog
from iqpilot.tools.lib.filereader import DATA_ENDPOINT, FileReader, file_exists, internal_source_available
from iqpilot.tools.lib.route import Route, SegmentRange, FileName
LogMessage = type[capnp._DynamicStructReader]
LogIterable = Iterable[LogMessage]
RawLogIterable = Iterable[bytes]
FileNames = tuple[str, ...]
Source = Callable[[SegmentRange, list[int], FileNames], dict[int, str]]
InternalUnavailableException = Exception("Internal source not available")
OPENPILOT_CI_BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
OPENPILOT_CI_ACCOUNT_URL = "https://commadataci.blob.core.windows.net"
def get_url(route_name: str, segment_num: str | int, filename: str) -> str:
return f"{OPENPILOT_CI_BASE_URL}{route_name.replace('|', '/')}/{segment_num}/{filename}"
def upload_file(path: str, blob_name: str, overwrite=False) -> str:
from azure.identity import AzureCliCredential
from azure.storage.blob import BlobClient
token_path = Path("/data/azure_token")
credential = os.environ.get("AZURE_TOKEN") or (token_path.read_text().strip() if token_path.is_file() else AzureCliCredential())
client = BlobClient(OPENPILOT_CI_ACCOUNT_URL, container_name="openpilotci", blob_name=blob_name, credential=credential)
with open(path, "rb") as f:
client.upload_blob(f, overwrite=overwrite)
return OPENPILOT_CI_BASE_URL + blob_name
def comma_api_source(sr: SegmentRange, seg_idxs: list[int], fns: FileNames) -> dict[int, str]:
route = Route(sr.route_name)
if fns == FileName.RLOG:
return {seg: route.log_paths()[seg] for seg in seg_idxs if route.log_paths()[seg] is not None}
return {seg: route.qlog_paths()[seg] for seg in seg_idxs if route.qlog_paths()[seg] is not None}
def internal_source(sr: SegmentRange, seg_idxs: list[int], fns: FileNames, endpoint_url: str = DATA_ENDPOINT) -> dict[int, str]:
if not internal_source_available(endpoint_url):
raise InternalUnavailableException
def internal_url(seg, file):
return f"{endpoint_url.rstrip('/')}/{sr.dongle_id}/{sr.log_id}/{seg}/{file}"
return eval_source({seg: [internal_url(seg, fn) for fn in fns] for seg in seg_idxs})
def openpilotci_source(sr: SegmentRange, seg_idxs: list[int], fns: FileNames) -> dict[int, str]:
return eval_source({seg: [get_url(sr.route_name, seg, fn) for fn in fns] for seg in seg_idxs})
def eval_source(files: dict[int, list[str] | str]) -> dict[int, str]:
valid_files: dict[int, str] = {}
for seg_idx, urls in files.items():
if isinstance(urls, str):
urls = [urls]
for url in urls:
if file_exists(url):
valid_files[seg_idx] = url
break
return valid_files
ALL_SERVICES = list(SERVICE_LIST.keys())
def raw_live_logreader(services: list[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> RawLogIterable:
if addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.reset_context()
poller = messaging.Poller()
for service in services:
messaging.sub_sock(service, poller, addr=addr)
while True:
for sock in poller.poll(100):
yield sock.receive()
def live_logreader(services: list[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> LogIterable:
for msg in raw_live_logreader(services, addr):
with capnp_log.Event.from_bytes(msg) as evt:
yield evt
def flatten_type_dict(data, sep="/", prefix=None):
result = {}
if isinstance(data, dict):
for key, value in data.items():
result.update(flatten_type_dict(value, sep, key if prefix is None else prefix + sep + key))
return result
if isinstance(data, list):
return {prefix: np.array(data)}
return {prefix: data}
def get_message_dict(message, typ):
valid = message.valid
message = message._get(typ)
if not hasattr(message, 'to_dict') or typ in ('qcomGnss', 'ubloxGnss'):
return None
result = flatten_type_dict(message.to_dict(verbose=True))
result['_valid'] = valid
return result
def potentially_ragged_array(values, dtype=None, **kwargs):
try:
return np.array(values, dtype=dtype, **kwargs)
except ValueError:
return np.array(values, dtype=object, **kwargs)
def msgs_to_time_series(msgs):
values = {}
for msg in msgs:
typ = msg.which()
msg_dict = get_message_dict(msg, typ)
if msg_dict is None:
continue
group = values.setdefault(typ, {"t": [], **{key: [] for key in msg_dict}})
group["t"].append(msg.logMonoTime / 1.0e9)
for key, value in msg_dict.items():
group[key].append(value)
for group in values.values():
order = np.argsort(group["t"])
for name, group_values in group.items():
group[name] = potentially_ragged_array(group_values)[order]
return values
def save_log(dest, log_msgs, compress=True):
dat = b"".join(msg.as_builder().to_bytes() for msg in log_msgs)
if compress and dest.endswith(".bz2"):
dat = bz2.compress(dat)
elif compress and dest.endswith(".zst"):
dat = zstd.compress(dat, 10)
with open(dest, "wb") as f:
f.write(dat)
def decompress_stream(data: bytes):
dctx = zstd.ZstdDecompressor()
decompressed_data = b""
with dctx.stream_reader(data) as reader:
decompressed_data = reader.read()
return decompressed_data
class CachedEventReader:
__slots__ = ('_evt', '_enum')
def __init__(self, evt: capnp._DynamicStructReader, _enum: str | None = None):
"""All capnp attribute accesses are expensive, and which() is often called multiple times"""
self._evt = evt
self._enum: str | None = _enum
# fast pickle support
def __reduce__(self):
return CachedEventReader._reducer, (self._evt.as_builder().to_bytes(), self._enum)
@staticmethod
def _reducer(data: bytes, _enum: str | None = None):
with capnp_log.Event.from_bytes(data) as evt:
return CachedEventReader(evt, _enum)
def __repr__(self):
return self._evt.__repr__()
def __str__(self):
return self._evt.__str__()
def __dir__(self):
return dir(self._evt)
def which(self) -> str:
if self._enum is None:
self._enum = self._evt.which()
return self._enum
def __getattr__(self, name: str):
if name.startswith("__") and name.endswith("__"):
return getattr(self, name)
return getattr(self._evt, name)
class _LogFileReader:
def __init__(self, fn, only_union_types=False, sort_by_time=False, dat=None):
self.data_version = None
self._only_union_types = only_union_types
ext = None
if not dat:
_, ext = os.path.splitext(urllib.parse.urlparse(fn).path)
if ext not in ('', '.bz2', '.zst'):
# old rlogs weren't compressed
raise ValueError(f"unknown extension {ext}")
with FileReader(fn) as f:
dat = f.read()
if ext == ".bz2" or dat.startswith(b'BZh9'):
dat = bz2.decompress(dat)
elif ext == ".zst" or dat.startswith(b'\x28\xB5\x2F\xFD'):
# https://github.com/facebook/zstd/blob/dev/doc/zstd_compression_format.md#zstandard-frames
dat = decompress_stream(dat)
ents = capnp_log.Event.read_multiple_bytes(dat)
self._ents = []
try:
for e in ents:
self._ents.append(CachedEventReader(e))
except capnp.KjException:
warnings.warn("Corrupted events detected", RuntimeWarning, stacklevel=1)
if sort_by_time:
self._ents.sort(key=lambda x: x.logMonoTime)
def __iter__(self) -> Iterator[capnp._DynamicStructReader]:
for ent in self._ents:
if self._only_union_types:
try:
ent.which()
yield ent
except (capnp.lib.capnp.KjException, RuntimeError):
pass
else:
yield ent
class ReadMode(enum.StrEnum):
RLOG = "r" # only read rlogs
QLOG = "q" # only read qlogs
AUTO = "a" # default to rlogs, fallback to qlogs
AUTO_INTERACTIVE = "i" # default to rlogs, fallback to qlogs with a prompt from the user
class LogsUnavailable(Exception):
pass
def direct_source(file_or_url: str) -> list[str]:
return [file_or_url]
# TODO this should apply to camera files as well
def auto_source(identifier: str, sources: list[Source], default_mode: ReadMode) -> list[str]:
exceptions = {}
sr = SegmentRange(identifier)
needed_seg_idxs = sr.seg_idxs
mode = default_mode if sr.selector is None else ReadMode(sr.selector)
if mode == ReadMode.QLOG:
try_fns = [FileName.QLOG]
else:
try_fns = [FileName.RLOG]
# If selector allows it, fallback to qlogs
if mode in (ReadMode.AUTO, ReadMode.AUTO_INTERACTIVE):
try_fns.append(FileName.QLOG)
# Build a dict of valid files as we evaluate each source. May contain mix of rlogs, qlogs, and None.
# This function only returns when we've sourced all files, or throws an exception
valid_files: dict[int, str] = {}
for fn in try_fns:
for source in sources:
try:
files = source(sr, needed_seg_idxs, fn)
# Build a dict of valid files
valid_files |= files
# Don't check for segment files that have already been found
needed_seg_idxs = [idx for idx in needed_seg_idxs if idx not in valid_files]
# We've found all files, return them
if len(needed_seg_idxs) == 0:
return list(valid_files.values())
else:
raise FileNotFoundError(f"Did not find {fn} for seg idxs {needed_seg_idxs} of {sr.route_name}")
except Exception as e:
exceptions[source.__name__] = e
if fn == try_fns[0]:
missing_logs = len(needed_seg_idxs)
if mode == ReadMode.AUTO:
cloudlog.warning(f"{missing_logs}/{len(sr.seg_idxs)} rlogs were not found, falling back to qlogs for those segments...")
elif mode == ReadMode.AUTO_INTERACTIVE:
if input(f"{missing_logs}/{len(sr.seg_idxs)} rlogs were not found, would you like to fallback to qlogs for those segments? (y/N) ").lower() != "y":
break
missing_logs = len(needed_seg_idxs)
raise LogsUnavailable(f"{missing_logs}/{len(sr.seg_idxs)} logs were not found, please ensure all logs " +
"are uploaded. You can fall back to qlogs with '/a' selector at the end of the route name.\n\n" +
"Exceptions for sources:\n - " + "\n - ".join([f"{k}: {repr(v)}" for k, v in exceptions.items()]))
def parse_indirect(identifier: str) -> str:
if "useradmin.comma.ai" in identifier:
query = parse_qs(urlparse(identifier).query)
identifier = query["onebox"][0]
elif "connect.comma.ai" in identifier or "konn3kt.com" in identifier:
path = urlparse(identifier).path.strip("/").split("/")
if path and path[0] == "connectdata":
# signed data URL from the API host (api-*.konn3kt.com/connectdata/...), not a share link
return identifier
path = ['/'.join(path[:2]), *path[2:]] # recombine log id
identifier = path[0]
if len(path) > 2:
# convert url with seconds to segments
start, end = int(path[1]) // 60, int(path[2]) // 60 + 1
identifier = f"{identifier}/{start}:{end}"
# add selector if it exists
if len(path) > 3:
identifier += f"/{path[3]}"
else:
# add selector if it exists
identifier = "/".join(path)
return identifier
def parse_direct(identifier: str):
if identifier.startswith(("http://", "https://", "cd:/")) or pathlib.Path(identifier).exists():
return identifier
return None
class LogReader:
def _parse_identifier(self, identifier: str) -> list[str]:
# useradmin, etc.
identifier = parse_indirect(identifier)
# direct url or file
direct_parsed = parse_direct(identifier)
if direct_parsed is not None:
return direct_source(identifier)
identifiers = auto_source(identifier, self.sources, self.default_mode)
return identifiers
def __init__(self, identifier: str | list[str], default_mode: ReadMode = ReadMode.RLOG,
sources: list[Source] | None = None, sort_by_time=False, only_union_types=False):
if sources is None:
sources = [internal_source, comma_api_source, openpilotci_source]
self.default_mode = default_mode
self.sources = sources
self.identifier = identifier
if isinstance(identifier, str):
self.identifier = [identifier]
self.sort_by_time = sort_by_time
self.only_union_types = only_union_types
self.__lrs: dict[int, _LogFileReader] = {}
self.reset()
def _get_lr(self, i):
if i not in self.__lrs:
self.__lrs[i] = _LogFileReader(self.logreader_identifiers[i], sort_by_time=self.sort_by_time, only_union_types=self.only_union_types)
return self.__lrs[i]
def __iter__(self):
for i in range(len(self.logreader_identifiers)):
yield from self._get_lr(i)
def _run_on_segment(self, func, i):
return func(self._get_lr(i))
def run_across_segments(self, num_processes, func, disable_tqdm=False, desc=None):
with multiprocessing.Pool(num_processes) as pool:
ret = []
num_segs = len(self.logreader_identifiers)
for p in tqdm.tqdm(pool.imap(partial(self._run_on_segment, func), range(num_segs)), total=num_segs, disable=disable_tqdm, desc=desc):
ret.extend(p)
return ret
def reset(self):
self.logreader_identifiers = []
for identifier in self.identifier:
self.logreader_identifiers.extend(self._parse_identifier(identifier))
@staticmethod
def from_bytes(dat):
return _LogFileReader("", dat=dat)
def filter(self, msg_type: str):
return (getattr(m, m.which()) for m in filter(lambda m: m.which() == msg_type, self))
def first(self, msg_type: str):
return next(self.filter(msg_type), None)
@property
def time_series(self):
return msgs_to_time_series(self)
if __name__ == "__main__":
import codecs
# capnproto <= 0.8.0 throws errors converting byte data to string
# below line catches those errors and replaces the bytes with \x__
codecs.register_error("strict", codecs.backslashreplace_errors)
log_path = sys.argv[1]
lr = LogReader(log_path, sort_by_time=True)
for msg in lr:
print(msg)

382
iqpilot/tools/lib/route.py Normal file
View File

@@ -0,0 +1,382 @@
import os
import re
import requests
from functools import cache
from urllib.parse import urlparse
from collections import defaultdict
from itertools import chain
from iqpilot.tools.lib.api import APIError, CommaApi, get_token
class RE:
DONGLE_ID = r'(?P<dongle_id>[a-f0-9]{16})'
TIMESTAMP = r'(?P<timestamp>[0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2})'
LOG_ID_V2 = r'(?P<count>[a-f0-9]{8})--(?P<uid>[a-z0-9]{10})'
LOG_ID = fr'(?P<log_id>(?:{TIMESTAMP}|{LOG_ID_V2}))'
ROUTE_NAME = fr'(?P<route_name>{DONGLE_ID}[|_/]{LOG_ID})'
SEGMENT_NAME = fr'{ROUTE_NAME}(?:--|/)(?P<segment_num>[0-9]+)'
INDEX = r'-?[0-9]+'
SLICE = fr'(?P<start>{INDEX})?:?(?P<end>{INDEX})?:?(?P<step>{INDEX})?'
SEGMENT_RANGE = fr'{ROUTE_NAME}(?:(--|/)(?P<slice>({SLICE})))?(?:/(?P<selector>([qra])))?'
BOOTLOG_NAME = ROUTE_NAME
EXPLORER_FILE = fr'^(?P<segment_name>{SEGMENT_NAME})--(?P<file_name>[a-z]+\.[a-z0-9]+)$'
OP_SEGMENT_DIR = fr'^(?P<segment_name>{SEGMENT_NAME})$'
class FileName:
RLOG = ("rlog.zst", "rlog.bz2")
QLOG = ("qlog.zst", "qlog.bz2")
QCAMERA = ('qcamera.ts',)
FCAMERA = ('fcamera.hevc',)
ECAMERA = ('ecamera.hevc',)
DCAMERA = ('dcamera.hevc',)
BOOTLOG = ('bootlog.zst', 'bootlog.bz2')
class Route:
def __init__(self, name, data_dir=None):
self._name = RouteName(name)
self.files = None
if data_dir is not None:
self._segments = self._get_segments_local(data_dir)
else:
self._segments = self._get_segments_remote()
self.max_seg_number = self._segments[-1].name.segment_num
@property
def name(self):
return self._name
@property
def segments(self):
return self._segments
def log_paths(self):
log_path_by_seg_num = {s.name.segment_num: s.log_path for s in self._segments}
return [log_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
def qlog_paths(self):
qlog_path_by_seg_num = {s.name.segment_num: s.qlog_path for s in self._segments}
return [qlog_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
def camera_paths(self):
camera_path_by_seg_num = {s.name.segment_num: s.camera_path for s in self._segments}
return [camera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
def dcamera_paths(self):
dcamera_path_by_seg_num = {s.name.segment_num: s.dcamera_path for s in self._segments}
return [dcamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
def ecamera_paths(self):
ecamera_path_by_seg_num = {s.name.segment_num: s.ecamera_path for s in self._segments}
return [ecamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
def qcamera_paths(self):
qcamera_path_by_seg_num = {s.name.segment_num: s.qcamera_path for s in self._segments}
return [qcamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number + 1)]
# TODO: refactor this, it's super repetitive
def _get_segments_remote(self):
api = CommaApi(get_token())
route_files = api.get('v1/route/' + self.name.canonical_name + '/files')
self.files = [f['url'] if isinstance(f, dict) else f
for f in chain.from_iterable(route_files.values())]
segments = {}
for url in self.files:
_, dongle_id, time_str, segment_num, fn = urlparse(url).path.rsplit('/', maxsplit=4)
segment_name = f'{dongle_id}|{time_str}--{segment_num}'
if segments.get(segment_name):
segments[segment_name] = Segment(
segment_name,
url if fn in FileName.RLOG else segments[segment_name].log_path,
url if fn in FileName.QLOG else segments[segment_name].qlog_path,
url if fn in FileName.FCAMERA else segments[segment_name].camera_path,
url if fn in FileName.DCAMERA else segments[segment_name].dcamera_path,
url if fn in FileName.ECAMERA else segments[segment_name].ecamera_path,
url if fn in FileName.QCAMERA else segments[segment_name].qcamera_path,
)
else:
segments[segment_name] = Segment(
segment_name,
url if fn in FileName.RLOG else None,
url if fn in FileName.QLOG else None,
url if fn in FileName.FCAMERA else None,
url if fn in FileName.DCAMERA else None,
url if fn in FileName.ECAMERA else None,
url if fn in FileName.QCAMERA else None,
)
return sorted(segments.values(), key=lambda seg: seg.name.segment_num)
def _get_segments_local(self, data_dir):
files = os.listdir(data_dir)
segment_files = defaultdict(list)
for f in files:
fullpath = os.path.join(data_dir, f)
explorer_match = re.match(RE.EXPLORER_FILE, f)
op_match = re.match(RE.OP_SEGMENT_DIR, f)
if explorer_match:
segment_name = explorer_match.group('segment_name')
fn = explorer_match.group('file_name')
if segment_name.replace('_', '|').startswith(self.name.canonical_name):
segment_files[segment_name].append((fullpath, fn))
elif op_match and os.path.isdir(fullpath):
segment_name = op_match.group('segment_name')
if segment_name.startswith(self.name.canonical_name):
for seg_f in os.listdir(fullpath):
segment_files[segment_name].append((os.path.join(fullpath, seg_f), seg_f))
elif f == self.name.canonical_name:
for seg_num in os.listdir(fullpath):
if not seg_num.isdigit():
continue
segment_name = f'{self.name.canonical_name}--{seg_num}'
for seg_f in os.listdir(os.path.join(fullpath, seg_num)):
segment_files[segment_name].append((os.path.join(fullpath, seg_num, seg_f), seg_f))
segments = []
for segment, files in segment_files.items():
try:
log_path = next(path for path, filename in files if filename in FileName.RLOG)
except StopIteration:
log_path = None
try:
qlog_path = next(path for path, filename in files if filename in FileName.QLOG)
except StopIteration:
qlog_path = None
try:
camera_path = next(path for path, filename in files if filename in FileName.FCAMERA)
except StopIteration:
camera_path = None
try:
dcamera_path = next(path for path, filename in files if filename in FileName.DCAMERA)
except StopIteration:
dcamera_path = None
try:
ecamera_path = next(path for path, filename in files if filename in FileName.ECAMERA)
except StopIteration:
ecamera_path = None
try:
qcamera_path = next(path for path, filename in files if filename in FileName.QCAMERA)
except StopIteration:
qcamera_path = None
segments.append(Segment(segment, log_path, qlog_path, camera_path, dcamera_path, ecamera_path, qcamera_path))
if len(segments) == 0:
raise ValueError(f'Could not find segments for route {self.name.canonical_name} in data directory {data_dir}')
return sorted(segments, key=lambda seg: seg.name.segment_num)
class Segment:
def __init__(self, name, log_path, qlog_path, camera_path, dcamera_path, ecamera_path, qcamera_path):
self._events = None
self._name = SegmentName(name)
self.log_path = log_path
self.qlog_path = qlog_path
self.camera_path = camera_path
self.dcamera_path = dcamera_path
self.ecamera_path = ecamera_path
self.qcamera_path = qcamera_path
@property
def name(self):
return self._name
@staticmethod
@cache
def _get_route_metadata(route_name: str):
api = CommaApi(get_token())
return api.get(f'v1/route/{route_name}')
@property
def url(self):
route_name = self._name.route_name.canonical_name
metadata = self._get_route_metadata(route_name)
return f'{metadata["url"]}/{self._name.segment_num}'
@property
def events(self):
if not self._events:
try:
resp = requests.get(f'{self.url}/events.json')
resp.raise_for_status()
self._events = resp.json()
except Exception as e:
raise APIError(f'error getting events for segment {self._name}') from e
return self._events
class RouteName:
def __init__(self, name_str: str):
self._name_str = name_str
delim = next(c for c in self._name_str if c in ("|", "/"))
self._dongle_id, self._time_str = self._name_str.split(delim)
assert len(self._dongle_id) == 16, self._name_str
assert len(self._time_str) == 20, self._name_str
self._canonical_name = f"{self._dongle_id}|{self._time_str}"
@property
def canonical_name(self) -> str: return self._canonical_name
@property
def dongle_id(self) -> str: return self._dongle_id
@property
def log_id(self) -> str: return self._time_str
@property
def time_str(self) -> str: return self._time_str
@property
def azure_prefix(self):
return f'{self.dongle_id}/{self.log_id}'
def __str__(self) -> str: return self._canonical_name
class SegmentName:
# TODO: add constructor that takes dongle_id, time_str, segment_num and then create instances
# of this class instead of manually constructing a segment name (use canonical_name prop instead)
def __init__(self, name_str: str, allow_route_name=False):
data_dir_path_separator_index = name_str.rsplit("|", 1)[0].rfind("/")
use_data_dir = (data_dir_path_separator_index != -1) and ("|" in name_str)
self._name_str = name_str[data_dir_path_separator_index + 1:] if use_data_dir else name_str
self._data_dir = name_str[:data_dir_path_separator_index] if use_data_dir else None
seg_num_delim = "--" if self._name_str.count("--") == 2 else "/"
name_parts = self._name_str.rsplit(seg_num_delim, 1)
if allow_route_name and len(name_parts) == 1:
name_parts.append("-1") # no segment number
self._route_name = RouteName(name_parts[0])
self._num = int(name_parts[1])
self._canonical_name = f"{self._route_name._dongle_id}|{self._route_name._time_str}--{self._num}"
@property
def canonical_name(self) -> str: return self._canonical_name
# TODO should only use one name
@property
def data_name(self) -> str: return f"{self._route_name.canonical_name}/{self._num}"
@property
def azure_prefix(self):
return f'{self.dongle_id}/{self.log_id}/{self._num}'
@property
def dongle_id(self) -> str: return self._route_name.dongle_id
@property
def time_str(self) -> str: return self._route_name.time_str
@property
def log_id(self) -> str: return self._route_name.time_str
@property
def segment_num(self) -> int: return self._num
@property
def route_name(self) -> RouteName: return self._route_name
@property
def data_dir(self) -> str | None: return self._data_dir
def __str__(self) -> str: return self._canonical_name
@staticmethod
def from_file_name(file_name):
# ??????/xxxxxxxxxxxxxxxx|1111-11-11-11--11-11-11/1/rlog.bz2
dongle_id, route_name, segment_num = file_name.replace('|', '/').split('/')[-4:-1]
return SegmentName(dongle_id + "|" + route_name + "--" + segment_num)
@staticmethod
def from_device_key(dongle_id, key):
# 2018-05-07--18-56-13--5/rlog.bz2
segment_name = key.split('/')[0]
return SegmentName(dongle_id + "|" + segment_name)
@staticmethod
def from_file_key(key):
# 38c52c217150700f/2018-05-07--18-56-13/5/rlog.bz2
az_prefix = '/'.join(key.split('/')[:3])
return SegmentName.from_azure_prefix(az_prefix)
@staticmethod
def from_azure_prefix(prefix):
# xxxxxxxx/1111-11-11-11--11-11-11/0
dongle_id, route_name, segment_num = prefix.split("/")
return SegmentName(dongle_id + "|" + route_name + "--" + segment_num)
@cache
def get_max_seg_number_cached(sr: 'SegmentRange') -> int:
try:
api = CommaApi(get_token())
max_seg_number = api.get("/v1/route/" + sr.route_name.replace("/", "|"))["maxqlog"]
assert isinstance(max_seg_number, int)
return max_seg_number
except Exception as e:
raise Exception("unable to get max_segment_number. ensure you have access to this route or the route is public.") from e
class SegmentRange:
def __init__(self, segment_range: str):
m = re.fullmatch(RE.SEGMENT_RANGE, segment_range)
assert m is not None, f"Segment range is not valid {segment_range}"
self.m = m
@property
def route_name(self) -> str:
return self.m.group("route_name")
@property
def dongle_id(self) -> str:
return self.m.group("dongle_id")
@property
def log_id(self) -> str:
return self.m.group("log_id")
@property
def slice(self) -> str:
return self.m.group("slice") or ""
@property
def selector(self) -> str | None:
return self.m.group("selector")
@property
def seg_idxs(self) -> list[int]:
m = re.fullmatch(RE.SLICE, self.slice)
assert m is not None, f"Invalid slice: {self.slice}"
start, end, step = (None if s is None else int(s) for s in m.groups())
# one segment specified
if start is not None and end is None and ':' not in self.slice:
if start < 0:
start += get_max_seg_number_cached(self) + 1
return [start]
s = slice(start, end, step)
# no specified end or using relative indexing, need number of segments
if end is None or end < 0 or (start is not None and start < 0):
return list(range(get_max_seg_number_cached(self) + 1))[s]
else:
return list(range(end + 1))[s]
def __str__(self) -> str:
return f"{self.dongle_id}/{self.log_id}" + (f"/{self.slice}" if self.slice else "") + (f"/{self.selector}" if self.selector else "")
def __repr__(self) -> str:
return self.__str__()

0
iqpilot/tools/lib/tests/__init__.py Normal file
View File

205
iqpilot/tools/lib/tests/test_caching.py Normal file
View File

@@ -0,0 +1,205 @@
import http.server
import multiprocessing
import os
import shutil
import socket
import tempfile
import pytest
from iqpilot.selfdrive.test.helpers import http_server_context
from iqpilot.system.hardware.hw import Paths
from iqpilot.tools.lib.url_file import URLFile, prune_cache
import iqpilot.tools.lib.url_file as url_file_module
def concurrent_prune_cache(cache_root, entry, barrier):
Paths.download_cache_root = staticmethod(lambda: cache_root)
barrier.wait()
prune_cache(entry)
class CachingTestRequestHandler(http.server.BaseHTTPRequestHandler):
FILE_EXISTS = True
def do_GET(self):
if self.FILE_EXISTS:
self.send_response(206 if "Range" in self.headers else 200, b'1234')
else:
self.send_response(404)
self.end_headers()
def do_HEAD(self):
if self.FILE_EXISTS:
self.send_response(200)
self.send_header("Content-Length", "4")
else:
self.send_response(404)
self.end_headers()
@pytest.fixture
def host():
with http_server_context(handler=CachingTestRequestHandler) as (host, port):
yield f"http://{host}:{port}"
class TestFileDownload:
def test_head_connection_released(self, monkeypatch):
class Response:
status = 200
headers = {"content-length": "4"}
released = False
def release_conn(self):
self.released = True
response = Response()
monkeypatch.setattr(URLFile, "_request", lambda self, method, url, headers=None: response)
assert URLFile("https://example.com/test").get_length_online() == 4
assert response.released
def test_pipeline_defaults(self, host):
# TODO: parameterize the defaults so we don't rely on hard-coded values in xx
assert URLFile.pool_manager().pools._maxsize == 10# PoolManager num_pools param
pool_manager_defaults = {
"maxsize": 100,
"socket_options": [(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1),],
}
for k, v in pool_manager_defaults.items():
assert URLFile.pool_manager().connection_pool_kw.get(k) == v
retry_defaults = {
"total": 6,
"backoff_factor": 0.75,
"status_forcelist": [409, 429, 500, 502, 503, 504],
}
for k, v in retry_defaults.items():
assert getattr(URLFile.pool_manager().connection_pool_kw["retries"], k) == v
# ensure caching on by default and cache dir gets created
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
if os.path.exists(Paths.download_cache_root()):
shutil.rmtree(Paths.download_cache_root())
URLFile(f"{host}/test.txt").get_length()
URLFile(f"{host}/test.txt").read()
assert os.path.exists(Paths.download_cache_root())
def compare_loads(self, url, start=0, length=None):
"""Compares range between cached and non cached version"""
file_cached = URLFile(url, cache=True)
file_downloaded = URLFile(url, cache=False)
file_cached.seek(start)
file_downloaded.seek(start)
assert file_cached.get_length() == file_downloaded.get_length()
assert length + start if length is not None else 0 <= file_downloaded.get_length()
response_cached = file_cached.read(ll=length)
response_downloaded = file_downloaded.read(ll=length)
assert response_cached == response_downloaded
# Now test with cache in place
file_cached = URLFile(url, cache=True)
file_cached.seek(start)
response_cached = file_cached.read(ll=length)
assert file_cached.get_length() == file_downloaded.get_length()
assert response_cached == response_downloaded
def test_small_file(self):
# Make sure we don't force cache
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
small_file_url = "https://raw.githubusercontent.com/commaai/openpilot/master/docs/SAFETY.md"
# If you want large file to be larger than a chunk
# large_file_url = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/fcamera.hevc"
# Load full small file
self.compare_loads(small_file_url)
file_small = URLFile(small_file_url)
length = file_small.get_length()
self.compare_loads(small_file_url, length - 100, 100)
self.compare_loads(small_file_url, 50, 100)
# Load small file 100 bytes at a time
for i in range(length // 100):
self.compare_loads(small_file_url, 100 * i, 100)
def test_large_file(self):
large_file_url = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/qlog.bz2"
# Load the end 100 bytes of both files
file_large = URLFile(large_file_url)
length = file_large.get_length()
self.compare_loads(large_file_url, length - 100, 100)
self.compare_loads(large_file_url)
@pytest.mark.parametrize("cache_enabled", [True, False])
def test_recover_from_missing_file(self, host, cache_enabled):
if cache_enabled:
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
else:
os.environ["DISABLE_FILEREADER_CACHE"] = "1"
file_url = f"{host}/test.png"
CachingTestRequestHandler.FILE_EXISTS = False
length = URLFile(file_url).get_length()
assert length == -1
CachingTestRequestHandler.FILE_EXISTS = True
length = URLFile(file_url).get_length()
assert length == 4
class TestCache:
def test_concurrent_prune_cache(self, tmp_path):
context = multiprocessing.get_context("fork")
barrier = context.Barrier(16)
processes = [context.Process(target=concurrent_prune_cache, args=(f"{tmp_path}/", f"entry_{i}", barrier)) for i in range(16)]
for process in processes:
process.start()
for process in processes:
process.join(10)
assert process.exitcode == 0
manifest = set()
for line in (tmp_path / "manifest.txt").read_text().splitlines():
parts = line.split()
if len(parts) == 2:
manifest.add(parts[0])
assert manifest == {f"entry_{i}" for i in range(16)}
def test_prune_cache(self, monkeypatch):
with tempfile.TemporaryDirectory() as tmpdir:
monkeypatch.setattr(Paths, 'download_cache_root', staticmethod(lambda: tmpdir + "/"))
# setup test files and manifest
manifest_lines = []
for i in range(3):
fname = f"hash_{i}"
with open(tmpdir + "/" + fname, "wb") as f:
f.truncate(1000)
manifest_lines.append(f"{fname} {1000 + i}")
with open(tmpdir + "/manifest.txt", "w") as f:
f.write('\n'.join(manifest_lines))
# under limit, shouldn't prune
assert len(os.listdir(tmpdir)) == 4
prune_cache()
assert len([name for name in os.listdir(tmpdir) if name != "manifest.lock"]) == 4
# set a tiny cache limit to force eviction (1.5 chunks worth)
monkeypatch.setattr(url_file_module, 'CACHE_SIZE', url_file_module.CHUNK_SIZE + url_file_module.CHUNK_SIZE // 2)
# prune_cache should evict oldest files to get under limit
prune_cache()
remaining = [name for name in os.listdir(tmpdir) if name != "manifest.lock"]
# should have evicted at least one file + manifest
assert len(remaining) < 4
# newest file should remain
assert manifest_lines[2].split()[0] in remaining

176
iqpilot/tools/lib/tests/test_logreader.py Normal file
View File

@@ -0,0 +1,176 @@
import capnp
import contextlib
import shutil
import tempfile
import os
import pytest
import requests
from parameterized import parameterized
from iqpilot.cereal import log as capnp_log
from iqpilot.tools.lib.logreader import InternalUnavailableException, LogReader, parse_indirect
from iqpilot.tools.lib.route import SegmentRange
from iqpilot.tools.lib.url_file import URLFileException
NUM_SEGS = 17 # number of segments in the test route
ALL_SEGS = list(range(NUM_SEGS))
TEST_ROUTE = "344c5c15b34f2d8a/2024-01-03--09-37-12"
QLOG_FILE = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/qlog.bz2"
@contextlib.contextmanager
def setup_source_scenario(mocker, is_internal=False):
internal_source_mock = mocker.patch("iqpilot.tools.lib.logreader.internal_source")
internal_source_mock.__name__ = internal_source_mock._mock_name
openpilotci_source_mock = mocker.patch("iqpilot.tools.lib.logreader.openpilotci_source")
openpilotci_source_mock.__name__ = openpilotci_source_mock._mock_name
comma_api_source_mock = mocker.patch("iqpilot.tools.lib.logreader.comma_api_source")
comma_api_source_mock.__name__ = comma_api_source_mock._mock_name
if is_internal:
internal_source_mock.return_value = {3: QLOG_FILE}
else:
internal_source_mock.side_effect = InternalUnavailableException
openpilotci_source_mock.return_value = {}
comma_api_source_mock.return_value = {3: QLOG_FILE}
yield
class TestLogReader:
@pytest.mark.parametrize(("identifier", "expected"), [
(f"{TEST_ROUTE}", ALL_SEGS),
(f"{TEST_ROUTE.replace('/', '|')}", ALL_SEGS),
(f"{TEST_ROUTE}--0", [0]),
(f"{TEST_ROUTE}--5", [5]),
(f"{TEST_ROUTE}/0", [0]),
(f"{TEST_ROUTE}/5", [5]),
(f"{TEST_ROUTE}/0:10", ALL_SEGS[0:10]),
(f"{TEST_ROUTE}/0:0", []),
(f"{TEST_ROUTE}/4:6", ALL_SEGS[4:6]),
(f"{TEST_ROUTE}/0:-1", ALL_SEGS[0:-1]),
(f"{TEST_ROUTE}/:5", ALL_SEGS[:5]),
(f"{TEST_ROUTE}/2:", ALL_SEGS[2:]),
(f"{TEST_ROUTE}/2:-1", ALL_SEGS[2:-1]),
(f"{TEST_ROUTE}/-1", [ALL_SEGS[-1]]),
(f"{TEST_ROUTE}/-2", [ALL_SEGS[-2]]),
(f"{TEST_ROUTE}/-2:-1", ALL_SEGS[-2:-1]),
(f"{TEST_ROUTE}/-4:-2", ALL_SEGS[-4:-2]),
(f"{TEST_ROUTE}/:10:2", ALL_SEGS[:10:2]),
(f"{TEST_ROUTE}/5::2", ALL_SEGS[5::2]),
(f"https://useradmin.comma.ai/?onebox={TEST_ROUTE}", ALL_SEGS),
(f"https://useradmin.comma.ai/?onebox={TEST_ROUTE.replace('/', '|')}", ALL_SEGS),
(f"https://useradmin.comma.ai/?onebox={TEST_ROUTE.replace('/', '%7C')}", ALL_SEGS),
])
def test_indirect_parsing(self, identifier, expected, mocker):
mocker.patch("iqpilot.tools.lib.route.get_max_seg_number_cached", return_value=NUM_SEGS - 1)
parsed = parse_indirect(identifier)
sr = SegmentRange(parsed)
assert list(sr.seg_idxs) == expected, identifier
@parameterized.expand([
(f"{TEST_ROUTE}", f"{TEST_ROUTE}"),
(f"{TEST_ROUTE.replace('/', '|')}", f"{TEST_ROUTE}"),
(f"{TEST_ROUTE}--5", f"{TEST_ROUTE}/5"),
(f"{TEST_ROUTE}/0/q", f"{TEST_ROUTE}/0/q"),
(f"{TEST_ROUTE}/5:6/r", f"{TEST_ROUTE}/5:6/r"),
(f"{TEST_ROUTE}/5", f"{TEST_ROUTE}/5"),
])
def test_canonical_name(self, identifier, expected):
sr = SegmentRange(identifier)
assert str(sr) == expected
@pytest.mark.parametrize("cache_enabled", [True, False])
def test_direct_parsing(self, mocker, cache_enabled):
file_exists_mock = mocker.patch("iqpilot.tools.lib.filereader.file_exists")
if cache_enabled:
os.environ.pop("DISABLE_FILEREADER_CACHE", None)
else:
os.environ["DISABLE_FILEREADER_CACHE"] = "1"
qlog = tempfile.NamedTemporaryFile(mode='wb', delete=False)
with requests.get(QLOG_FILE, stream=True) as r:
with qlog as f:
shutil.copyfileobj(r.raw, f)
for f in [QLOG_FILE, qlog.name]:
l = len(list(LogReader(f)))
assert l > 100
with pytest.raises(URLFileException) if not cache_enabled else pytest.raises(AssertionError):
l = len(list(LogReader(QLOG_FILE.replace("/3/", "/200/"))))
# file_exists should not be called for direct files
assert file_exists_mock.call_count == 0
@parameterized.expand([
(f"{TEST_ROUTE}///",),
(f"{TEST_ROUTE}---",),
(f"{TEST_ROUTE}/-4:--2",),
(f"{TEST_ROUTE}/-a",),
(f"{TEST_ROUTE}/j",),
(f"{TEST_ROUTE}/0:1:2:3",),
(f"{TEST_ROUTE}/:::3",),
(f"{TEST_ROUTE}3",),
(f"{TEST_ROUTE}-3",),
(f"{TEST_ROUTE}--3a",),
])
def test_bad_ranges(self, segment_range):
with pytest.raises(AssertionError):
_ = SegmentRange(segment_range).seg_idxs
@pytest.mark.parametrize("segment_range, api_call", [
(f"{TEST_ROUTE}/0", False),
(f"{TEST_ROUTE}/:2", False),
(f"{TEST_ROUTE}/0:", True),
(f"{TEST_ROUTE}/-1", True),
(f"{TEST_ROUTE}", True),
])
def test_slicing_api_call(self, mocker, segment_range, api_call):
max_seg_mock = mocker.patch("iqpilot.tools.lib.route.get_max_seg_number_cached")
max_seg_mock.return_value = NUM_SEGS
_ = SegmentRange(segment_range).seg_idxs
assert api_call == max_seg_mock.called
@pytest.mark.parametrize("is_internal", [True, False])
def test_auto_source_scenarios(self, mocker, is_internal):
lr = LogReader(QLOG_FILE)
qlog_len = len(list(lr))
with setup_source_scenario(mocker, is_internal=is_internal):
lr = LogReader(f"{TEST_ROUTE}/3/q")
log_len = len(list(lr))
assert qlog_len == log_len
def test_only_union_types(self):
with tempfile.NamedTemporaryFile() as qlog:
# write valid Event messages
num_msgs = 100
with open(qlog.name, "wb") as f:
f.write(b"".join(capnp_log.Event.new_message().to_bytes() for _ in range(num_msgs)))
msgs = list(LogReader(qlog.name))
assert len(msgs) == num_msgs
[m.which() for m in msgs]
# append non-union Event message
event_msg = capnp_log.Event.new_message()
non_union_bytes = bytearray(event_msg.to_bytes())
non_union_bytes[event_msg.total_size.word_count * 8] = 0xff # set discriminant value out of range using Event word offset
with open(qlog.name, "ab") as f:
f.write(non_union_bytes)
# ensure new message is added, but is not a union type
msgs = list(LogReader(qlog.name))
assert len(msgs) == num_msgs + 1
with pytest.raises((capnp.KjException, RuntimeError)):
[m.which() for m in msgs]
# should not be added when only_union_types=True
msgs = list(LogReader(qlog.name, only_union_types=True))
assert len(msgs) == num_msgs
[m.which() for m in msgs]

27
iqpilot/tools/lib/tests/test_route_library.py Normal file
View File

@@ -0,0 +1,27 @@
from collections import namedtuple
from iqpilot.tools.lib.route import SegmentName
class TestRouteLibrary:
def test_segment_name_formats(self):
Case = namedtuple('Case', ['input', 'expected_route', 'expected_segment_num', 'expected_data_dir'])
cases = [ Case("a2a0ccea32023010|2023-07-27--13-01-19", "a2a0ccea32023010|2023-07-27--13-01-19", -1, None),
Case("a2a0ccea32023010/2023-07-27--13-01-19--1", "a2a0ccea32023010|2023-07-27--13-01-19", 1, None),
Case("a2a0ccea32023010|2023-07-27--13-01-19/2", "a2a0ccea32023010|2023-07-27--13-01-19", 2, None),
Case("a2a0ccea32023010/2023-07-27--13-01-19/3", "a2a0ccea32023010|2023-07-27--13-01-19", 3, None),
Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19", "a2a0ccea32023010|2023-07-27--13-01-19", -1, "/data/media/0/realdata"),
Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19--1", "a2a0ccea32023010|2023-07-27--13-01-19", 1, "/data/media/0/realdata"),
Case("/data/media/0/realdata/a2a0ccea32023010|2023-07-27--13-01-19/2", "a2a0ccea32023010|2023-07-27--13-01-19", 2, "/data/media/0/realdata") ]
def _validate(case):
route_or_segment_name = case.input
s = SegmentName(route_or_segment_name, allow_route_name=True)
assert str(s.route_name) == case.expected_route
assert s.segment_num == case.expected_segment_num
assert s.data_dir == case.expected_data_dir
for case in cases:
_validate(case)

255
iqpilot/tools/lib/url_file.py Normal file
View File

@@ -0,0 +1,255 @@
import logging
import fcntl
import os
import re
import socket
import time
from hashlib import md5
from urllib3 import PoolManager, Retry
from urllib3.response import BaseHTTPResponse
from urllib3.util import Timeout
from iqpilot.common.utils import atomic_write
from iqpilot.system.hardware.hw import Paths
from urllib3.exceptions import MaxRetryError
# Cache chunk size
K = 1000
CHUNK_SIZE = 1000 * K
CACHE_SIZE = 10 * 1024 * 1024 * 1024 # total cache size in GB
logging.getLogger("urllib3").setLevel(logging.WARNING)
USER_AGENT = os.getenv("IQPILOT_HTTP_USER_AGENT", "Mozilla/5.0 (Macintosh; Intel Mac OS X 10_15_7) "
"AppleWebKit/537.36 (KHTML, like Gecko) Chrome/128.0.0.0 Safari/537.36")
def _env_int(name: str, default: int) -> int:
raw = os.getenv(name)
if raw is None:
return default
try:
value = int(raw)
return value if value > 0 else default
except ValueError:
return default
def hash_url(link: str) -> str:
return md5((link.split("?")[0]).encode('utf-8')).hexdigest()
def prune_cache(new_entry: str | None = None) -> None:
"""Evicts oldest cache files (LRU) until cache is under the size limit."""
cache_root = Paths.download_cache_root()
os.makedirs(cache_root, exist_ok=True)
manifest_path = os.path.join(cache_root, "manifest.txt")
with open(os.path.join(cache_root, "manifest.lock"), "w") as lock_file:
fcntl.flock(lock_file, fcntl.LOCK_EX)
manifest = {}
try:
with open(manifest_path) as f:
manifest = {parts[0]: int(parts[1]) for line in f if (parts := line.strip().split()) and len(parts) == 2}
except FileNotFoundError:
pass
if new_entry:
manifest[new_entry] = int(time.time()) # noqa: TID251
sorted_items = sorted(manifest.items(), key=lambda x: x[1])
while len(manifest) * CHUNK_SIZE > CACHE_SIZE and sorted_items:
key, _ = sorted_items.pop(0)
try:
os.remove(os.path.join(cache_root, key))
except OSError:
pass
manifest.pop(key, None)
with atomic_write(manifest_path, mode="w", overwrite=True) as f:
f.write('\n'.join(f"{k} {v}" for k, v in manifest.items()))
class URLFileException(Exception):
pass
class URLFile:
_pool_manager: PoolManager | None = None
@staticmethod
def reset() -> None:
URLFile._pool_manager = None
@staticmethod
def pool_manager() -> PoolManager:
if URLFile._pool_manager is None:
socket_options = [(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)]
retries = Retry(
total=_env_int("URLFILE_RETRIES_TOTAL", 6),
connect=_env_int("URLFILE_RETRIES_CONNECT", 6),
read=_env_int("URLFILE_RETRIES_READ", 6),
backoff_factor=float(os.getenv("URLFILE_RETRIES_BACKOFF", "0.75")),
status_forcelist=[409, 429, 500, 502, 503, 504],
)
URLFile._pool_manager = PoolManager(num_pools=10, maxsize=100, socket_options=socket_options, retries=retries)
return URLFile._pool_manager
def __init__(self, url: str, timeout: int = 10, cache: bool | None = None):
self._url = url
connect_timeout = _env_int("URLFILE_CONNECT_TIMEOUT", min(timeout, 10))
read_timeout = _env_int("URLFILE_READ_TIMEOUT", max(timeout, 30))
total_timeout = _env_int("URLFILE_TOTAL_TIMEOUT", max(read_timeout * 4, 180))
self._timeout = Timeout(connect=connect_timeout, read=read_timeout, total=total_timeout)
self._pos = 0
self._length: int | None = None
# Caching enabled by default, can be disabled with DISABLE_FILEREADER_CACHE=1, or overwritten by the cache input
self._force_download = int(os.environ.get("DISABLE_FILEREADER_CACHE", "0")) == 1
if cache is not None:
self._force_download = not cache
if not self._force_download:
os.makedirs(Paths.download_cache_root(), exist_ok=True)
def __enter__(self):
return self
def __exit__(self, exc_type, exc_value, traceback) -> None:
pass
def _request(self, method: str, url: str, headers: dict[str, str] | None = None) -> BaseHTTPResponse:
# the data host is behind cloudflare, which answers a default urllib3 agent
# with a 1010 block. It reads as 403 Forbidden on a correctly signed url, so
# it looks like an auth problem and is not one.
headers = {**(headers or {}), "User-Agent": USER_AGENT}
try:
return URLFile.pool_manager().request(method, url, timeout=self._timeout, headers=headers)
except MaxRetryError as e:
raise URLFileException(f"Failed to {method} {url}: {e}") from e
def get_length_online(self) -> int:
response = self._request('HEAD', self._url)
try:
if not (200 <= response.status <= 299):
return -1
length = response.headers.get('content-length', 0)
return int(length)
finally:
response.release_conn()
def get_length(self) -> int:
if self._length is not None:
return self._length
file_length_path = os.path.join(Paths.download_cache_root(), hash_url(self._url) + "_length")
if not self._force_download and os.path.exists(file_length_path):
with open(file_length_path) as file_length:
content = file_length.read()
self._length = int(content)
return self._length
self._length = self.get_length_online()
if not self._force_download and self._length != -1:
with atomic_write(file_length_path, mode="w", overwrite=True) as file_length:
file_length.write(str(self._length))
return self._length
def read(self, ll: int | None = None) -> bytes:
if self._force_download:
return self.read_aux(ll=ll)
file_begin = self._pos
file_end = self._pos + ll if ll is not None else self.get_length()
assert file_end != -1, f"Remote file is empty or doesn't exist: {self._url}"
# We have to align with chunks we store. Position is the begginiing of the latest chunk that starts before or at our file
position = (file_begin // CHUNK_SIZE) * CHUNK_SIZE
response = b""
while True:
self._pos = position
chunk_number = self._pos / CHUNK_SIZE
file_name = hash_url(self._url) + "_" + str(chunk_number)
full_path = os.path.join(Paths.download_cache_root(), str(file_name))
data = None
# If we don't have a file, download it
if not os.path.exists(full_path):
data = self.read_aux(ll=CHUNK_SIZE)
with atomic_write(full_path, mode="wb", overwrite=True) as new_cached_file:
new_cached_file.write(data)
prune_cache(file_name)
else:
with open(full_path, "rb") as cached_file:
data = cached_file.read()
response += data[max(0, file_begin - position): min(CHUNK_SIZE, file_end - position)]
position += CHUNK_SIZE
if position >= file_end:
self._pos = file_end
return response
def read_aux(self, ll: int | None = None) -> bytes:
if ll is None:
length = self.get_length()
if length == -1:
raise URLFileException(f"Remote file is empty or doesn't exist: {self._url}")
end = length
else:
end = self._pos + ll
data = self.get_multi_range([(self._pos, end)])
self._pos += len(data[0])
return data[0]
def get_multi_range(self, ranges: list[tuple[int, int]]) -> list[bytes]:
# HTTP range requests are inclusive
assert all(e > s for s, e in ranges), "Range end must be greater than start"
rs = [f"{s}-{e-1}" for s, e in ranges if e > s]
r = self._request("GET", self._url, headers={"Range": "bytes=" + ",".join(rs)})
if r.status not in [200, 206]:
raise URLFileException(f"Expected 206 or 200 response {r.status} ({self._url})")
ctype = (r.headers.get("content-type") or "").lower()
if "multipart/byteranges" not in ctype:
return [r.data,]
m = re.search(r'boundary="?([^";]+)"?', ctype)
if not m:
raise URLFileException(f"Missing multipart boundary ({self._url})")
boundary = m.group(1).encode()
parts = []
for chunk in r.data.split(b"--" + boundary):
if b"\r\n\r\n" not in chunk:
continue
payload = chunk.split(b"\r\n\r\n", 1)[1].rstrip(b"\r\n")
if payload and payload != b"--":
parts.append(payload)
if len(parts) != len(ranges):
raise URLFileException(f"Expected {len(ranges)} parts, got {len(parts)} ({self._url})")
return parts
def seekable(self) -> bool:
return True
def seek(self, pos: int, whence: int = 0) -> int:
pos = int(pos)
if whence == os.SEEK_SET:
self._pos = pos
elif whence == os.SEEK_CUR:
self._pos += pos
elif whence == os.SEEK_END:
length = self.get_length()
assert length != -1, "Cannot seek from end on unknown length file"
self._pos = length + pos
else:
raise URLFileException("Invalid whence value")
return self._pos
def tell(self) -> int:
return self._pos
@property
def name(self) -> str:
return self._url
os.register_at_fork(after_in_child=URLFile.reset)

311
iqpilot/tools/lib/vidindex.py Executable file
View File

@@ -0,0 +1,311 @@
#!/usr/bin/env python3
import argparse
import os
import struct
from enum import IntEnum
from iqpilot.tools.lib.filereader import FileReader
DEBUG = int(os.getenv("DEBUG", "0"))
# compare to ffmpeg parsing
# ffmpeg -i <input.hevc> -c copy -bsf:v trace_headers -f null - 2>&1 | grep -B4 -A32 '] 0 '
# H.265 specification
# https://www.itu.int/rec/dologin_pub.asp?lang=e&id=T-REC-H.265-201802-S!!PDF-E&type=items
NAL_UNIT_START_CODE = b"\x00\x00\x01"
NAL_UNIT_START_CODE_SIZE = len(NAL_UNIT_START_CODE)
NAL_UNIT_HEADER_SIZE = 2
class HevcNalUnitType(IntEnum):
TRAIL_N = 0 # RBSP structure: slice_segment_layer_rbsp( )
TRAIL_R = 1 # RBSP structure: slice_segment_layer_rbsp( )
TSA_N = 2 # RBSP structure: slice_segment_layer_rbsp( )
TSA_R = 3 # RBSP structure: slice_segment_layer_rbsp( )
STSA_N = 4 # RBSP structure: slice_segment_layer_rbsp( )
STSA_R = 5 # RBSP structure: slice_segment_layer_rbsp( )
RADL_N = 6 # RBSP structure: slice_segment_layer_rbsp( )
RADL_R = 7 # RBSP structure: slice_segment_layer_rbsp( )
RASL_N = 8 # RBSP structure: slice_segment_layer_rbsp( )
RASL_R = 9 # RBSP structure: slice_segment_layer_rbsp( )
RSV_VCL_N10 = 10
RSV_VCL_R11 = 11
RSV_VCL_N12 = 12
RSV_VCL_R13 = 13
RSV_VCL_N14 = 14
RSV_VCL_R15 = 15
BLA_W_LP = 16 # RBSP structure: slice_segment_layer_rbsp( )
BLA_W_RADL = 17 # RBSP structure: slice_segment_layer_rbsp( )
BLA_N_LP = 18 # RBSP structure: slice_segment_layer_rbsp( )
IDR_W_RADL = 19 # RBSP structure: slice_segment_layer_rbsp( )
IDR_N_LP = 20 # RBSP structure: slice_segment_layer_rbsp( )
CRA_NUT = 21 # RBSP structure: slice_segment_layer_rbsp( )
RSV_IRAP_VCL22 = 22
RSV_IRAP_VCL23 = 23
RSV_VCL24 = 24
RSV_VCL25 = 25
RSV_VCL26 = 26
RSV_VCL27 = 27
RSV_VCL28 = 28
RSV_VCL29 = 29
RSV_VCL30 = 30
RSV_VCL31 = 31
VPS_NUT = 32 # RBSP structure: video_parameter_set_rbsp( )
SPS_NUT = 33 # RBSP structure: seq_parameter_set_rbsp( )
PPS_NUT = 34 # RBSP structure: pic_parameter_set_rbsp( )
AUD_NUT = 35
EOS_NUT = 36
EOB_NUT = 37
FD_NUT = 38
PREFIX_SEI_NUT = 39
SUFFIX_SEI_NUT = 40
RSV_NVCL41 = 41
RSV_NVCL42 = 42
RSV_NVCL43 = 43
RSV_NVCL44 = 44
RSV_NVCL45 = 45
RSV_NVCL46 = 46
RSV_NVCL47 = 47
UNSPEC48 = 48
UNSPEC49 = 49
UNSPEC50 = 50
UNSPEC51 = 51
UNSPEC52 = 52
UNSPEC53 = 53
UNSPEC54 = 54
UNSPEC55 = 55
UNSPEC56 = 56
UNSPEC57 = 57
UNSPEC58 = 58
UNSPEC59 = 59
UNSPEC60 = 60
UNSPEC61 = 61
UNSPEC62 = 62
UNSPEC63 = 63
# B.2.2 Byte stream NAL unit semantics
# - The nal_unit_type within the nal_unit( ) syntax structure is equal to VPS_NUT, SPS_NUT or PPS_NUT.
# - The byte stream NAL unit syntax structure contains the first NAL unit of an access unit in decoding
# order, as specified in clause 7.4.2.4.4.
HEVC_PARAMETER_SET_NAL_UNITS = (
HevcNalUnitType.VPS_NUT,
HevcNalUnitType.SPS_NUT,
HevcNalUnitType.PPS_NUT,
)
# 3.29 coded slice segment NAL unit: A NAL unit that has nal_unit_type in the range of TRAIL_N to RASL_R,
# inclusive, or in the range of BLA_W_LP to RSV_IRAP_VCL23, inclusive, which indicates that the NAL unit
# contains a coded slice segment
HEVC_CODED_SLICE_SEGMENT_NAL_UNITS = (
HevcNalUnitType.TRAIL_N,
HevcNalUnitType.TRAIL_R,
HevcNalUnitType.TSA_N,
HevcNalUnitType.TSA_R,
HevcNalUnitType.STSA_N,
HevcNalUnitType.STSA_R,
HevcNalUnitType.RADL_N,
HevcNalUnitType.RADL_R,
HevcNalUnitType.RASL_N,
HevcNalUnitType.RASL_R,
HevcNalUnitType.BLA_W_LP,
HevcNalUnitType.BLA_W_RADL,
HevcNalUnitType.BLA_N_LP,
HevcNalUnitType.IDR_W_RADL,
HevcNalUnitType.IDR_N_LP,
HevcNalUnitType.CRA_NUT,
)
class VideoFileInvalid(Exception):
pass
def get_ue(dat: bytes, start_idx: int, skip_bits: int) -> tuple[int, int]:
prefix_val = 0
prefix_len = 0
suffix_val = 0
suffix_len = 0
i = start_idx
while i < len(dat):
j = 7
while j >= 0:
if skip_bits > 0:
skip_bits -= 1
elif prefix_val == 0:
prefix_val = (dat[i] >> j) & 1
prefix_len += 1
else:
suffix_val = (suffix_val << 1) | ((dat[i] >> j) & 1)
suffix_len += 1
j -= 1
if prefix_val == 1 and prefix_len - 1 == suffix_len:
val = int(2**(prefix_len-1) - 1 + suffix_val)
size = prefix_len + suffix_len
return val, size
i += 1
raise VideoFileInvalid("invalid exponential-golomb code")
def require_nal_unit_start(dat: bytes, nal_unit_start: int) -> None:
if nal_unit_start < 1:
raise ValueError("start index must be greater than zero")
if dat[nal_unit_start:nal_unit_start + NAL_UNIT_START_CODE_SIZE] != NAL_UNIT_START_CODE:
raise VideoFileInvalid("data must begin with start code")
def get_hevc_nal_unit_length(dat: bytes, nal_unit_start: int) -> int:
try:
pos = dat.index(NAL_UNIT_START_CODE, nal_unit_start + NAL_UNIT_START_CODE_SIZE)
except ValueError:
pos = -1
# length of NAL unit is byte count up to next NAL unit start index
nal_unit_len = (pos if pos != -1 else len(dat)) - nal_unit_start
if DEBUG:
print(" nal_unit_len:", nal_unit_len)
return nal_unit_len
def get_hevc_nal_unit_type(dat: bytes, nal_unit_start: int) -> HevcNalUnitType:
# 7.3.1.2 NAL unit header syntax
# nal_unit_header( ) { // descriptor
# forbidden_zero_bit f(1)
# nal_unit_type u(6)
# nuh_layer_id u(6)
# nuh_temporal_id_plus1 u(3)
# }
header_start = nal_unit_start + NAL_UNIT_START_CODE_SIZE
nal_unit_header = dat[header_start:header_start + NAL_UNIT_HEADER_SIZE]
if len(nal_unit_header) != 2:
raise VideoFileInvalid("data to short to contain nal unit header")
nal_unit_type = HevcNalUnitType((nal_unit_header[0] >> 1) & 0x3F)
if DEBUG:
print(" nal_unit_type:", nal_unit_type.name, f"({nal_unit_type.value})")
return nal_unit_type
def get_hevc_slice_type(dat: bytes, nal_unit_start: int, nal_unit_type: HevcNalUnitType) -> tuple[int, bool]:
# 7.3.2.9 Slice segment layer RBSP syntax
# slice_segment_layer_rbsp( ) {
# slice_segment_header( )
# slice_segment_data( )
# rbsp_slice_segment_trailing_bits( )
# }
# ...
# 7.3.6.1 General slice segment header syntax
# slice_segment_header( ) { // descriptor
# first_slice_segment_in_pic_flag u(1)
# if( nal_unit_type >= BLA_W_LP && nal_unit_type <= RSV_IRAP_VCL23 )
# no_output_of_prior_pics_flag u(1)
# slice_pic_parameter_set_id ue(v)
# if( !first_slice_segment_in_pic_flag ) {
# if( dependent_slice_segments_enabled_flag )
# dependent_slice_segment_flag u(1)
# slice_segment_address u(v)
# }
# if( !dependent_slice_segment_flag ) {
# for( i = 0; i < num_extra_slice_header_bits; i++ )
# slice_reserved_flag[ i ] u(1)
# slice_type ue(v)
# ...
rbsp_start = nal_unit_start + NAL_UNIT_START_CODE_SIZE + NAL_UNIT_HEADER_SIZE
skip_bits = 0
# 7.4.7.1 General slice segment header semantics
# first_slice_segment_in_pic_flag equal to 1 specifies that the slice segment is the first slice segment of the picture in
# decoding order. first_slice_segment_in_pic_flag equal to 0 specifies that the slice segment is not the first slice segment
# of the picture in decoding order.
is_first_slice = dat[rbsp_start] >> 7 & 1 == 1
if not is_first_slice:
# TODO: parse dependent_slice_segment_flag and slice_segment_address and get real slice_type
# for now since we don't use it return -1 for slice_type
return (-1, is_first_slice)
skip_bits += 1 # skip past first_slice_segment_in_pic_flag
if nal_unit_type >= HevcNalUnitType.BLA_W_LP and nal_unit_type <= HevcNalUnitType.RSV_IRAP_VCL23:
# 7.4.7.1 General slice segment header semantics
# no_output_of_prior_pics_flag affects the output of previously-decoded pictures in the decoded picture buffer after the
# decoding of an IDR or a BLA picture that is not the first picture in the bitstream as specified in Annex C.
skip_bits += 1 # skip past no_output_of_prior_pics_flag
# 7.4.7.1 General slice segment header semantics
# slice_pic_parameter_set_id specifies the value of pps_pic_parameter_set_id for the PPS in use.
# The value of slice_pic_parameter_set_id shall be in the range of 0 to 63, inclusive.
_, size = get_ue(dat, rbsp_start, skip_bits)
skip_bits += size # skip past slice_pic_parameter_set_id
# 7.4.3.3.1 General picture parameter set RBSP semanal_unit_lenntics
# num_extra_slice_header_bits specifies the number of extra slice header bits that are present in the slice header RBSP
# for coded pictures referring to the PPS. The value of num_extra_slice_header_bits shall be in the range of 0 to 2, inclusive,
# in bitstreams conforming to this version of this Specification. Other values for num_extra_slice_header_bits are reserved
# for future use by ITU-T | ISO/IEC. However, decoders shall allow num_extra_slice_header_bits to have any value.
# TODO: get from PPS_NUT pic_parameter_set_rbsp( ) for corresponding slice_pic_parameter_set_id
num_extra_slice_header_bits = 0
skip_bits += num_extra_slice_header_bits
# 7.4.7.1 General slice segment header semantics
# slice_type specifies the coding type of the slice according to Table 7-7.
# Table 7-7 - Name association to slice_type
# slice_type | Name of slice_type
# 0 | B (B slice)
# 1 | P (P slice)
# 2 | I (I slice)
# unsigned integer 0-th order Exp-Golomb-coded syntax element with the left bit first
slice_type, _ = get_ue(dat, rbsp_start, skip_bits)
if DEBUG:
print(" slice_type:", slice_type, f"(first slice: {is_first_slice})")
if slice_type > 2:
raise VideoFileInvalid("slice_type must be 0, 1, or 2")
return slice_type, is_first_slice
def hevc_index(hevc_file_name: str, allow_corrupt: bool=False) -> tuple[list, int, bytes]:
with FileReader(hevc_file_name) as f:
dat = f.read()
if len(dat) < NAL_UNIT_START_CODE_SIZE + 1:
raise VideoFileInvalid("data is too short")
if dat[0] != 0x00:
raise VideoFileInvalid("first byte must be 0x00")
prefix_dat = b""
frame_types = list()
i = 1 # skip past first byte 0x00
try:
while i < len(dat):
require_nal_unit_start(dat, i)
nal_unit_len = get_hevc_nal_unit_length(dat, i)
nal_unit_type = get_hevc_nal_unit_type(dat, i)
if nal_unit_type in HEVC_PARAMETER_SET_NAL_UNITS:
prefix_dat += dat[i:i+nal_unit_len]
elif nal_unit_type in HEVC_CODED_SLICE_SEGMENT_NAL_UNITS:
slice_type, is_first_slice = get_hevc_slice_type(dat, i, nal_unit_type)
if is_first_slice:
frame_types.append((slice_type, i))
i += nal_unit_len
except Exception as e:
if not allow_corrupt:
raise
print(f"ERROR: NAL unit skipped @ {i}\n", str(e))
return frame_types, len(dat), prefix_dat
def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("input_file", type=str)
parser.add_argument("output_prefix_file", type=str)
parser.add_argument("output_index_file", type=str)
args = parser.parse_args()
frame_types, dat_len, prefix_dat = hevc_index(args.input_file)
with open(args.output_prefix_file, "wb") as f:
f.write(prefix_dat)
with open(args.output_index_file, "wb") as f:
for ft, fp in frame_types:
f.write(struct.pack("<II", ft, fp))
f.write(struct.pack("<II", 0xFFFFFFFF, dat_len))
if __name__ == "__main__":
main()

60
iqpilot/tools/mac_setup.sh Executable file
View File

@@ -0,0 +1,60 @@
#!/usr/bin/env bash
set -e
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
ROOT="$(cd $DIR/../../ && pwd)"
ARCH=$(uname -m)
# homebrew update is slow
export HOMEBREW_NO_AUTO_UPDATE=1
if [[ $SHELL == "/bin/zsh" ]]; then
RC_FILE="$HOME/.zshrc"
elif [[ $SHELL == "/bin/bash" ]]; then
RC_FILE="$HOME/.bash_profile"
fi
# Install brew if required
if [[ $(command -v brew) == "" ]]; then
echo "Installing Homebrew"
/bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
echo "[ ] installed brew t=$SECONDS"
# make brew available now
if [[ $ARCH == "x86_64" ]]; then
echo 'eval "$(/usr/local/bin/brew shellenv)"' >> $RC_FILE
eval "$(/usr/local/bin/brew shellenv)"
else
echo 'eval "$(/opt/homebrew/bin/brew shellenv)"' >> $RC_FILE
eval "$(/opt/homebrew/bin/brew shellenv)"
fi
else
brew up
fi
brew bundle --file=$DIR/Brewfile
echo "[ ] finished brew install t=$SECONDS"
BREW_PREFIX=$(brew --prefix)
# archive backend tools for pip dependencies
export LDFLAGS="$LDFLAGS -L${BREW_PREFIX}/opt/zlib/lib"
export LDFLAGS="$LDFLAGS -L${BREW_PREFIX}/opt/bzip2/lib"
export CPPFLAGS="$CPPFLAGS -I${BREW_PREFIX}/opt/zlib/include"
export CPPFLAGS="$CPPFLAGS -I${BREW_PREFIX}/opt/bzip2/include"
# pycurl curl/openssl backend dependencies
export LDFLAGS="$LDFLAGS -L${BREW_PREFIX}/opt/openssl@3/lib"
export CPPFLAGS="$CPPFLAGS -I${BREW_PREFIX}/opt/openssl@3/include"
export PYCURL_CURL_CONFIG=/usr/bin/curl-config
export PYCURL_SSL_LIBRARY=openssl
# install python dependencies
$DIR/install_python_dependencies.sh
echo "[ ] installed python dependencies t=$SECONDS"
echo
echo "---- OPENPILOT SETUP DONE ----"
echo "Open a new shell or configure your active shell env by running:"
echo "source $RC_FILE"

1
iqpilot/tools/maneuvers/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
/reports/

87
iqpilot/tools/maneuvers/README-lateral.md Normal file
View File

@@ -0,0 +1,87 @@
# Lateral Maneuvers Testing Tool
> [!WARNING]
> Use caution when using this tool.
Test your vehicle's lateral control tuning with this tool. The tool will test the vehicle's ability to follow a few lateral maneuvers and includes a tool to generate a report from the route.
## Instructions
1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
2. The full maneuver suite runs at 20 and 30 mph.
3. Enable "Lateral Maneuver Mode" in Settings > Developer on the device while offroad. Alternatively, set the parameter manually:
```sh
echo -n 1 > /data/params/d/LateralManeuverMode
```
To run only some of the maneuvers, set `LateralManeuverFilter` to a substring of their
descriptions. Aborts mean a session often never reaches the later maneuvers, so target the one
you need directly. Unset or unmatched runs the full suite.
```sh
echo -n 'sine 0.5Hz 30mph' > /data/params/d/LateralManeuverFilter # comma's published comparison
echo -n '30mph' > /data/params/d/LateralManeuverFilter # all four 30 mph maneuvers
```
4. Turn your vehicle back on. You will see "Lateral Maneuver Mode".
5. Ensure the area ahead is clear, as IQ.Pilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let IQ.Pilot stabilize lateral. After 2 seconds of steady straight driving on a road under 250 m radius and under 6.8° of roll, the maneuver will begin automatically. IQ.Pilot lateral control stays engaged between maneuvers normally while waiting for the next maneuver's readiness conditions. The maneuver will be aborted and repeated if speed is out of range, the steering wheel or gas is touched, or IQ.Pilot disengages.
6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle.
7. Locate the route(s) — they will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
```sh
$ python iqpilot/tools/maneuvers/lateral_report.py 98395b7c5b27882e/000001cc--5a73bde686
processing report for KIA_EV6
plotting maneuver: step right 20mph, runs: 3
plotting maneuver: step left 20mph, runs: 3
plotting maneuver: sine 0.5Hz 20mph, runs: 3
plotting maneuver: step right 30mph, runs: 3
Opening report: iqpilot/tools/maneuvers/reports/lateral/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html
```
The IQ.Pilot `lateral_report.py` also takes a path to a local `rlog.zst` or a directory of them, supports
auto-detection of lateral sweeps in any route without `alertDebug` markers (pass `--auto`), and ranks the
top-N highest-peak sweeps by speed/peak filters. See `lateral_report.py --help`.
## Blog-style response plot
`lateral_response_plot.py` renders the "requested vs actual + 50% response time" figure comma
publishes, for one or more routes on the same axes:
```sh
$ python iqpilot/tools/maneuvers/lateral_response_plot.py '<route>' \
--maneuver 'sine 0.5Hz 30mph' --label 'IQ.Lvbs angle — VW Golf MK7' --out response.png
```
Three stacked panels sharing a time axis: lateral acceleration (comma's panel, with the 50% marker),
steering wheel angle (commanded vs measured, the commanded trace only exists on angle-control cars),
and steering wheel rate. `--accel-only` drops to comma's single panel.
`controlsd` derives curvature as `-calc_curvature(steeringAngleDeg)`, so the raw wheel angle always
reads opposite to lateral acceleration. The angle and rate panels are flipped to match the
acceleration panel; pass `--raw-angle` to plot the raw log sign instead.
The 50% response time is only comparable between runs of the **same maneuver at the same speed**. A
step and a sine of equal amplitude do not produce comparable numbers: the step's request rises
instantly, so its 50% crossing measures rack rise time alone, while the 0.5 Hz sine's own request
takes ~0.167 s to reach 50%. comma's published 350 ms (ID.4) and 423 ms (Model Y) are both from the
0.5 Hz sine at 30 mph.
## Testing the tooling without a car
`simulate_lateral.py` runs `lateral_maneuversd` as a real process against a synthetic steering rack and writes an
rlog that `lateral_report.py` reads. Use it to verify the daemon and the report generator after changing either:
```sh
$ python iqpilot/tools/maneuvers/simulate_lateral.py --out /tmp/lat/rlog.zst
$ python iqpilot/tools/maneuvers/lateral_report.py /tmp/lat/rlog.zst
```
The full suite takes about 5 minutes of wall clock; `--max-maneuvers N` stops early.

60
iqpilot/tools/maneuvers/README-longitudinal.md Normal file
View File

@@ -0,0 +1,60 @@
# Longitudinal Maneuvers Testing Tool
Test your vehicle's longitudinal control tuning with this tool. The tool will test the vehicle's ability to follow a few longitudinal maneuvers and includes a tool to generate a report from the route.
<details><summary>Sample snapshot of a report.</summary><img width="600px" src="https://github.com/user-attachments/assets/d18d0c7d-2bde-44c1-8e86-1741ed442ad8"></details>
## Instructions
1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage IQ.Pilot between maneuvers and turn around if there is not enough space.
3. Turn off the vehicle and enable "Longitudinal Maneuver Mode" in Settings > Developer. The toggle requires IQ.Pilot longitudinal control and only enables while offroad. Alternatively, set the parameter manually:
```sh
echo -n 1 > /data/params/d/LongitudinalManeuverMode
```
4. Turn your vehicle back on. You will see the "Longitudinal Maneuver Mode" alert:
![videoframe_6652](https://github.com/user-attachments/assets/e9d4c95a-cd76-4ab7-933e-19937792fa0f)
5. Ensure the road ahead is clear, as openpilot will not brake for any obstructions in this mode. Once you are ready, press "Set" on your steering wheel to start the tests. The tests will run for about 4 minutes. If you need to pause the tests, press "Cancel" on your steering wheel. You can resume the tests by pressing "Resume" on your steering wheel.
**Note:** For GM cars, it is recommended to hold down the resume button for all low-speed tests (starting, stopping and creep) to avoid the car entering standstill.
![cog-clip-00 01 11 250-00 01 22 250](https://github.com/user-attachments/assets/c312c1cc-76e8-46e1-a05e-bb9dfb58994f)
6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle.
![fin2](https://github.com/user-attachments/assets/c06960ae-7cfb-44af-beaa-4dc28848e49d)
7. Visit https://connect.comma.ai and locate the route(s). They will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
![image](https://github.com/user-attachments/assets/cfe4c6d9-752f-4b24-b421-4b90a01933dc)
8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
```sh
$ python iqpilot/tools/maneuvers/longitudinal_report.py 57048cfce01d9625/0000010e--5b26bc3be7 'pcm accel compensation'
processing report for LEXUS_ES_TSS2
plotting maneuver: start from stop, runs: 4
plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2
plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2
Report written to iqpilot/tools/maneuvers/reports/longitudinal/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
```
`longitudinal_report.py` also takes a path to a local `rlog.zst` or a directory of them.
## Testing the tooling without a car
`simulate_longitudinal.py` runs `maneuversd` as a real process against a synthetic powertrain and writes an rlog
that `longitudinal_report.py` reads. Use it to verify the daemon and the report generator after changing either:
```sh
$ python iqpilot/tools/maneuvers/simulate_longitudinal.py --out /tmp/long/rlog.zst
$ python iqpilot/tools/maneuvers/longitudinal_report.py /tmp/long/rlog.zst
```
The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early.

238
iqpilot/tools/maneuvers/lateral_maneuversd.py Normal file
View File

@@ -0,0 +1,238 @@
#!/usr/bin/env python3
import numpy as np
from dataclasses import dataclass
from iqpilot.cereal import messaging, car
from iqpilot.common.constants import CV
from iqpilot.common.realtime import DT_MDL, Ratekeeper
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
from iqpilot.tools.maneuvers.longitudinal_maneuversd import Action, Maneuver as _Maneuver
# thresholds for starting maneuvers
MAX_SPEED_DEV = 0.7 # deviation in m/s
MAX_CURV = 0.004 # 250 m radius
MAX_ROLL = 0.12 # 6.8°
TIMER = 2.0 # sec stable conditions before starting maneuver
# The curvature step yanks the rim and spikes driver torque for a frame or two, which single-frame
# aborts read as a driver grab. Measured on VW_GOLF_MK7: 9/9 maneuvers died 0.15s in with the driver
# near hands-off. The EPS torque signal is also noisy enough to clip the ALC override threshold for
# ~10 ms at a time, blipping steeringPressed on its own, so require the hold to exceed 0.2 s of
# continuous frames — longer than any sensor blip or step reaction, far shorter than a real grab.
STEER_PRESSED_ABORT_S = 0.25
STEER_PRESSED_FRAMES = int(STEER_PRESSED_ABORT_S / DT_MDL) # 5 frames at 20 Hz
@dataclass
class Maneuver(_Maneuver):
_baseline_curvature: float = 0.0
def get_accel(self, v_ego: float, lat_active: bool, curvature: float, roll: float) -> float:
self._run_completed = False
# only start maneuver on straight, flat roads
ready = abs(v_ego - self.initial_speed) < MAX_SPEED_DEV and lat_active and abs(curvature) < MAX_CURV and abs(roll) < MAX_ROLL
self._ready_cnt = (self._ready_cnt + 1) if ready else max(self._ready_cnt - 1, 0)
if self._ready_cnt > (TIMER / DT_MDL):
if not self._active:
self._baseline_curvature = curvature
self._active = True
if not self._active:
return 0.0
return self._step()
def reset(self):
super().reset()
self._ready_cnt = 0
def _sine_action(amplitude, period, duration):
t = np.linspace(0, duration, int(duration / DT_MDL) + 1)
a = amplitude * np.sin(2 * np.pi * t / period)
return Action(a.tolist(), t.tolist())
MANEUVERS = [
Maneuver(
"step right 20mph",
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"step left 20mph",
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"sine 0.5Hz 20mph",
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"jitter 20mph",
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"step right 30mph",
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
Maneuver(
"step left 30mph",
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
Maneuver(
"sine 0.5Hz 30mph",
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
Maneuver(
"jitter 30mph",
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
]
def select_maneuvers(params) -> list[Maneuver]:
# LateralManeuverFilter runs only the maneuvers whose description contains it, so a session can
# target one maneuver (e.g. the 0.5 Hz sine at 30 mph) without driving the whole suite to reach it
needle = (params.get("LateralManeuverFilter") or "").strip()
if not needle:
return MANEUVERS
selected = [m for m in MANEUVERS if needle.lower() in m.description.lower()]
if not selected:
cloudlog.error(f"LateralManeuverFilter {needle!r} matched no maneuvers, running the full suite")
return MANEUVERS
cloudlog.info(f"LateralManeuverFilter {needle!r} selected: {[m.description for m in selected]}")
return selected
def main():
params = Params()
cloudlog.info("lateral_maneuversd is waiting for CarParams")
messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
# iqpilot: subscribe only to the services we actually read and drive timing with a
# Ratekeeper instead of polling modelV2. msgq caps each topic at NUM_READERS (15) and
# evicts ALL subscribers when exceeded; iqpilot runs many daemons, and unlike longitudinal
# maneuver mode (which disables plannerd), lateral mode keeps plannerd running. Subscribing
# to selfdriveState/modelV2 here (selfdriveState is unused; modelV2 was only a poll source)
# tips those topics past 15 → eviction storm → UI/speed render drops to a few fps.
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'])
pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug'])
rk = Ratekeeper(int(1. / DT_MDL), print_delay_threshold=None) # 20 Hz, matches DT_MDL maneuver timing
maneuvers = iter(select_maneuvers(params))
maneuver = None
complete_cnt = 0
aborted_cnt = 0
steer_pressed_cnt = 0
abort_reason = ''
display_holdoff = 0
prev_text = ''
while True:
sm.update(0)
if maneuver is None:
maneuver = next(maneuvers, None)
alert_msg = messaging.new_message('alertDebug')
alert_msg.valid = True
plan_send = messaging.new_message('lateralManeuverPlan')
accel = 0
v_ego = max(sm['carState'].vEgo, 0)
curvature = sm['controlsState'].desiredCurvature
if complete_cnt > 0:
complete_cnt -= 1
alert_msg.alertDebug.alertText1 = 'Completed'
alert_msg.alertDebug.alertText2 = maneuver.description
elif maneuver is not None:
# any driver input aborts the maneuver, but only a sustained hold counts as steering override
CS = sm['carState']
steer_pressed_cnt = (steer_pressed_cnt + 1) if CS.steeringPressed else 0
steer_override = steer_pressed_cnt >= STEER_PRESSED_FRAMES
if steer_override or CS.gasPressed:
aborted_cnt = int(1.0 / DT_MDL)
abort_reason = ('steering pressed' if steer_override else 'gas pressed').ljust(20)
aborted = aborted_cnt > 0
speed_out_of_range = maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV
if aborted or speed_out_of_range:
maneuver.reset()
roll = sm['carControl'].orientationNED[0] if len(sm['carControl'].orientationNED) == 3 else 0.0
accel = maneuver.get_accel(v_ego, sm['carControl'].latActive, curvature, roll)
if maneuver._run_completed:
complete_cnt = int(1.0 / DT_MDL)
alert_msg.alertDebug.alertText1 = 'Complete'
alert_msg.alertDebug.alertText2 = maneuver.description
elif maneuver.active:
action_remaining = maneuver.actions[maneuver._action_index].time_bp[-1] - maneuver._action_frames * DT_MDL
if maneuver.description.startswith('sine'):
freq = maneuver.description.split()[1]
alert_msg.alertDebug.alertText1 = f'Active sine {freq} {max(action_remaining, 0):.1f}s'
else:
alert_msg.alertDebug.alertText1 = f'Active {accel:+.1f}m/s² {max(action_remaining, 0):.1f}s'
alert_msg.alertDebug.alertText2 = maneuver.description
elif aborted_cnt > 0:
aborted_cnt -= 1
alert_msg.alertDebug.alertText1 = abort_reason
elif not (abs(v_ego - maneuver.initial_speed) < MAX_SPEED_DEV and sm['carControl'].latActive):
alert_msg.alertDebug.alertText1 = f'Set speed to {maneuver.initial_speed * CV.MS_TO_MPH:0.0f} mph'
elif maneuver._ready_cnt > 0:
ready_time = max(TIMER - maneuver._ready_cnt * DT_MDL, 0)
alert_msg.alertDebug.alertText1 = f'Starting: {int(ready_time) + 1}'
alert_msg.alertDebug.alertText2 = maneuver.description
else:
curv_ok = abs(curvature) < MAX_CURV
reason = 'road not straight' if not curv_ok else 'road not flat'
alert_msg.alertDebug.alertText1 = f'Waiting: {reason}'
alert_msg.alertDebug.alertText2 = maneuver.description
else:
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
# prevent flickering text
setup = ('Set speed', 'Starting', 'Waiting')
text = alert_msg.alertDebug.alertText1
same = text == prev_text or (text.startswith('Starting') and prev_text.startswith('Starting'))
if not same and text.startswith(setup) and prev_text.startswith(setup) and display_holdoff > 0:
alert_msg.alertDebug.alertText1 = prev_text
display_holdoff -= 1
else:
prev_text = text
display_holdoff = int(0.5 / DT_MDL) if text.startswith(setup) else 0
pm.send('alertDebug', alert_msg)
plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0
if plan_send.valid:
plan_send.lateralManeuverPlan.desiredCurvature = maneuver._baseline_curvature + accel / max(v_ego, MIN_SPEED) ** 2
pm.send('lateralManeuverPlan', plan_send)
if maneuver is not None and maneuver.finished and complete_cnt == 0:
maneuver = None
rk.keep_time()
if __name__ == "__main__":
main()

280
iqpilot/tools/maneuvers/lateral_report.py Normal file
View File

@@ -0,0 +1,280 @@
#!/usr/bin/env python3
import argparse
import base64
import io
import math
import numpy as np
import os
import webbrowser
from collections import defaultdict
from pathlib import Path
import matplotlib.pyplot as plt
from iqpilot.common.utils import tabulate
from iqpilot.cereal import car
from iqpilot.common.filter_simple import FirstOrderFilter
from iqpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
from iqpilot.tools.lib.logreader import LogReader
from iqpilot.system.hardware.hw import Paths
from iqpilot.common.constants import CV
from iqpilot.common.realtime import DT_MDL
from iqpilot.tools.maneuvers.lateral_maneuversd import STEER_PRESSED_FRAMES
from iqpilot.tools.maneuvers.longitudinal_report import format_car_params
ANGLE_CONTROL = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED)
STEER_OVERRIDE_S = STEER_PRESSED_FRAMES * DT_MDL
def lat_accel(curvature, v):
return curvature * max(v, 1.0) ** 2
def steering_overridden(t_carState, carState):
# mirrors lateral_maneuversd: the curvature step spikes driver torque for a frame or two on
# cars with a tight override threshold, so only a sustained hold invalidates the run
start = None
for t, cs in zip(t_carState, carState, strict=True):
if not cs.steeringPressed:
start = None
elif start is None:
start = t
elif t - start >= STEER_OVERRIDE_S:
return True
return False
def report(platform, route, _description, CP, ID, maneuvers):
output_path = Path(__file__).resolve().parent / "reports" / "lateral"
output_fn = output_path / f"{platform}_{route.replace('/', '_').replace('|', '_')}.html"
output_path.mkdir(parents=True, exist_ok=True)
target_cross_times = defaultdict(list)
builder = [
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
"<h1>Lateral maneuver report</h1>\n",
f"<h3>{platform}</h3>\n",
f"<h3>{route}</h3>\n",
f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
]
if _description is not None:
builder.append(f"<h3>Description: {_description}</h3>\n")
builder.append(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\n")
builder.append('{ summary }') # to be replaced below
for description, runs in maneuvers:
# filter incomplete runs
completed_runs = [msgs for msgs in runs
if any(m.alertDebug.alertText1 == 'Complete' for m in msgs if m.which() == 'alertDebug')]
print(f'plotting maneuver: {description}, runs: {len(completed_runs)}')
if not completed_runs:
continue
builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
builder.append(f"<h2>{description}</h2>\n")
for run, msgs in enumerate(completed_runs):
last_active = max(m.logMonoTime for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid)
msgs = [m for m in msgs if m.logMonoTime <= last_active]
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
# make time relative seconds
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
t_controlsState = [(t - t_controlsState[0]) / 1e9 for t in t_controlsState]
t_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan]
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
# maneuver validity
latActive = [m.latActive for m in carControl]
maneuver_valid = all(latActive) and not steering_overridden(t_carState, carState)
_open = 'open' if maneuver_valid else ''
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
baseline_accel = lat_accel(controlsState[0].curvature, carState[0].vEgo)
v_ego = [m.vEgo for m in carState]
cross_markers = []
if description.startswith(('sine', 'jitter')):
amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel)
for lp, v in zip(lateralPlan, v_ego, strict=False))
threshold = amplitude * 0.5
builder.append('<h3 style="font-weight: normal">50% peak')
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
actual = lat_accel(cs.curvature, v) - baseline_accel
if abs(actual) > threshold:
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
cross_markers.append((t, actual + baseline_accel))
if maneuver_valid:
target_cross_times[description].append(t)
break
else:
builder.append(', <strong>not crossed</strong>')
builder.append('</h3>')
if maneuver_valid:
target_cross_times.setdefault(description, [])
else:
action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_ego[0]) - baseline_accel)]
for i in range(1, min(len(lateralPlan), len(v_ego))):
if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001:
desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel
action_targets.append((i, desired))
for j, (start_i, act_target) in enumerate(action_targets):
start_time = t_lateralPlan[start_i]
end_time = t_lateralPlan[action_targets[j + 1][0]] if j + 1 < len(action_targets) else t_controlsState[-1]
builder.append(f'<h3 style="font-weight: normal">aTarget: {round(act_target, 1)} m/s^2')
prev_crossed = False
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
if not (start_time <= t <= end_time):
continue
actual_accel = lat_accel(cs.curvature, v) - baseline_accel
crossed = (0 < act_target < actual_accel) or (0 > act_target > actual_accel)
if crossed and prev_crossed:
cross_time = t - start_time
builder.append(f', <strong>crossed in {cross_time:.3f}s</strong>')
cross_markers.append((t, act_target + baseline_accel))
if maneuver_valid:
target_cross_times[description].append(cross_time)
break
prev_crossed = crossed
else:
builder.append(', <strong>not crossed</strong>')
builder.append('</h3>')
if maneuver_valid:
target_cross_times.setdefault(description, [])
plt.rcParams['font.size'] = 40
fig = plt.figure(figsize=(30, 40))
ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]})
ax[0].grid(linewidth=4)
desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2'
desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_ego, strict=False)]
if description.startswith(('sine', 'jitter')):
ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, 'C1', label=desired_label, linewidth=6)
else:
t_desired = [t_lateralPlan[0]] + t_lateralPlan[:len(desired_lat_accel)]
desired_lat_accel = [baseline_accel] + desired_lat_accel
ax[0].step(t_desired, desired_lat_accel, 'C1', label=desired_label, linewidth=6, where='post')
actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_ego, strict=False)]
ax[0].plot(t_controlsState[:len(actual_lat_accel)], actual_lat_accel, 'g', label='controlsState.curvature * vEgo^2', linewidth=6)
ax[0].set_ylabel('Lateral Accel (m/s^2)')
for ct, cv in cross_markers:
ax[0].plot(ct, cv, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
ax[0].legend(prop={'size': 30})
ax[1].grid(linewidth=4)
if CP.steerControlType in ANGLE_CONTROL:
steer_field, steer_ylabel = 'steeringAngleDeg', 'Steer angle (deg)'
else:
steer_field, steer_ylabel = 'torque', 'Steer torque'
ax[1].plot(t_carControl, [getattr(m.actuators, steer_field) for m in carControl], 'C1', label=f'carControl.actuators.{steer_field}', linewidth=6)
ax[1].plot(t_carOutput, [getattr(m.actuatorsOutput, steer_field) for m in carOutput], 'g', label=f'carOutput.actuatorsOutput.{steer_field}', linewidth=6)
ax[1].set_ylabel(steer_ylabel)
ax[1].legend(prop={'size': 30})
ax[2].grid(linewidth=4)
ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6)
ax[2].set_ylabel('Velocity (mph)')
ax[2].yaxis.set_major_formatter(plt.FormatStrFormatter('%.1f'))
ax[2].legend()
t_accel = np.array(t_controlsState[:len(actual_lat_accel)])
raw_jerk = np.gradient(actual_lat_accel, t_accel)
dt_avg = np.mean(np.diff(t_accel))
jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt_avg)
filtered_jerk = [jerk_filter.update(j) for j in raw_jerk]
ax[3].grid(linewidth=4)
ax[3].plot(t_accel, filtered_jerk, label='d/dt(controlsState.curvature * vEgo^2)', linewidth=6)
ax[3].set_ylabel('Jerk (m/s^3)')
ax[3].legend()
ax[4].grid(linewidth=4)
ax[4].plot(t_carControl, [math.degrees(m.orientationNED[0]) if len(m.orientationNED) == 3 else 0.0 for m in carControl],
label='carControl.orientationNED[0]', linewidth=6)
ax[4].set_ylabel('Roll (deg)')
ax[4].legend()
ax[-1].set_xlabel("Time (s)")
fig.tight_layout()
buffer = io.BytesIO()
fig.savefig(buffer, format='webp')
plt.close(fig)
buffer.seek(0)
builder.append(f"<img src='data:image/webp;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
builder.append("</details>\n")
summary = ["<h2>Summary</h2>\n"]
cols = ['maneuver', 'crossed', 'mean', 'min', 'max']
table = []
for description, times in target_cross_times.items():
l = [description, len(times)]
if len(times):
l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
table.append(l)
summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n')
sum_idx = builder.index('{ summary }')
builder[sum_idx:sum_idx + 1] = summary
with open(output_fn, "w") as f:
f.write(''.join(builder))
print(f"\nOpening report: {output_fn}\n")
webbrowser.open_new_tab(str(output_fn))
def open_route(route: str) -> LogReader:
if os.path.isdir(route):
rlogs = sorted(str(p) for p in Path(route).glob("*rlog.zst"))
if not rlogs:
raise SystemExit(f"no *rlog.zst files in {route}")
print(f"loading {len(rlogs)} rlogs from {route}")
return LogReader(rlogs, only_union_types=True)
if route.endswith(('.zst', '.bz2', '.log')) and not os.path.exists(route):
raise SystemExit(f"no such file: {route}")
if os.path.exists(route) or '/' in route or '|' in route:
return LogReader(route, only_union_types=True)
segs = [seg for seg in os.listdir(Paths.log_root()) if route in seg]
return LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs], only_union_types=True)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Generate lateral maneuver report from route')
parser.add_argument('route', type=str, help='Route name, local rlog path, or directory of rlogs')
parser.add_argument('description', type=str, nargs='?')
args = parser.parse_args()
lr = open_route(args.route)
CP = lr.first('carParams')
ID = lr.first('initData')
platform = CP.carFingerprint
print('processing report for', platform)
maneuvers: list[tuple[str, list[list]]] = []
active_prev = False
description_prev = None
for msg in lr:
if msg.which() == 'alertDebug':
active = 'Active' in msg.alertDebug.alertText1 or msg.alertDebug.alertText1 == 'Complete'
if active and not active_prev:
if msg.alertDebug.alertText2 == description_prev:
maneuvers[-1][1].append([])
else:
maneuvers.append((msg.alertDebug.alertText2, [[]]))
description_prev = maneuvers[-1][0]
active_prev = active
if active_prev:
maneuvers[-1][1][-1].append(msg)
report(platform, args.route, args.description, CP, ID, maneuvers)

238
iqpilot/tools/maneuvers/lateral_response_plot.py Executable file
View File

@@ -0,0 +1,238 @@
#!/usr/bin/env python3
"""Blog-style lateral actuator response plot.
Renders the single-panel "requested vs actual lateral acceleration + 50% response time" figure
comma publishes for lateral maneuver comparisons, for one or more routes on the same axes.
./iqpilot/tools/maneuvers/lateral_response_plot.py 1ce1b50dd82993a1'|'00000011--6cb007b200/0:4 \
--maneuver 'sine 0.5Hz 30mph' --label 'IQ.Lvbs angle (Golf MK7)'
The 50% response time is only comparable between runs of the SAME maneuver at the SAME speed;
a step and a sine of equal amplitude do not produce comparable numbers.
"""
import argparse
from pathlib import Path
from typing import NamedTuple
import matplotlib.pyplot as plt
import numpy as np
from iqpilot.tools.maneuvers.lateral_report import lat_accel, open_route, steering_overridden
SERIES_COLORS = ('#2ca02c', '#ff7f0e', '#1f77b4', '#d62728')
REQUESTED_COLOR = '#999999'
def completed_runs(msgs, maneuver):
runs, active_prev, desc_prev = [], False, None
for m in msgs:
if m.which() == 'alertDebug':
active = 'Active' in m.alertDebug.alertText1 or m.alertDebug.alertText1 == 'Complete'
if active and not active_prev:
if m.alertDebug.alertText2 == desc_prev:
runs[-1][1].append([])
else:
runs.append((m.alertDebug.alertText2, [[]]))
desc_prev = runs[-1][0]
active_prev = active
if active_prev:
runs[-1][1][-1].append(m)
out = []
for description, windows in runs:
if maneuver is not None and description != maneuver:
continue
for w in windows:
if any(m.alertDebug.alertText1 == 'Complete' for m in w if m.which() == 'alertDebug'):
out.append((description, w))
return out
class Run(NamedTuple):
t_requested: np.ndarray
requested: np.ndarray
t_actual: np.ndarray
actual: np.ndarray
t_wheel: np.ndarray
angle: np.ndarray
rate: np.ndarray
t_angle_cmd: np.ndarray
angle_cmd: np.ndarray | None
v_mean: float
valid: bool
def extract(msgs, raw_angle: bool = False) -> Run:
"""requested/actual lateral accel and wheel angle on a common relative timebase, baseline removed"""
t_cs, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
t_ct, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
t_cc, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
t_lp, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs
if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
t0 = t_lp[0]
def rel(ts):
return np.array([(t - t0) / 1e9 for t in ts])
t_cs_s, t_ct_s, t_cc_s, t_lp_s = rel(t_cs), rel(t_ct), rel(t_cc), rel(t_lp)
v_ego = np.array([m.vEgo for m in carState])
v_at_lp = np.interp(t_lp_s, t_cs_s, v_ego)
v_at_ct = np.interp(t_ct_s, t_cs_s, v_ego)
baseline = lat_accel(controlsState[0].curvature, carState[0].vEgo)
requested = np.array([lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_at_lp, strict=True)]) - baseline
actual = np.array([lat_accel(m.curvature, v) for m, v in zip(controlsState, v_at_ct, strict=True)]) - baseline
# controlsd derives curvature as -calc_curvature(steeringAngleDeg), so raw wheel angle always reads
# opposite to lateral accel; flip it unless the caller wants the raw signal
sign = 1.0 if raw_angle else -1.0
angle = sign * (np.array([m.steeringAngleDeg for m in carState]) - carState[0].steeringAngleDeg)
rate = sign * np.array([m.steeringRateDeg for m in carState])
if not np.any(rate): # not all brands populate steeringRateDeg
rate = np.gradient(angle, t_cs_s)
# angle command only exists on angle-control cars
cmd = sign * (np.array([m.actuators.steeringAngleDeg for m in carControl]) - carControl[0].actuators.steeringAngleDeg)
angle_cmd = cmd if np.any(cmd) else None
window = lambda t: (t >= 0) & (t <= t_lp_s[-1]) # noqa: E731
k_ct, k_cs, k_cc = window(t_ct_s), window(t_cs_s), window(t_cc_s)
lat_active = all(m.latActive for m in carControl)
# steering_overridden takes seconds, not raw logMonoTime
overridden = steering_overridden(t_cs_s.tolist(), carState)
return Run(t_lp_s, requested, t_ct_s[k_ct], actual[k_ct],
t_cs_s[k_cs], angle[k_cs], rate[k_cs],
t_cc_s[k_cc], angle_cmd[k_cc] if angle_cmd is not None else None,
float(np.mean(v_ego)), lat_active and not overridden)
def response_time(t_actual, actual, requested):
"""time to first reach 50% of the requested peak, comma's metric"""
amplitude = float(np.max(np.abs(requested)))
if amplitude < 1e-3:
return None, amplitude
threshold = 0.5 * amplitude
crossed = np.flatnonzero(np.abs(actual) > threshold)
if not len(crossed):
return None, amplitude
return float(t_actual[crossed[0]]), amplitude
def main():
parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('routes', nargs='+', help='route, local rlog path, or directory (one per series)')
parser.add_argument('--label', action='append', default=[], help='series label, repeat to match routes')
parser.add_argument('--maneuver', default='sine 0.5Hz 30mph', help='maneuver description to plot')
parser.add_argument('--run', type=int, default=0, help='which completed run to plot (default first)')
parser.add_argument('--out', type=Path, default=Path('lateral_response.png'))
parser.add_argument('--title', default=None)
parser.add_argument('--accel-only', action='store_true',
help="just comma's single lateral-accel panel, without wheel angle and rate")
parser.add_argument('--raw-angle', action='store_true',
help='plot wheel angle in the raw log sign instead of aligned to lateral accel')
args = parser.parse_args()
if args.accel_only:
fig, ax0 = plt.subplots(figsize=(9, 5.5), dpi=200)
ax_angle = ax_rate = None
axes = [ax0]
else:
fig, axes = plt.subplots(3, 1, figsize=(9, 10), dpi=200, sharex=True,
gridspec_kw={'height_ratios': [3, 2, 2]})
ax0, ax_angle, ax_rate = axes
ax = ax0
annotations = []
plotted_requested = False
plotted_cmd = False
for i, route in enumerate(args.routes):
label = args.label[i] if i < len(args.label) else route
color = SERIES_COLORS[i % len(SERIES_COLORS)]
msgs = list(open_route(route))
runs = completed_runs(msgs, args.maneuver)
if not runs:
have = sorted({d for d, _ in completed_runs(msgs, None)})
raise SystemExit(f"{route}: no completed '{args.maneuver}' runs. completed maneuvers in this route: {have or 'none'}")
if args.run >= len(runs):
raise SystemExit(f"{route}: only {len(runs)} completed '{args.maneuver}' run(s), --run {args.run} out of range")
description, msgs_run = runs[args.run]
r = extract(msgs_run, args.raw_angle)
t_req, requested, t_act, actual, v_mean, valid = r.t_requested, r.requested, r.t_actual, r.actual, r.v_mean, r.valid
cross, amplitude = response_time(t_act, actual, requested)
if not plotted_requested:
ax.plot(t_req, requested, color=REQUESTED_COLOR, linestyle=':', linewidth=2.5, label='requested', zorder=1)
plotted_requested = True
ax.plot(t_act, actual, color=color, linewidth=2.5, label=label, zorder=3)
if ax_angle is not None:
if r.angle_cmd is not None and not plotted_cmd:
ax_angle.plot(r.t_angle_cmd, r.angle_cmd, color=REQUESTED_COLOR, linestyle=':', linewidth=2.5,
label='commanded', zorder=1)
plotted_cmd = True
ax_angle.plot(r.t_wheel, r.angle, color=color, linewidth=2.5, zorder=3)
ax_rate.plot(r.t_wheel, r.rate, color=color, linewidth=2.5, zorder=3)
if cross is not None:
y = float(np.interp(cross, t_act, actual))
ax.axvline(cross, color=color, linestyle='--', linewidth=1.5, ymax=0.92, zorder=2)
ax.plot(cross, y, marker='o', markersize=9, markeredgewidth=2,
markeredgecolor=color, markerfacecolor='none', zorder=4)
annotations.append((f'50% response in {cross:.3f} s', color))
else:
annotations.append(('50% response not reached', color))
flag = '' if valid else ' (INVALID: lat not active or steering overridden)'
cross_str = f'{cross:.3f} s' if cross else 'n/a'
print(', '.join([
f"{label}: {description}",
f"run {args.run}",
f"{v_mean * 2.23694:.1f} mph",
f"peak requested {amplitude:.2f} m/s^2",
f"50% in {cross_str}",
f"peak wheel {np.abs(r.angle).max():.1f} deg",
f"peak rate {np.abs(r.rate).max():.0f} deg/s{flag}",
]))
# headroom so the annotation block never sits on the trace
lo, hi = ax.get_ylim()
ax.set_ylim(lo, hi + (hi - lo) * 0.10 * max(len(annotations), 1))
for j, (text, color) in enumerate(annotations):
ax.text(0.03, 0.965 - j * 0.06, text, transform=ax.transAxes, color=color,
fontsize=11, fontweight='bold', va='top',
bbox={'facecolor': 'white', 'edgecolor': 'none', 'alpha': 0.75, 'pad': 2})
ax.set_ylabel('Lateral acceleration (m/s²)')
if ax_angle is not None:
ax_angle.set_ylabel('Steering wheel angle (deg)')
ax_rate.set_ylabel('Steering wheel rate (deg/s)')
if not args.raw_angle:
fig.text(0.5, 0.005, 'wheel angle sign aligned to lateral accel (raw log sign is inverted; --raw-angle to keep it)',
ha='center', fontsize=8, color='#777777')
if plotted_cmd:
ax_angle.legend(loc='upper right', frameon=False, fontsize=9)
axes[-1].set_xlabel('Time (s)')
for a in axes:
a.grid(True, color='#dddddd', linewidth=0.8)
a.set_axisbelow(True)
for side in ('top', 'right'):
a.spines[side].set_visible(False)
ax.legend(loc='upper center', bbox_to_anchor=(0.5, 1.12 if args.accel_only else 1.10),
ncol=4, frameon=False, fontsize=10)
if args.title:
ax.set_title(args.title, pad=28)
fig.tight_layout()
fig.savefig(args.out, bbox_inches='tight')
print(f"\nwrote {args.out}")
if __name__ == '__main__':
main()

200
iqpilot/tools/maneuvers/longitudinal_maneuversd.py Executable file
View File

@@ -0,0 +1,200 @@
#!/usr/bin/env python3
import numpy as np
from dataclasses import dataclass
from iqpilot.cereal import messaging
from iqpilot.common.constants import CV
from iqpilot.common.realtime import DT_MDL
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.controls.lib.drive_helpers import should_stop
@dataclass
class Action:
accel_bp: list[float] # m/s^2
time_bp: list[float] # seconds
def __post_init__(self):
assert len(self.accel_bp) == len(self.time_bp)
@dataclass
class Maneuver:
description: str
actions: list[Action]
repeat: int = 0
initial_speed: float = 0. # m/s
_active: bool = False
_finished: bool = False
_run_completed: bool = False
_action_index: int = 0
_action_frames: int = 0
_ready_cnt: int = 0
_repeated: int = 0
def _step(self) -> float:
self._run_completed = False
action = self.actions[self._action_index]
action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp)
self._action_frames += 1
# reached duration of action
if self._action_frames > (action.time_bp[-1] / DT_MDL):
# next action
if self._action_index < len(self.actions) - 1:
self._action_index += 1
self._action_frames = 0
# repeat maneuver
elif self._repeated < self.repeat:
self._repeated += 1
self._run_completed = True
self.reset()
# finish maneuver
else:
self._run_completed = True
self._finished = True
return float(action_accel)
def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_standstill: bool) -> float:
ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not cruise_standstill
if self.initial_speed < 0.01:
ready = ready and standstill
self._ready_cnt = (self._ready_cnt + 1) if ready else 0
if self._ready_cnt > (3. / DT_MDL):
self._active = True
if not self._active:
return min(max(self.initial_speed - v_ego, -2.), 2.)
return self._step()
def reset(self):
self._active = False
self._action_frames = 0
self._action_index = 0
@property
def finished(self):
return self._finished
@property
def active(self):
return self._active
MANEUVERS = [
Maneuver(
"come to stop",
[Action([-0.5], [12])],
repeat=2,
initial_speed=5.,
),
Maneuver(
"start from stop",
[Action([1.5], [6])],
repeat=2,
initial_speed=0.,
),
Maneuver(
"creep: alternate between +1m/s^2 and -1m/s^2",
[
Action([1], [3]), Action([-1], [3]),
Action([1], [3]), Action([-1], [3]),
Action([1], [3]), Action([-1], [3]),
],
repeat=2,
initial_speed=0.,
),
Maneuver(
"brake step response: -1m/s^2 from 20mph",
[Action([-1], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"brake step response: -4m/s^2 from 20mph",
[Action([-4], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"gas step response: +1m/s^2 from 20mph",
[Action([1], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"gas step response: +4m/s^2 from 20mph",
[Action([4], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
]
def main():
params = Params()
cloudlog.info("maneuversd is waiting for CarParams")
params.get("CarParams", block=True)
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
pm = messaging.PubMaster(['longitudinalPlan', 'iqPlan', 'driverAssistance', 'alertDebug'])
maneuvers = iter(MANEUVERS)
maneuver = None
while True:
sm.update()
if maneuver is None:
maneuver = next(maneuvers, None)
alert_msg = messaging.new_message('alertDebug')
alert_msg.valid = True
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks()
longitudinalPlan = plan_send.longitudinalPlan
accel = 0
v_ego = max(sm['carState'].vEgo, 0)
if maneuver is not None:
accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill)
if maneuver.active:
alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2'
else:
alert_msg.alertDebug.alertText1 = f'Setting up to {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph'
alert_msg.alertDebug.alertText2 = f'{maneuver.description}'
else:
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
pm.send('alertDebug', alert_msg)
longitudinalPlan.aTarget = accel
longitudinalPlan.shouldStop = should_stop(v_ego, accel)
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = True
longitudinalPlan.hasLead = True
longitudinalPlan.speeds = [0.2] # triggers carControl.cruiseControl.resume in controlsd
pm.send('longitudinalPlan', plan_send)
plan_iq_send = messaging.new_message('iqPlan')
plan_iq_send.valid = True
pm.send('iqPlan', plan_iq_send)
assistance_send = messaging.new_message('driverAssistance')
assistance_send.valid = True
pm.send('driverAssistance', assistance_send)
if maneuver is not None and maneuver.finished:
maneuver = None

187
iqpilot/tools/maneuvers/longitudinal_report.py Executable file
View File

@@ -0,0 +1,187 @@
#!/usr/bin/env python3
import argparse
import base64
import io
import os
import math
import pprint
import webbrowser
from collections import defaultdict
from pathlib import Path
import matplotlib.pyplot as plt
from iqpilot.common.utils import tabulate
from iqpilot.tools.lib.logreader import LogReader
from iqpilot.system.hardware.hw import Paths
def format_car_params(CP):
return pprint.pformat({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2)
def report(platform, route, _description, CP, ID, maneuvers):
output_path = Path(__file__).resolve().parent / "reports" / "longitudinal"
output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
output_path.mkdir(parents=True, exist_ok=True)
target_cross_times = defaultdict(list)
builder = [
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
"<h1>Longitudinal maneuver report</h1>\n",
f"<h3>{platform}</h3>\n",
f"<h3>{route}</h3>\n",
f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
]
if _description is not None:
builder.append(f"<h3>Description: {_description}</h3>\n")
builder.append(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\n")
builder.append('{ summary }') # to be replaced below
for description, runs in maneuvers:
print(f'plotting maneuver: {description}, runs: {len(runs)}')
builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
builder.append(f"<h2>{description}</h2>\n")
for run, msgs in enumerate(runs):
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
t_deviceMotion, deviceMotion = zip(*[(m.logMonoTime, m.deviceMotion) for m in msgs if m.which() == 'deviceMotion'], strict=True)
t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True)
# make time relative seconds
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
t_deviceMotion = [(t - t_deviceMotion[0]) / 1e9 for t in t_deviceMotion]
t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan]
# maneuver validity
longActive = [m.longActive for m in carControl]
maneuver_valid = all(longActive) and (not any(cs.cruiseState.standstill for cs in carState) or CP.autoResumeSng)
_open = 'open' if maneuver_valid else ''
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
# get first acceleration target and first intersection
aTarget = longitudinalPlan[0].aTarget
target_cross_time = None
builder.append(f'<h3 style="font-weight: normal">Initial aTarget: {round(aTarget, 2)} m/s^2')
# Localizer is noisy, require two consecutive 20Hz frames above threshold
prev_crossed = False
for t, lp in zip(t_deviceMotion, deviceMotion, strict=True):
crossed = (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x)
if crossed and prev_crossed:
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
target_cross_time = t
if maneuver_valid:
target_cross_times[description].append(t)
break
prev_crossed = crossed
else:
builder.append(', <strong>not crossed</strong>')
builder.append('</h3>')
pitches = [math.degrees(m.orientationNED[1]) for m in carControl]
builder.append(f'<h3 style="font-weight: normal">Average pitch: <strong>{sum(pitches) / len(pitches):0.2f} degrees</strong></h3>')
plt.rcParams['font.size'] = 40
fig = plt.figure(figsize=(30, 26))
ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'height_ratios': [5, 3, 1, 1]})
ax[0].grid(linewidth=4)
ax[0].plot(t_carControl, [m.actuators.accel for m in carControl], label='carControl.actuators.accel', linewidth=6)
ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6)
ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6)
ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6)
ax[0].plot(t_deviceMotion, [m.accelerationDevice.x for m in deviceMotion], label='deviceMotion.accelerationDevice.x', linewidth=6)
# TODO localizer accel
ax[0].set_ylabel('Acceleration (m/s^2)')
#ax[0].set_ylim(-6.5, 6.5)
ax[0].legend(prop={'size': 30})
if target_cross_time is not None:
ax[0].plot(target_cross_time, aTarget, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
ax[1].grid(linewidth=4)
ax[1].plot(t_carState, [m.vEgo for m in carState], 'g', label='vEgo', linewidth=6)
ax[1].set_ylabel('Velocity (m/s)')
ax[1].legend()
ax[2].plot(t_carControl, longActive, label='longActive', linewidth=6)
ax[3].plot(t_carState, [m.gasPressed for m in carState], label='gasPressed', linewidth=6)
ax[3].plot(t_carState, [m.brakePressed for m in carState], label='brakePressed', linewidth=6)
for i in (2, 3):
ax[i].set_yticks([0, 1], minor=False)
ax[i].set_ylim(-1, 2)
ax[i].legend()
ax[-1].set_xlabel("Time (s)")
fig.tight_layout()
buffer = io.BytesIO()
fig.savefig(buffer, format='webp')
plt.close(fig)
buffer.seek(0)
builder.append(f"<img src='data:image/webp;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
builder.append("</details>\n")
summary = ["<h2>Summary</h2>\n"]
cols = ['maneuver', 'crossed', 'runs', 'mean', 'min', 'max']
table = []
for description, runs in maneuvers:
times = target_cross_times[description]
l = [description, len(times), len(runs)]
if len(times):
l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
table.append(l)
summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n')
sum_idx = builder.index('{ summary }')
builder[sum_idx:sum_idx + 1] = summary
with open(output_fn, "w") as f:
f.write(''.join(builder))
print(f"\nOpening report: {output_fn}\n")
webbrowser.open_new_tab(str(output_fn))
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Generate longitudinal maneuver report from route')
parser.add_argument('route', type=str, help='Route name (e.g. 00000000--5f742174be)')
parser.add_argument('description', type=str, nargs='?')
args = parser.parse_args()
if '/' in args.route or '|' in args.route:
lr = LogReader(args.route)
else:
segs = [seg for seg in os.listdir(Paths.log_root()) if args.route in seg]
lr = LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs])
CP = lr.first('carParams')
ID = lr.first('initData')
platform = CP.carFingerprint
print('processing report for', platform)
maneuvers: list[tuple[str, list[list]]] = []
active_prev = False
description_prev = None
for msg in lr:
if msg.which() == 'alertDebug':
active = 'Maneuver Active' in msg.alertDebug.alertText1
if active and not active_prev:
if msg.alertDebug.alertText2 == description_prev:
maneuvers[-1][1].append([])
else:
maneuvers.append((msg.alertDebug.alertText2, [[]]))
description_prev = maneuvers[-1][0]
active_prev = active
if active_prev:
maneuvers[-1][1][-1].append(msg)
report(platform, args.route, args.description, CP, ID, maneuvers)

18
iqpilot/tools/maneuvers/maneuver_helpers.py Normal file
View File

@@ -0,0 +1,18 @@
from enum import IntEnum
class Axis(IntEnum):
TIME = 0
EGO_POSITION = 1
LEAD_DISTANCE= 2
EGO_V = 3
LEAD_V = 4
EGO_A = 5
D_REL = 6
axis_labels = {Axis.TIME: 'Time (s)',
Axis.EGO_POSITION: 'Ego position (m)',
Axis.LEAD_DISTANCE: 'Lead absolute position (m)',
Axis.EGO_V: 'Ego Velocity (m/s)',
Axis.LEAD_V: 'Lead Velocity (m/s)',
Axis.EGO_A: 'Ego acceleration (m/s^2)',
Axis.D_REL: 'Lead distance (m)'}

293
iqpilot/tools/maneuvers/mpc_longitudinal_tuning_report.py Normal file
View File

@@ -0,0 +1,293 @@
import io
import sys
import numpy as np
import matplotlib.pyplot as plt
from iqpilot.common.realtime import DT_MDL
from iqpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance
from iqpilot.tools.maneuvers.maneuver_helpers import Axis, axis_labels
from iqpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
def get_html_from_results(results, labels, AXIS):
fig, ax = plt.subplots(figsize=(16, 8))
for idx, key in enumerate(results.keys()):
ax.plot(results[key][:, Axis.TIME], results[key][:, AXIS], label=labels[idx])
ax.set_xlabel(axis_labels[Axis.TIME])
ax.set_ylabel(axis_labels[AXIS])
ax.legend(bbox_to_anchor=(1.02, 1), loc='upper left', borderaxespad=0)
ax.grid(True, linestyle='--', alpha=0.7)
ax.text(-0.075, 0.5, '.', transform=ax.transAxes, color='none')
fig_buffer = io.StringIO()
fig.savefig(fig_buffer, format='svg', bbox_inches='tight')
plt.close(fig)
return fig_buffer.getvalue() + '<br/>'
def generate_mpc_tuning_report():
htmls = []
results = {}
name = 'Resuming behind lead'
labels = []
for lead_accel in np.linspace(1.0, 4.0, 4):
man = Maneuver(
'',
duration=11,
initial_speed=0.0,
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(0.0, 0.0),
speed_lead_values=[0.0, 10 * lead_accel],
cruise_values=[100, 100],
prob_lead_values=[1.0, 1.0],
breakpoints=[1., 11],
)
valid, results[lead_accel] = man.evaluate()
labels.append(f'{lead_accel} m/s^2 lead acceleration')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
results = {}
name = 'Approaching stopped car from 140m'
labels = []
for speed in np.arange(0, 45, 5):
man = Maneuver(
name,
duration=30.,
initial_speed=float(speed),
lead_relevancy=True,
initial_distance_lead=140.,
speed_lead_values=[0.0, 0.],
breakpoints=[0., 30.],
)
valid, results[speed] = man.evaluate()
labels.append(f'{speed} m/s approach speed')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
results = {}
name = 'Following 5s (triangular) oscillating lead'
labels = []
speed = np.int64(10)
for oscil in np.arange(0, 10, 1):
man = Maneuver(
'',
duration=30.,
initial_speed=float(speed),
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(speed, speed),
speed_lead_values=[speed, speed, speed - oscil, speed + oscil, speed - oscil, speed + oscil, speed - oscil],
breakpoints=[0., 2., 5, 8, 15, 18, 25.],
)
valid, results[oscil] = man.evaluate()
labels.append(f'{oscil} m/s oscillation size')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
results = {}
name = 'Following 5s (sinusoidal) oscillating lead'
labels = []
speed = np.int64(10)
duration = float(30)
f_osc = 1. / 5
for oscil in np.arange(0, 10, 1):
bps = DT_MDL * np.arange(int(duration / DT_MDL))
lead_speeds = speed + oscil * np.sin(2 * np.pi * f_osc * bps)
man = Maneuver(
'',
duration=duration,
initial_speed=float(speed),
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(speed, speed),
speed_lead_values=lead_speeds,
breakpoints=bps,
)
valid, results[oscil] = man.evaluate()
labels.append(f'{oscil} m/s oscillation size')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
results = {}
name = 'Speed profile when converging to steady state lead at 30m/s'
labels = []
for distance in np.arange(20, 140, 10):
man = Maneuver(
'',
duration=50,
initial_speed=30.0,
lead_relevancy=True,
initial_distance_lead=distance,
speed_lead_values=[30.0],
breakpoints=[0.],
)
valid, results[distance] = man.evaluate()
labels.append(f'{distance} m initial distance')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
results = {}
name = 'Speed profile when converging to steady state lead at 20m/s'
labels = []
for distance in np.arange(20, 140, 10):
man = Maneuver(
'',
duration=50,
initial_speed=20.0,
lead_relevancy=True,
initial_distance_lead=distance,
speed_lead_values=[20.0],
breakpoints=[0.],
)
valid, results[distance] = man.evaluate()
labels.append(f'{distance} m initial distance')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
results = {}
name = 'Following car at 30m/s that comes to a stop'
labels = []
for stop_time in np.arange(4, 14, 1):
man = Maneuver(
'',
duration=30,
initial_speed=30.0,
cruise_values=[30.0, 30.0, 30.0],
lead_relevancy=True,
initial_distance_lead=60.0,
speed_lead_values=[30.0, 30.0, 0.0],
breakpoints=[0., 5., 5 + stop_time],
)
valid, results[stop_time] = man.evaluate()
labels.append(f'{stop_time} seconds stop time')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
results = {}
name = 'Response to cut-in at half follow distance'
labels = []
for speed in np.arange(0, 40, 5):
man = Maneuver(
'',
duration=20,
initial_speed=float(speed),
cruise_values=[speed, speed, speed],
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(speed, speed) / 2,
speed_lead_values=[speed, speed, speed],
prob_lead_values=[0.0, 0.0, 1.0],
breakpoints=[0., 5.0, 5.01],
)
valid, results[speed] = man.evaluate()
labels.append(f'{speed} m/s speed')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
results = {}
name = 'Follow a lead that accelerates at 2m/s^2 until steady state speed'
labels = []
for speed in np.arange(0, 40, 5):
man = Maneuver(
'',
duration=60,
initial_speed=0.0,
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(0.0, 0.0),
speed_lead_values=[0.0, 0.0, speed],
prob_lead_values=[1.0, 1.0, 1.0],
breakpoints=[0., 1.0, speed / 2],
)
valid, results[speed] = man.evaluate()
labels.append(f'{speed} m/s speed')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
results = {}
name = 'From stop to cruise'
labels = []
for speed in np.arange(0, 40, 5):
man = Maneuver(
'',
duration=50,
initial_speed=0.0,
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(0.0, 0.0),
speed_lead_values=[0.0, 0.0],
cruise_values=[0.0, speed],
prob_lead_values=[0.0, 0.0],
breakpoints=[1., 1.01],
)
valid, results[speed] = man.evaluate()
labels.append(f'{speed} m/s speed')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
results = {}
name = 'From cruise to min'
labels = []
for speed in np.arange(10, 40, 5):
man = Maneuver(
'',
duration=50,
initial_speed=float(speed),
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(0.0, 0.0),
speed_lead_values=[0.0, 0.0],
cruise_values=[speed, 10.0],
prob_lead_values=[0.0, 0.0],
breakpoints=[1., 1.01],
)
valid, results[speed] = man.evaluate()
labels.append(f'{speed} m/s speed')
htmls.append(f'<h1>{name}</h1>')
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
return htmls
if __name__ == '__main__':
htmls = generate_mpc_tuning_report()
if len(sys.argv) < 2:
file_name = 'long_mpc_tune_report.html'
else:
file_name = sys.argv[1]
with open(file_name, 'w') as f:
f.write('<h1>MPC longitudinal tuning report</h1>')
for html in htmls:
f.write(html)

244
iqpilot/tools/maneuvers/sim_harness.py Normal file
View File

@@ -0,0 +1,244 @@
#!/usr/bin/env python3
"""Closed-loop offline harness for the maneuver daemons.
Runs maneuversd / lateral_maneuversd as real subprocesses over msgq, drives them with a
synthetic vehicle, and records every message to an rlog that the report generators can read.
Used to validate the maneuver tooling without a car.
"""
import math
import os
import signal
import subprocess
import sys
import time
from pathlib import Path
from typing import NamedTuple
import numpy as np
import zstandard as zstd
from iqpilot.cereal import car, messaging
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_CTRL, Ratekeeper
from iqpilot.common.basedir import BASEDIR
PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
PUB_20HZ = ('modelV2', 'deviceMotion', 'vehicleParameters')
SUB = ('alertDebug', 'longitudinalPlan', 'lateralManeuverPlan')
STEER_RATIO = 15.0
WHEELBASE = 2.78
class LongPlan(NamedTuple):
aTarget: float
shouldStop: bool
class LatPlan(NamedTuple):
desiredCurvature: float
class Plant:
"""Vehicle model. Subclasses consume the daemon's plan and fill the published messages."""
sim = None
PLAN = 'longitudinalPlan'
def __init__(self, v_ego: float = 0.0):
self.v_ego = v_ego
self.a_ego = 0.0
self.curvature = 0.0 # commanded, controlsState.desiredCurvature
self.achieved_curvature = 0.0 # measured, controlsState.curvature
self.lat_accel = 0.0
self.long_active = True
self.lat_active = True
self.steering_pressed = False
self.gas_pressed = False
def step(self, dt: float, plan) -> None:
raise NotImplementedError
def _angle(self, curvature: float) -> float:
return math.degrees(curvature * WHEELBASE * STEER_RATIO)
def _torque(self, curvature: float) -> float:
return float(np.clip(curvature * max(self.v_ego, 1.0) ** 2 / 3.0, -1.0, 1.0))
def fill_car_state(self, cs) -> None:
cs.vEgo = float(self.v_ego)
cs.vEgoRaw = float(self.v_ego)
cs.vEgoCluster = float(self.v_ego)
cs.aEgo = float(self.a_ego)
cs.standstill = self.v_ego < 0.01
cs.steeringPressed = self.steering_pressed
cs.gasPressed = self.gas_pressed
cs.steeringAngleDeg = self._angle(self.achieved_curvature)
cs.cruiseState.enabled = True
cs.cruiseState.available = True
cs.cruiseState.speed = float(max(self.v_ego, 1.0))
def fill_car_control(self, cc) -> None:
cc.enabled = True
cc.latActive = self.lat_active
cc.longActive = self.long_active
cc.orientationNED = [0.0, 0.0, 0.0]
cc.actuators.curvature = float(self.curvature)
cc.actuators.accel = float(self.a_ego)
cc.actuators.steeringAngleDeg = self._angle(self.curvature)
cc.actuators.torque = self._torque(self.curvature)
class ManeuverSim:
def __init__(self, module: str, plant: Plant, fingerprint: str = "TOYOTA_SIENNA",
max_maneuvers: int = 0, timeout: float = 600.0, verbose: bool = True):
self.module = module
self.plant = plant
plant.sim = self
self.fingerprint = fingerprint
self.max_maneuvers = max_maneuvers
self.timeout = timeout
self.verbose = verbose
self.events: list[bytes] = []
self.alert1 = ''
self.alert2 = ''
self.seen_maneuvers: list[str] = []
self.finished = False
def _write_car_params(self):
CP = car.CarParams.new_message()
CP.carFingerprint = self.fingerprint
CP.brand = "toyota"
CP.openpilotLongitudinalControl = True
CP.autoResumeSng = True
CP.steerRatio = STEER_RATIO
CP.wheelbase = WHEELBASE
Params().put("CarParams", CP.to_bytes())
return CP
def _head_events(self, CP):
init = messaging.new_message('initData')
init.valid = True
init.initData.gitCommit = "simulated"
init.initData.gitBranch = "sim"
init.initData.gitRemote = "iqpilot-sim"
self.events.append(init.to_bytes())
cpm = messaging.new_message('carParams')
cpm.valid = True
cpm.carParams = CP
self.events.append(cpm.to_bytes())
def _launch(self):
env = dict(os.environ)
env["PYTHONPATH"] = str(BASEDIR) + os.pathsep + env.get("PYTHONPATH", "")
return subprocess.Popen([sys.executable, "-c", f"from {self.module} import main; main()"],
cwd=str(BASEDIR), env=env, start_new_session=True)
def _on_alert(self, ad):
text1, text2 = ad.alertText1, ad.alertText2
if (text1, text2) != (self.alert1, self.alert2):
if self.verbose:
print(f" [{time.monotonic() - self.t_start:6.1f}s] {text1!r} | {text2!r}")
if text2 and text2 not in self.seen_maneuvers:
self.seen_maneuvers.append(text2)
if text1 == 'Maneuvers Finished':
self.finished = True
self.alert1, self.alert2 = text1, text2
def run(self, out: Path) -> Path:
self._head_events(self._write_car_params())
pm = messaging.PubMaster(list(PUB_100HZ) + list(PUB_20HZ))
socks = {s: messaging.sub_sock(s, conflate=False, timeout=0) for s in SUB}
proc = self._launch()
self.t_start = time.monotonic()
rk = Ratekeeper(int(1.0 / DT_CTRL), print_delay_threshold=None)
plans: dict[str, object | None] = {'longitudinalPlan': None, 'lateralManeuverPlan': None}
frame = 0
try:
while True:
for s, sock in socks.items():
while True:
raw = sock.receive(non_blocking=True)
if raw is None:
break
self.events.append(raw)
evt = messaging.log_from_bytes(raw)
if s == 'alertDebug':
self._on_alert(evt.alertDebug)
elif s == 'longitudinalPlan':
plans[s] = LongPlan(evt.longitudinalPlan.aTarget, evt.longitudinalPlan.shouldStop)
elif s == 'lateralManeuverPlan':
plans[s] = LatPlan(evt.lateralManeuverPlan.desiredCurvature) if evt.valid else None
self.plant.step(DT_CTRL, plans[self.plant.PLAN])
for s in PUB_100HZ:
raw = self._build(s).to_bytes()
self.events.append(raw)
pm.send(s, raw)
if frame % 5 == 0:
for s in PUB_20HZ:
raw = self._build(s).to_bytes()
self.events.append(raw)
pm.send(s, raw)
frame += 1
if self.finished:
break
if self.max_maneuvers and len(self.seen_maneuvers) > self.max_maneuvers:
break
if time.monotonic() - self.t_start > self.timeout:
print(" timed out")
break
rk.keep_time()
finally:
if proc.poll() is None:
os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
proc.wait(timeout=5)
for sock in socks.values():
del sock
out.parent.mkdir(parents=True, exist_ok=True)
out.write_bytes(zstd.compress(b"".join(self.events), 10))
return out
def _build(self, s: str):
msg = messaging.new_message(s)
msg.valid = True
if s == 'carState':
self.plant.fill_car_state(msg.carState)
elif s == 'carControl':
self.plant.fill_car_control(msg.carControl)
elif s == 'carOutput':
msg.carOutput.actuatorsOutput.accel = float(self.plant.a_ego)
msg.carOutput.actuatorsOutput.curvature = float(self.plant.curvature)
msg.carOutput.actuatorsOutput.steeringAngleDeg = self.plant._angle(self.plant.achieved_curvature)
msg.carOutput.actuatorsOutput.torque = self.plant._torque(self.plant.achieved_curvature)
elif s == 'controlsState':
msg.controlsState.curvature = float(self.plant.achieved_curvature)
msg.controlsState.desiredCurvature = float(self.plant.curvature)
elif s == 'selfdriveState':
msg.selfdriveState.enabled = True
msg.selfdriveState.active = True
msg.selfdriveState.state = 'enabled'
elif s == 'modelV2':
msg.modelV2.frameId = 0
msg.modelV2.action.desiredCurvature = 0.0
elif s == 'deviceMotion':
msg.deviceMotion.accelerationDevice.x = float(self.plant.a_ego)
msg.deviceMotion.accelerationDevice.y = float(self.plant.lat_accel)
msg.deviceMotion.velocityDevice.x = float(self.plant.v_ego)
msg.deviceMotion.inputsOK = True
msg.deviceMotion.posenetOK = True
msg.deviceMotion.sensorsOK = True
elif s == 'vehicleParameters':
msg.vehicleParameters.valid = True
msg.vehicleParameters.roll = 0.0
msg.vehicleParameters.steerRatio = STEER_RATIO
return msg

87
iqpilot/tools/maneuvers/simulate_lateral.py Executable file
View File

@@ -0,0 +1,87 @@
#!/usr/bin/env python3
"""Run lateral_maneuversd against a synthetic lateral plant and write an rlog.
./iqpilot/tools/maneuvers/simulate_lateral.py --out /tmp/lat_rlog.zst
./iqpilot/tools/maneuvers/lateral_report.py /tmp/lat_rlog.zst
"""
import argparse
import re
from pathlib import Path
from iqpilot.common.constants import CV
from iqpilot.tools.maneuvers.lateral_maneuversd import MANEUVERS
from iqpilot.tools.maneuvers.sim_harness import ManeuverSim, Plant
CURV_TAU = 0.05 # controlsd curvature command tracking
RACK_WN = 8.0 # steering rack + tire natural frequency (rad/s)
RACK_ZETA = 0.7 # underdamped, so achieved curvature overshoots like a real rack
CRUISE_ACCEL = 1.2
SET_SPEED_RE = re.compile(r"Set speed to (\d+) mph")
class LateralPlant(Plant):
PLAN = 'lateralManeuverPlan'
def __init__(self, steer_input: float = 0.0):
super().__init__(v_ego=MANEUVERS[0].initial_speed)
self.sim = None
self._rack_rate = 0.0
self.target_speed = MANEUVERS[0].initial_speed
self._by_description = {m.description: m.initial_speed for m in MANEUVERS}
# seconds of steeringPressed to assert at each maneuver start, mimicking the driver-torque
# spike a curvature step produces on a car with a tight override threshold
self.steer_input = steer_input
self._steer_hold = 0.0
self._was_active = False
def _update_target(self):
if self.sim is None:
return
speed = self._by_description.get(self.sim.alert2)
if speed is None:
match = SET_SPEED_RE.search(self.sim.alert1)
speed = float(match.group(1)) * CV.MPH_TO_MS if match else None
if speed is not None:
self.target_speed = speed
def step(self, dt, plan):
self._update_target()
active = plan is not None
if self.steer_input > 0 and active and not self._was_active:
self._steer_hold = self.steer_input
self._was_active = active
self.steering_pressed = self._steer_hold > 0
self._steer_hold = max(self._steer_hold - dt, 0.0)
err = self.target_speed - self.v_ego
self.a_ego = max(min(err / 1.0, CRUISE_ACCEL), -CRUISE_ACCEL)
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
desired_curvature = float(plan.desiredCurvature) if plan is not None else 0.0
self.curvature += (dt / (CURV_TAU + dt)) * (desired_curvature - self.curvature)
self._rack_rate += dt * (RACK_WN ** 2 * (self.curvature - self.achieved_curvature) - 2 * RACK_ZETA * RACK_WN * self._rack_rate)
self.achieved_curvature += dt * self._rack_rate
self.lat_accel = self.achieved_curvature * max(self.v_ego, 1.0) ** 2
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--out", type=Path, default=Path("/tmp/lateral_maneuvers_sim/rlog.zst"))
parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
parser.add_argument("--timeout", type=float, default=900.0)
parser.add_argument("--steer-input", type=float, default=0.0,
help="seconds of steeringPressed to assert at each maneuver start (0 = hands off)")
args = parser.parse_args()
sim = ManeuverSim("iqpilot.tools.maneuvers.lateral_maneuversd", LateralPlant(args.steer_input),
max_maneuvers=args.max_maneuvers, timeout=args.timeout)
out = sim.run(args.out)
print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
if __name__ == "__main__":
main()

51
iqpilot/tools/maneuvers/simulate_longitudinal.py Executable file
View File

@@ -0,0 +1,51 @@
#!/usr/bin/env python3
"""Run maneuversd against a synthetic longitudinal plant and write an rlog.
./iqpilot/tools/maneuvers/simulate_longitudinal.py --out /tmp/long_rlog.zst
./iqpilot/tools/maneuvers/longitudinal_report.py /tmp/long_rlog.zst
"""
import argparse
from pathlib import Path
from iqpilot.tools.maneuvers.sim_harness import ManeuverSim, Plant
WN = 6.0 # powertrain natural frequency (rad/s)
ZETA = 0.6 # underdamped, so actual accel overshoots the target like a real car
class LongitudinalPlant(Plant):
def __init__(self):
super().__init__()
self.jerk = 0.0
def step(self, dt, plan):
a_target = float(plan.aTarget) if plan is not None else 0.0
if plan is not None and plan.shouldStop:
a_target = min(a_target, -0.5)
self.jerk += dt * (WN ** 2 * (a_target - self.a_ego) - 2 * ZETA * WN * self.jerk)
self.a_ego += dt * self.jerk
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
if self.v_ego <= 0.0:
self.a_ego = min(self.a_ego, 0.0)
self.jerk = min(self.jerk, 0.0)
self.lat_accel = 0.0
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--out", type=Path, default=Path("/tmp/longitudinal_maneuvers_sim/rlog.zst"))
parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
parser.add_argument("--timeout", type=float, default=900.0)
args = parser.parse_args()
sim = ManeuverSim("iqpilot.tools.maneuvers.longitudinal_maneuversd", LongitudinalPlant(),
max_maneuvers=args.max_maneuvers, timeout=args.timeout)
out = sim.run(args.out)
print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
if __name__ == "__main__":
main()

108
iqpilot/tools/scons/site_init.py Normal file
View File

@@ -0,0 +1,108 @@
import os
import sys
import SCons.Script.Main as _main
try:
from site_tools import pretty as _pretty
except Exception:
_pretty = None
def _tty():
return sys.stdout.isatty() and not os.environ.get("NO_COLOR")
_DIM = "\033[2;38;5;246m"
_BLUE = "\033[38;5;111m"
_GREEN = "\033[38;5;114m"
_RED = "\033[38;5;203m"
_RST = "\033[0m"
_READING = "scons: Reading SConscript files ..."
_PHASES = {
_READING: f"{_DIM}reading sconscripts…{_RST}",
"scons: done reading SConscript files.": f"{_DIM}sconscripts read{_RST}",
"scons: Building targets ...": f"{_BLUE}building…{_RST}",
"scons: done building targets.": f"{_GREEN}✓ build complete{_RST}",
"scons: done building targets (errors occurred during build).": f"{_RED}✗ build failed{_RST}",
"scons: writing .sconsign file.": f"{_DIM}writing .sconsign{_RST}",
"scons: Cleaning targets ...": f"{_DIM}cleaning…{_RST}",
"scons: done cleaning targets.": f"{_GREEN}✓ clean complete{_RST}",
"scons: done cleaning targets (errors occurred during clean).": f"{_RED}✗ clean failed{_RST}",
}
def _phase(text):
if _tty() and isinstance(text, str) and text in _PHASES:
return _PHASES[text]
return text
def _clean(text):
if not (_tty() and _pretty and isinstance(text, str)):
return text
for prefix in ("Removed directory ", "Removed "):
if text.startswith(prefix):
return _pretty._format("CLEAN", text[len(prefix):])
return text
class _Restyle:
# delegates unknown attrs (.set_mode etc.) to the wrapped DisplayEngine
def __init__(self, orig, transform):
self._orig = orig
self._transform = transform
def __call__(self, text, *args, **kwargs):
return self._orig(self._transform(text), *args, **kwargs)
def __getattr__(self, name):
return getattr(self._orig, name)
if not isinstance(_main.progress_display, _Restyle):
_main.progress_display = _Restyle(_main.progress_display, _phase)
if not isinstance(_main.display, _Restyle):
_main.display = _Restyle(_main.display, _clean)
# SConstruct imports this instead of scons auto-loading a root site_scons dir, so the reading
# banner is already on screen by now; rewrite that one line in place
if not getattr(_main, "_iq_banner_restyled", False):
_main._iq_banner_restyled = True
if _tty() and _main.progress_display.print_it:
sys.stdout.write(f"\033[F\033[2K{_PHASES[_READING]}\n")
sys.stdout.flush()
# drop only "Could not remove ... No such file" during clean; real errors still print
import builtins as _builtins
def _wrap_clean(orig):
def wrapper(self, *args, **kwargs):
if getattr(_builtins.print, "_iq_clean", False):
return orig(self, *args, **kwargs)
real = _builtins.print
def filtered(*a, **k):
if a and isinstance(a[0], str) and a[0].startswith("scons: Could not remove"):
if "No such file" in " ".join(str(x) for x in a):
return
return real(*a, **k)
filtered._iq_clean = True
_builtins.print = filtered
try:
return orig(self, *args, **kwargs)
finally:
_builtins.print = real
return wrapper
if not getattr(_main.CleanTask.fs_delete, "_iq_wrapped", False):
_main.CleanTask.fs_delete = _wrap_clean(_main.CleanTask.fs_delete)
_main.CleanTask.remove = _wrap_clean(_main.CleanTask.remove)
_main.CleanTask.fs_delete._iq_wrapped = True
_main.CleanTask.remove._iq_wrapped = True

82
iqpilot/tools/scons/site_tools/cython.py Normal file
View File

@@ -0,0 +1,82 @@
import re
import sys
import SCons
from SCons.Action import Action
from SCons.Scanner import Scanner
import numpy as np
pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M)
pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M)
cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M)
np_version = SCons.Script.Value(np.__version__)
def pyx_scan(node, env, path, arg=None):
contents = node.get_text_contents()
env.Depends(str(node).split('.')[0] + env['CYTHONCFILESUFFIX'], np_version)
# from <module> cimport ...
matches = pyx_from_import_re.findall(contents)
# cimport <module>
matches += pyx_import_re.findall(contents)
# Modules can be either .pxd or .pyx files
files = [m.replace('.', '/') + '.pxd' for m in matches]
files += [m.replace('.', '/') + '.pyx' for m in matches]
# cdef extern from <file>
files += cdef_import_re.findall(contents)
# Handle relative imports
cur_dir = str(node.get_dir())
files = [cur_dir + f if f.startswith('/') else f for f in files]
# Filter out non-existing files (probably system imports)
files = [f for f in files if env.File(f).exists()]
return env.File(files)
pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True)
cythonAction = Action("$CYTHONCOM", "$CYTHONCOMSTR")
def create_builder(env):
try:
cython = env['BUILDERS']['Cython']
except KeyError:
cython = SCons.Builder.Builder(
action=cythonAction,
emitter={},
suffix=cython_suffix_emitter,
single_source=1
)
env.Append(SCANNERS=pyxscanner)
env['BUILDERS']['Cython'] = cython
return cython
def cython_suffix_emitter(env, source):
return "$CYTHONCFILESUFFIX"
def generate(env):
env["CYTHON"] = f'"{sys.executable}" -m Cython.Build.Cythonize'
# drop cythonize's stdout progress; errors stay on stderr. kept under --verbose
try:
from SCons.Script import GetOption
quiet = not GetOption("verbose")
except Exception:
quiet = True
env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE" + (" > /dev/null" if quiet else "")
env["CYTHONCFILESUFFIX"] = ".cpp"
c_file, _ = SCons.Tool.createCFileBuilders(env)
c_file.suffix['.pyx'] = cython_suffix_emitter
c_file.add_action('.pyx', cythonAction)
c_file.suffix['.py'] = cython_suffix_emitter
c_file.add_action('.py', cythonAction)
create_builder(env)
def exists(env):
return True

122
iqpilot/tools/scons/site_tools/pretty.py Normal file
View File

@@ -0,0 +1,122 @@
import os
import sys
from SCons.Action import Action
from SCons.Script import GetOption
_GRAD = {
"CC": ((95, 215, 255), (70, 130, 245)),
"CXX": ((95, 205, 255), (130, 110, 250)),
"LINK": ((95, 240, 150), (40, 200, 120)),
"AR": ((120, 235, 220), (40, 175, 185)),
"RANLIB": ((140, 240, 225), (45, 165, 180)),
"SKIP": ((150, 160, 185), (100, 110, 145)),
"CLEAN": ((200, 130, 145), (120, 120, 155)),
"OBJCOPY": ((255, 200, 90), (255, 120, 50)),
"SIGN": ((255, 190, 90), (230, 80, 70)),
"CYTHON": ((215, 130, 255), (140, 80, 250)),
"CAPNP": ((255, 120, 225), (190, 90, 255)),
"RCC": ((200, 160, 255), (150, 110, 250)),
"FONTS": ((130, 190, 255), (90, 120, 250)),
"GEN": ((160, 160, 255), (120, 90, 250)),
"CDB": ((150, 175, 210), (105, 135, 185)),
"MODEL": ((255, 170, 70), (240, 60, 60)),
"META": ((255, 205, 110), (230, 130, 80)),
"MOC": ((255, 140, 205), (215, 90, 240)),
"UIC": ((255, 160, 190), (225, 110, 225)),
"MO": ((100, 235, 200), (55, 200, 155)),
}
_DEFAULT = ((120, 200, 255), (90, 140, 250))
_TARGET_RGB = (150, 152, 178)
def _mode():
if not sys.stdout.isatty() or os.environ.get("NO_COLOR"):
return None
if os.environ.get("COLORTERM", "").lower() in ("truecolor", "24bit"):
return "true"
return "256"
def _fg(rgb, mode):
r, g, b = rgb
if mode == "true":
return f"\033[38;2;{r};{g};{b}m"
if abs(r - g) < 12 and abs(g - b) < 12 and abs(r - b) < 12:
idx = 232 + min(23, round((r + g + b) / 3 / 255 * 23))
else:
idx = 16 + 36 * round(r / 255 * 5) + 6 * round(g / 255 * 5) + round(b / 255 * 5)
return f"\033[38;5;{idx}m"
def _gradient(word, start, end, mode):
n = max(1, len(word) - 1)
out = []
for i, ch in enumerate(word):
t = i / n
rgb = tuple(int(s + (e - s) * t) for s, e in zip(start, end))
out.append(f"\033[1m{_fg(rgb, mode)}{ch}")
return "".join(out) + "\033[0m"
def _format(label, body):
mode = _mode()
if mode is None:
return f"{label:>8} {body}"
start, end = _GRAD.get(label, _DEFAULT)
pad = " " * max(0, 8 - len(label))
word = _gradient(label, start, end, mode)
return f"{pad}{word} {_fg(_TARGET_RGB, mode)}{body}\033[0m"
def _line(label):
return _format(label, "$TARGET")
def _verbose():
try:
return bool(GetOption("verbose"))
except Exception:
return False
def generate(env):
def pretty_action(e, cmd, label, logfile=None, capture_stderr=False):
if _verbose():
return Action(cmd)
if callable(cmd):
return Action(cmd, _line(label))
if logfile:
cmd = f"{cmd} > {logfile}" + (" 2>&1" if capture_stderr else "")
return Action(cmd, _line(label))
env.AddMethod(pretty_action, "PrettyAction")
env.AddMethod(lambda e, label, msg: _format(label, msg), "PrettyNote")
# real errors are exceptions, not warnings, so they still surface with these ignored
env["PYWARN"] = "" if _verbose() else "PYTHONWARNINGS=ignore::UserWarning"
if _verbose():
return
try:
import SCons.CacheDir as _cachedir
_cachedir.CacheRetrieve.strfunction = lambda target, source, env: ""
except Exception:
pass
env["CCCOMSTR"] = _line("CC")
env["SHCCCOMSTR"] = _line("CC")
env["CXXCOMSTR"] = _line("CXX")
env["SHCXXCOMSTR"] = _line("CXX")
env["ASCOMSTR"] = _line("CC")
env["ASPPCOMSTR"] = _line("CC")
env["LINKCOMSTR"] = _line("LINK")
env["SHLINKCOMSTR"] = _line("LINK")
env["ARCOMSTR"] = _line("AR")
env["RANLIBCOMSTR"] = _line("RANLIB")
env["CYTHONCOMSTR"] = _line("CYTHON")
env["COMPILATIONDB_COMSTR"] = _line("CDB")
def exists(env):
return True

187
iqpilot/tools/setup.sh Executable file
View File

@@ -0,0 +1,187 @@
#!/usr/bin/env bash
set -euo pipefail
RED='\033[0;31m'
GREEN='\033[0;32m'
BOLD='\033[1m'
NC='\033[0m'
if [ -z "$OPENPILOT_ROOT" ]; then
# default to current directory for installation
OPENPILOT_ROOT="$(pwd)/openpilot"
fi
function show_motd() {
cat << 'EOF'
.~ssos+.
+8888888888i,
{888888888888o.
h8888888888888k
t888888888s888k
`t88888d/ h88k
``` h88l
,88k`
.d8h`
+d8h
_+d8h`
;y8h+`
|-`
openpilot installer
EOF
}
function sentry_send_event() {
SENTRY_KEY=dd0cba62ba0ac07ff9f388f8f1e6a7f4
SENTRY_URL=https://sentry.io/api/4507726145781760/store/
EVENT=$1
EVENT_TYPE=${2:-$EVENT}
EVENT_LOG=${3:-"NA"}
PLATFORM=$(uname -s)
ARCH=$(uname -m)
SYSTEM=$(uname -a)
if [[ $PLATFORM == "Darwin" ]]; then
OS="macos"
elif [[ $PLATFORM == "Linux" ]]; then
OS="linux"
fi
if [[ $ARCH == armv8* ]] || [[ $ARCH == arm64* ]] || [[ $ARCH == aarch64* ]]; then
ARCH="aarch64"
elif [[ $ARCH == "x86_64" ]] || [[ $ARCH == i686* ]]; then
ARCH="x86"
fi
PYTHON_VERSION=$(echo $(python3 --version 2> /dev/null || echo "NA"))
BRANCH=$(echo $(git -C $OPENPILOT_ROOT rev-parse --abbrev-ref HEAD 2> /dev/null || echo "NA"))
COMMIT=$(echo $(git -C $OPENPILOT_ROOT rev-parse HEAD 2> /dev/null || echo "NA"))
curl -s -o /dev/null -X POST -g --data "{ \"exception\": { \"values\": [{ \"type\": \"$EVENT\" }] }, \"tags\" : { \"event_type\" : \"$EVENT_TYPE\", \"event_log\" : \"$EVENT_LOG\", \"os\" : \"$OS\", \"arch\" : \"$ARCH\", \"python_version\" : \"$PYTHON_VERSION\" , \"git_branch\" : \"$BRANCH\", \"git_commit\" : \"$COMMIT\", \"system\" : \"$SYSTEM\" } }" \
-H 'Content-Type: application/json' \
-H "X-Sentry-Auth: Sentry sentry_version=7, sentry_key=$SENTRY_KEY, sentry_client=op_setup/0.1" \
$SENTRY_URL 2> /dev/null
}
function check_stdin() {
if [ -t 0 ]; then
INTERACTIVE=1
else
echo "Checking for valid invocation..."
echo -e " ↳ [${RED}✗${NC}] stdin not found! Running in non-interactive mode."
echo -e " Run ${BOLD}'bash <(curl -fsSL openpilot.comma.ai)'${NC} to run in interactive mode.\n"
fi
}
function ask_dir() {
echo -n "Enter directory in which to install openpilot (default $OPENPILOT_ROOT): "
if [[ -z $INTERACTIVE ]]; then
echo -e "\nBecause your are running in non-interactive mode, the installation"
echo -e "will default to $OPENPILOT_ROOT\n"
return 0
fi
read
if [[ ! -z "$REPLY" ]]; then
mkdir -p $REPLY
OPENPILOT_ROOT="$(realpath $REPLY)/openpilot"
fi
}
function check_dir() {
echo "Checking for installation directory..."
if [ -d "$OPENPILOT_ROOT" ]; then
echo -e " ↳ [${RED}✗${NC}] Installation destination $OPENPILOT_ROOT already exists!"
# not a valid clone, can't continue
if [[ ! -z "$(ls -A $OPENPILOT_ROOT)" && ! -f "$OPENPILOT_ROOT/launch_openpilot.sh" ]]; then
echo -e " $OPENPILOT_ROOT already contains files but does not seems"
echo -e " to be a valid openpilot git clone. Choose another location for"
echo -e " installing openpilot!\n"
return 1
fi
# already a "valid" openpilot clone, skip cloning again
if [[ ! -z "$(ls -A $OPENPILOT_ROOT)" ]]; then
SKIP_GIT_CLONE=1
fi
# by default, don't try installing in already existing directory
if [[ -z $INTERACTIVE ]]; then
return 0
fi
read -p " Would you like to attempt installation anyway? [Y/n] " -n 1 -r
echo -e "\n"
if [[ ! $REPLY =~ ^[Yy]$ ]]; then
return 1
fi
return 0
fi
echo -e " ↳ [${GREEN}✔${NC}] Successfully chosen $OPENPILOT_ROOT as installation directory\n"
}
function check_git() {
echo "Checking for git..."
if ! command -v "git" > /dev/null 2>&1; then
echo -e " ↳ [${RED}✗${NC}] git not found on your system, can't continue!"
sentry_send_event "SETUP_FAILURE" "ERROR_GIT_NOT_FOUND"
return 1
else
echo -e " ↳ [${GREEN}✔${NC}] git found.\n"
fi
}
function git_clone() {
st="$(date +%s)"
echo "Cloning openpilot..."
if $(git clone --filter=blob:none https://github.com/commaai/openpilot.git "$OPENPILOT_ROOT"); then
if [[ -f $OPENPILOT_ROOT/launch_openpilot.sh ]]; then
et="$(date +%s)"
echo -e " ↳ [${GREEN}✔${NC}] Successfully cloned openpilot in $((et - st)) seconds.\n"
return 0
fi
fi
echo -e " ↳ [${RED}✗${NC}] failed to clone openpilot!"
sentry_send_event "SETUP_FAILURE" "ERROR_GIT_CLONE"
return 1
}
function install_with_iq() {
cd $OPENPILOT_ROOT
$OPENPILOT_ROOT/iqpilot/tools/iq.sh install
LOG_FILE=$(mktemp)
if ! $OPENPILOT_ROOT/iqpilot/tools/iq.sh --log $LOG_FILE setup; then
echo -e "\n[${RED}✗${NC}] failed to install openpilot!"
ERROR_TYPE="$(cat "$LOG_FILE" | sed '1p;d')"
ERROR_LOG="$(cat "$LOG_FILE" | sed '2p;d')"
sentry_send_event "SETUP_FAILURE" "$ERROR_TYPE" "$ERROR_LOG" || true
return 1
else
sentry_send_event "SETUP_SUCCESS" || true
fi
echo -e "\n----------------------------------------------------------------------"
echo -e "[${GREEN}✔${NC}] openpilot was successfully installed into ${BOLD}$OPENPILOT_ROOT${NC}"
echo -e "Checkout the docs at https://docs.comma.ai"
echo -e "Checkout how to contribute at https://github.com/commaai/openpilot/blob/master/docs/CONTRIBUTING.md"
}
show_motd
check_stdin
ask_dir
check_dir
check_git
[ -z $SKIP_GIT_CLONE ] && git_clone
install_with_iq

178
iqpilot/tools/test_tools_local_route.py Normal file
View File

@@ -0,0 +1,178 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import os
import pty
import select
import shutil
import subprocess
import time
from pathlib import Path
import pytest
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import log
TOOLS_DIR = Path(__file__).parent
CABANA_BIN = TOOLS_DIR / "cabana" / "_cabana"
JOTPLUGGLER_BIN = TOOLS_DIR / "jotpluggler" / "jotpluggler"
DONGLE_ID = "0000000000000000"
TIMESTAMP = "2024-01-01--00-00-00"
ROUTE = f"{DONGLE_ID}|{TIMESTAMP}"
def write_rlog(path: Path, n_frames: int = 200):
with open(path, "wb") as f:
cp = messaging.new_message('carParams')
cp.carParams.carFingerprint = "TOYOTA_RAV4_TSS2"
cp.carParams.brand = "toyota"
f.write(cp.to_bytes())
for i in range(n_frames):
msg = messaging.new_message('can', 2)
msg.logMonoTime = int(i * 1e7)
for j, addr in enumerate((0x1D2, 0x260)):
msg.can[j].address = addr
msg.can[j].src = 0
msg.can[j].dat = bytes([i % 256] * 8)
f.write(msg.to_bytes())
def write_video_rlog(path: Path, n_frames: int):
with open(path, "wb") as f:
cp = messaging.new_message('carParams')
cp.logMonoTime = 1_000_000_000
cp.carParams.carFingerprint = "TOYOTA_RAV4_TSS2"
cp.carParams.brand = "toyota"
f.write(cp.to_bytes())
for i in range(n_frames):
timestamp = 1_000_000_000 + i * 50_000_000
msg = messaging.new_message('can', 1)
msg.logMonoTime = timestamp
msg.can[0].address = 0x1D2
msg.can[0].src = 0
msg.can[0].dat = bytes([i % 256] * 8)
f.write(msg.to_bytes())
idx = messaging.new_message('roadEncodeIdx')
idx.logMonoTime = timestamp
idx.roadEncodeIdx.frameId = i
idx.roadEncodeIdx.type = 'fullHEVC'
idx.roadEncodeIdx.encodeId = i
idx.roadEncodeIdx.segmentNum = 0
idx.roadEncodeIdx.segmentId = i
idx.roadEncodeIdx.segmentIdEncode = i
idx.roadEncodeIdx.timestampSof = timestamp
idx.roadEncodeIdx.timestampEof = timestamp + 10_000_000
f.write(idx.to_bytes())
@pytest.fixture(scope="module")
def local_route(tmp_path_factory):
data_dir = tmp_path_factory.mktemp("routes")
for seg in range(2):
seg_dir = data_dir / f"{DONGLE_ID}|{TIMESTAMP}--{seg}"
seg_dir.mkdir()
write_rlog(seg_dir / "rlog")
return data_dir
@pytest.fixture(scope="module")
def local_video_route(tmp_path_factory):
data_dir = tmp_path_factory.mktemp("video_routes")
seg_dir = data_dir / f"{DONGLE_ID}|{TIMESTAMP}--0"
seg_dir.mkdir()
frame_count = 20
result = subprocess.run([
"ffmpeg", "-hide_banner", "-loglevel", "error", "-f", "lavfi",
"-i", "testsrc=size=320x180:rate=20", "-frames:v", str(frame_count),
"-pix_fmt", "yuv420p", "-c:v", "libx265", "-preset", "ultrafast",
"-x265-params", "pools=1:frame-threads=1:log-level=error", "-f", "hevc",
str(seg_dir / "fcamera.hevc"),
], capture_output=True, text=True)
if result.returncode != 0:
pytest.skip(result.stderr)
write_video_rlog(seg_dir / "rlog", frame_count)
return data_dir
def run(cmd, timeout=180):
return subprocess.run(cmd, capture_output=True, text=True, timeout=timeout, env=os.environ.copy(),
cwd=TOOLS_DIR.parent)
def cabana_command(*args):
command = [str(CABANA_BIN), *args]
if os.uname().sysname == "Linux" and shutil.which("xvfb-run"):
command = ["xvfb-run", "-a", *command]
return command
def cabana_output_until(args, expected, timeout=60):
master, slave = pty.openpty()
proc = subprocess.Popen(cabana_command(*args), stdout=slave, stderr=slave,
env=os.environ.copy(), cwd=TOOLS_DIR.parent)
os.close(slave)
output = bytearray()
deadline = time.monotonic() + timeout
try:
while time.monotonic() < deadline:
ready, _, _ = select.select([master], [], [], min(1, deadline - time.monotonic()))
if not ready:
if proc.poll() is not None:
break
continue
try:
output.extend(os.read(master, 4096))
except OSError:
break
if expected.encode() in output:
break
finally:
proc.kill()
proc.wait()
os.close(master)
return output.decode(errors="replace")
def test_jotpluggler_renders_a_local_route(local_route, tmp_path):
assert JOTPLUGGLER_BIN.exists(), "jotpluggler not built"
out = tmp_path / "plot.png"
result = run([str(JOTPLUGGLER_BIN), "--data-dir", str(local_route),
"--sync-load", "--output", str(out), ROUTE])
assert result.returncode == 0, result.stdout + result.stderr
assert out.is_file(), result.stdout + result.stderr
assert out.stat().st_size > 5000, f"suspiciously small render: {out.stat().st_size} bytes"
def test_cabana_loads_a_local_route(local_route):
assert CABANA_BIN.exists(), "cabana not built"
loaded = f"loaded route {ROUTE} with 2 valid segments"
out = cabana_output_until(("--data_dir", str(local_route), "--no-vipc", ROUTE), loaded)
assert "failed to load route" not in out, out
assert "invalid route format" not in out, out
assert loaded in out, out
def test_cabana_replays_local_video(local_video_route):
expected = "camera[0] vipc send #1"
out = cabana_output_until(("--data_dir", str(local_video_route), ROUTE), expected)
assert "failed to get frame" not in out, out
assert expected in out, out
def test_replay_logreader_reports_load_stats(local_route):
assert shutil.which("python3") is not None
header = (TOOLS_DIR / "replay" / "logreader.h").read_text()
for accessor in ("compressed_size", "decompressed_size", "download_seconds",
"decompress_seconds", "parse_seconds"):
assert f"{accessor}() const" in header
def test_can_capnp_field_matches_extractor_codegen():
assert 'busTimeDEPRECATED' in log.CanData.schema.fields
gen = (TOOLS_DIR / "jotpluggler" / "generate_event_extractors.py").read_text()
assert "getBusTimeDEPRECATED()" in gen

112
iqpilot/tools/tests/test_iq_command.py Normal file
View File

@@ -0,0 +1,112 @@
# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
import os
from pathlib import Path
import subprocess
IQ_COMMAND = Path(__file__).parents[1] / "iq.sh"
def make_checkout(tmp_path: Path) -> Path:
checkout = tmp_path / "checkout"
(checkout / "iqpilot").mkdir(parents=True)
(checkout / "launch_iqpilot.sh").touch()
return checkout
def run_pkg(checkout: Path, path: Path) -> subprocess.CompletedProcess[str]:
env = os.environ.copy()
env["PATH"] = f"{path}:{env['PATH']}"
return subprocess.run(
["bash", str(IQ_COMMAND), "--dir", str(checkout), "pkg"],
check=False,
capture_output=True,
env=env,
text=True,
)
def run_iq(checkout: Path, command: str, *args: str) -> subprocess.CompletedProcess[str]:
return subprocess.run(
["bash", str(IQ_COMMAND), "--dir", str(checkout), command, *args],
check=False,
capture_output=True,
text=True,
)
def test_public_checkout_skips_private_package_source_setup(tmp_path: Path):
checkout = make_checkout(tmp_path)
result = run_pkg(checkout, tmp_path)
assert result.returncode == 0
assert "private package sources are not present" in result.stdout
assert "setup_private_packages.py" not in result.stderr
def test_internal_checkout_runs_private_package_source_setup(tmp_path: Path):
checkout = make_checkout(tmp_path)
setup_script = checkout / "iqpilot/tools/scripts/setup_private_packages.py"
setup_script.parent.mkdir(parents=True)
setup_script.touch()
python_log = tmp_path / "python.log"
python = tmp_path / "python3"
python.write_text(f"#!/usr/bin/env bash\nprintf '%s\\n' \"$*\" > {python_log!s}\n")
python.chmod(0o755)
result = run_pkg(checkout, tmp_path)
assert result.returncode == 0
assert python_log.read_text().strip() == "iqpilot/tools/scripts/setup_private_packages.py"
def test_public_checkout_runs_bundled_package_installer(tmp_path: Path):
checkout = make_checkout(tmp_path)
installer_log = tmp_path / "installer.log"
installer = checkout / "artifacts/runtime/ensure_private_installed.sh"
installer.parent.mkdir(parents=True)
installer.write_text(f"printf installed > {installer_log!s}\n")
result = run_pkg(checkout, tmp_path)
assert result.returncode == 0
assert installer_log.read_text() == "installed"
def test_cabana_command_runs_launcher_from_checkout(tmp_path: Path):
checkout = make_checkout(tmp_path)
log = tmp_path / "cabana.log"
launcher = checkout / "iqpilot/tools/cabana/cabana"
launcher.parent.mkdir(parents=True)
launcher.write_text(f"#!/usr/bin/env bash\nprintf '%s\\n' \"$PWD|$*\" > {log!s}\n")
launcher.chmod(0o755)
result = run_iq(checkout, "cabana", "--msgq")
assert result.returncode == 0
assert log.read_text().strip() == f"{checkout}|--msgq"
def test_juggle_command_runs_launcher_from_checkout(tmp_path: Path):
checkout = make_checkout(tmp_path)
log = tmp_path / "juggle.log"
launcher = checkout / "iqpilot/tools/jotpluggler/pluggle.py"
launcher.parent.mkdir(parents=True)
launcher.write_text(f"#!/usr/bin/env bash\nprintf '%s\\n' \"$PWD|$*\" > {log!s}\n")
launcher.chmod(0o755)
result = run_iq(checkout, "juggle", "route")
assert result.returncode == 0
assert log.read_text().strip() == f"{checkout}|route"
def test_desktop_tool_missing_from_host_checkout_fails(tmp_path: Path):
checkout = make_checkout(tmp_path)
result = run_iq(checkout, "cabana")
assert result.returncode == 1
assert "launcher is missing" in result.stderr

10
iqpilot/tools/ubuntu_setup.sh Executable file
View File

@@ -0,0 +1,10 @@
#!/usr/bin/env bash
set -e
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
# NOTE: this is used in a docker build, so do not run any scripts here.
"$DIR"/install_ubuntu_dependencies.sh
"$DIR"/install_python_dependencies.sh