IQ.Pilot Release Commit @ 4fcea4d

tools/diagnostics/lag_probe.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+import argparse
+import statistics
+import time
+
+import cereal.messaging as messaging
+
+
+SERVICES = [
+  "carState",
+  "selfdriveState",
+  "controlsState",
+  "modelV2",
+  "uiDebug",
+  "liveCalibration",
+]
+
+
+def summarize(values: list[float]) -> str:
+  if not values:
+    return "n=0"
+  return f"n={len(values)} avg_ms={statistics.fmean(values):.2f} max_ms={max(values):.2f}"
+
+
+def main() -> None:
+  parser = argparse.ArgumentParser(description="On-device runtime lag probe")
+  parser.add_argument("--seconds", type=float, default=15.0, help="Sampling window")
+  args = parser.parse_args()
+
+  sm = messaging.SubMaster(SERVICES)
+  last_seen: dict[str, float] = {}
+  gaps: dict[str, list[float]] = {service: [] for service in SERVICES}
+  ui_draw_times: list[float] = []
+  car_cum_lag: list[float] = []
+  model_frame_drop: list[float] = []
+
+  deadline = time.monotonic() + args.seconds
+  while time.monotonic() < deadline:
+    sm.update(100)
+    now = time.monotonic()
+    for service in SERVICES:
+      if not sm.updated[service]:
+        continue
+      previous = last_seen.get(service)
+      if previous is not None:
+        gaps[service].append((now - previous) * 1000.0)
+      last_seen[service] = now
+
+    if sm.updated["uiDebug"]:
+      ui_draw_times.append(float(sm["uiDebug"].drawTimeMillis))
+    if sm.updated["carState"]:
+      car_cum_lag.append(float(sm["carState"].cumLagMs))
+    if sm.updated["modelV2"]:
+      model_frame_drop.append(float(sm["modelV2"].frameDropPerc))
+
+  print("Lag probe summary")
+  for service in SERVICES:
+    print(f"{service}: {summarize(gaps[service])}")
+
+  print(f"uiDebug.drawTimeMillis: {summarize(ui_draw_times)}")
+  print(f"carState.cumLagMs: {summarize(car_cum_lag)}")
+  print(f"modelV2.frameDropPerc: {summarize(model_frame_drop)}")
+
+  if sm.seen["liveCalibration"]:
+    live_calib = sm["liveCalibration"]
+    print(
+      "liveCalibration:"
+      f" status={int(live_calib.calStatus)}"
+      f" calPerc={int(live_calib.calPerc)}"
+      f" rpy={list(live_calib.rpyCalib)}"
+      f" spread={list(live_calib.rpyCalibSpread)}"
+    )
+
+
+if __name__ == "__main__":
+  main()

tools/diagnostics/nav_lag_matrix.py

@@ -0,0 +1,285 @@
+#!/usr/bin/env python3
+import argparse
+import json
+import os
+import re
+import shlex
+import subprocess
+import tempfile
+import time
+from collections import defaultdict
+from pathlib import Path
+
+
+REPO_ROOT = Path(__file__).resolve().parents[2]
+VIDEO_AUDIT = REPO_ROOT / "tools" / "diagnostics" / "video_lag_audit.py"
+
+NAV_ALL_FALSE = {
+  "allow_mapd": False,
+  "allow_offline_fallback": False,
+  "allow_offline_routing": False,
+  "allow_route_updates": False,
+  "allow_live_data": False,
+  "allow_nav_state": False,
+  "allow_render": False,
+  "allow_nav_influence": False,
+  "allow_on_screen_navigation": False,
+  "allow_lane_position": False,
+}
+
+SCENARIOS: dict[str, dict | None] = {
+  "default": None,
+  "all_false": NAV_ALL_FALSE,
+  "no_nav_state": {"allow_nav_state": False},
+  "no_render": {"allow_render": False},
+  "no_live_data": {"allow_live_data": False},
+  "no_route_updates": {"allow_route_updates": False},
+  "no_mapd_offline_fallback": {"allow_mapd": False, "allow_offline_fallback": False},
+  "no_offline_routing": {"allow_offline_routing": False},
+  "no_influence_lane": {"allow_nav_influence": False, "allow_lane_position": False},
+  "no_onscreen": {"allow_on_screen_navigation": False},
+}
+
+LAG_PATTERNS = {
+  "navd": re.compile(r"navd step slow total_ms=(?P<total>[0-9.]+)"),
+  "card": re.compile(r"card step slow total_ms=(?P<total>[0-9.]+)"),
+  "controlsd": re.compile(r"controlsd step slow total_ms=(?P<total>[0-9.]+)"),
+  "selfdrived": re.compile(r"selfdrived step slow total_ms=(?P<total>[0-9.]+)"),
+  "selfdrived_sample": re.compile(r"selfdrived sample slow total_ms=(?P<total>[0-9.]+)"),
+}
+
+
+def run(cmd: list[str], *, check: bool = True, capture: bool = True, cwd: Path | None = None) -> str:
+  result = subprocess.run(
+    cmd,
+    cwd=cwd,
+    check=check,
+    capture_output=capture,
+    text=True,
+  )
+  return result.stdout if capture else ""
+
+
+def ssh(host: str, command: str, *, check: bool = True) -> str:
+  return run(["ssh", "-o", "BatchMode=yes", "-o", "ConnectTimeout=8", host, command], check=check)
+
+
+def scp_from(host: str, remote_path: str, local_path: Path) -> None:
+  local_path.parent.mkdir(parents=True, exist_ok=True)
+  subprocess.run(
+    ["scp", "-q", f"{host}:{remote_path}", str(local_path)],
+    check=True,
+    capture_output=True,
+    text=True,
+  )
+
+
+def remote_write_json(host: str, remote_path: str, payload: dict) -> None:
+  encoded = json.dumps(payload, sort_keys=True)
+  ssh(host, f"cat > {shlex.quote(remote_path)} <<'EOF'\n{encoded}\nEOF")
+
+
+def remote_remove(host: str, remote_path: str) -> None:
+  ssh(host, f"rm -f {shlex.quote(remote_path)}")
+
+
+def set_nav_flags(host: str, flags: dict | None) -> None:
+  remote_path = "/data/params/d/NavigationDebugFlags"
+  if flags is None:
+    remote_remove(host, remote_path)
+  else:
+    remote_write_json(host, remote_path, flags)
+
+
+def set_screen_recording(host: str, enabled: bool) -> None:
+  value = "1" if enabled else "0"
+  ssh(host, f"printf '{value}' > /data/params/d/ScreenRecording")
+
+
+def clear_issue_debug(host: str) -> None:
+  ssh(host, "mkdir -p /data/community && : > /data/community/iqpilot_issue_debug.txt")
+
+
+def list_screen_recordings(host: str) -> list[str]:
+  output = ssh(host, "ls -1t /data/media/0/screen_recordings/*.mp4 2>/dev/null || true")
+  return [line.strip() for line in output.splitlines() if line.strip()]
+
+
+def newest_recording_after(host: str, before: set[str]) -> str | None:
+  after = list_screen_recordings(host)
+  for candidate in after:
+    if candidate not in before:
+      return candidate
+  return after[0] if after else None
+
+
+def fetch_issue_debug(host: str, output_dir: Path) -> Path:
+  local_path = output_dir / "iqpilot_issue_debug.txt"
+  scp_from(host, "/data/community/iqpilot_issue_debug.txt", local_path)
+  return local_path
+
+
+def parse_issue_debug(path: Path) -> dict:
+  counts = defaultdict(int)
+  maxima = defaultdict(float)
+  calibration_lines = 0
+
+  if not path.exists():
+    return {"counts": {}, "max_total_ms": {}, "calibration_lines": 0}
+
+  for line in path.read_text(errors="replace").splitlines():
+    if "calibrationd" in line:
+      calibration_lines += 1
+    for key, pattern in LAG_PATTERNS.items():
+      match = pattern.search(line)
+      if match:
+        counts[key] += 1
+        maxima[key] = max(maxima[key], float(match.group("total")))
+
+  return {
+    "counts": dict(counts),
+    "max_total_ms": dict(maxima),
+    "calibration_lines": calibration_lines,
+  }
+
+
+def run_remote_demo(host: str, scenario_dir: str, fixture: str, provider: str) -> str:
+  cmd = (
+    f"cd /data/openpilot && "
+    f"scripts/iqpilot/run_device_nav_demo.sh --fixture {shlex.quote(fixture)} "
+    f"--provider {shlex.quote(provider)} --output-dir {shlex.quote(scenario_dir)} --no-gif"
+  )
+  return ssh(host, cmd)
+
+
+def run_remote_lag_probe(host: str, seconds: float, output_path: str) -> None:
+  cmd = (
+    "cd /data/openpilot && "
+    f"PYTHONPATH=. python3 tools/diagnostics/lag_probe.py --seconds {seconds:.1f} > {shlex.quote(output_path)} 2>&1"
+  )
+  ssh(host, cmd)
+
+
+def fetch_latest_demo_video(host: str, scenario_dir: str, output_dir: Path) -> Path | None:
+  remote_video = f"{scenario_dir}/nav_demo.mp4"
+  try:
+    local_path = output_dir / "nav_demo.mp4"
+    scp_from(host, remote_video, local_path)
+    return local_path
+  except subprocess.CalledProcessError:
+    return None
+
+
+def fetch_remote_file(host: str, remote_path: str, output_dir: Path, local_name: str) -> Path | None:
+  local_path = output_dir / local_name
+  try:
+    scp_from(host, remote_path, local_path)
+    return local_path
+  except subprocess.CalledProcessError:
+    return None
+
+
+def summarize_video(video_path: Path, output_dir: Path) -> dict | None:
+  if not video_path or not video_path.exists():
+    return None
+  payload = run([
+    "python3",
+    str(VIDEO_AUDIT),
+    str(video_path),
+    "--output-dir",
+    str(output_dir / "video_audit"),
+  ])
+  return json.loads(payload)
+
+
+def run_live_capture(host: str, seconds: float) -> tuple[str | None, str | None]:
+  before = set(list_screen_recordings(host))
+  set_screen_recording(host, True)
+  try:
+    time.sleep(seconds)
+  finally:
+    set_screen_recording(host, False)
+  time.sleep(3.0)
+  remote_video = newest_recording_after(host, before)
+  probe_remote = "/data/community/nav_lag_probe.txt"
+  try:
+    run_remote_lag_probe(host, min(seconds, 20.0), probe_remote)
+  except subprocess.CalledProcessError:
+    probe_remote = None
+  return remote_video, probe_remote
+
+
+def scenario_flags(name: str) -> dict | None:
+  if name not in SCENARIOS:
+    raise KeyError(f"unknown scenario: {name}")
+  return SCENARIOS[name]
+
+
+def main() -> None:
+  parser = argparse.ArgumentParser(description="Run nav lag feature matrix on a comma device and collect videos/logs.")
+  parser.add_argument("--host", default="arman3x", help="SSH host alias")
+  parser.add_argument("--mode", choices=["live", "demo"], default="live", help="Capture live screen recording or deterministic UI nav demo")
+  parser.add_argument("--duration", type=float, default=20.0, help="Live capture duration in seconds")
+  parser.add_argument("--fixture", default="bolingbrook-carol-stream", help="Fixture alias/path for demo mode")
+  parser.add_argument("--provider", default="offline", choices=["offline", "cached", "mapbox"], help="Provider for demo mode")
+  parser.add_argument("--scenarios", nargs="+", default=["default", "all_false"], help="Scenario names to run")
+  parser.add_argument("--output-dir", type=Path, default=Path("nav_lag_matrix_runs"), help="Local artifact directory")
+  args = parser.parse_args()
+
+  run_root = args.output_dir / time.strftime("%Y%m%d_%H%M%S")
+  run_root.mkdir(parents=True, exist_ok=True)
+  summary = {
+    "host": args.host,
+    "mode": args.mode,
+    "fixture": args.fixture,
+    "provider": args.provider,
+    "scenarios": [],
+  }
+
+  for scenario_name in args.scenarios:
+    flags = scenario_flags(scenario_name)
+    scenario_dir = run_root / scenario_name
+    scenario_dir.mkdir(parents=True, exist_ok=True)
+    clear_issue_debug(args.host)
+    set_nav_flags(args.host, flags)
+    time.sleep(2.0)
+
+    remote_probe = None
+    local_video = None
+    if args.mode == "demo":
+      remote_dir = f"/data/nav_demo_tests/{scenario_name}_{int(time.time())}"
+      demo_stdout = run_remote_demo(args.host, remote_dir, args.fixture, args.provider)
+      (scenario_dir / "demo_stdout.txt").write_text(demo_stdout)
+      local_video = fetch_latest_demo_video(args.host, remote_dir, scenario_dir)
+    else:
+      remote_video, remote_probe = run_live_capture(args.host, args.duration)
+      if remote_video:
+        local_video = fetch_remote_file(args.host, remote_video, scenario_dir, Path(remote_video).name)
+
+    iqdebug_path = fetch_issue_debug(args.host, scenario_dir)
+    probe_path = None
+    if remote_probe:
+      probe_path = fetch_remote_file(args.host, remote_probe, scenario_dir, "lag_probe.txt")
+
+    video_summary = summarize_video(local_video, scenario_dir) if local_video else None
+    debug_summary = parse_issue_debug(iqdebug_path)
+
+    scenario_summary = {
+      "scenario": scenario_name,
+      "flags": flags,
+      "video": str(local_video) if local_video else None,
+      "probe": str(probe_path) if probe_path else None,
+      "issue_debug": str(iqdebug_path),
+      "debug_summary": debug_summary,
+      "video_summary": video_summary,
+    }
+    summary["scenarios"].append(scenario_summary)
+    (scenario_dir / "summary.json").write_text(json.dumps(scenario_summary, indent=2, sort_keys=True) + "\n")
+
+  summary_path = run_root / "summary.json"
+  summary_path.write_text(json.dumps(summary, indent=2, sort_keys=True) + "\n")
+  print(json.dumps(summary, indent=2, sort_keys=True))
+
+
+if __name__ == "__main__":
+  main()

tools/diagnostics/video_lag_audit.py

@@ -0,0 +1,243 @@
+#!/usr/bin/env python3
+import argparse
+import csv
+import hashlib
+import json
+import math
+import shutil
+import subprocess
+import tempfile
+from pathlib import Path
+
+try:
+  from PIL import Image, ImageChops, ImageStat
+except ModuleNotFoundError:
+  Image = None
+  ImageChops = None
+  ImageStat = None
+
+
+DEFAULT_PLANNER_ROI = (0.22, 0.54, 0.78, 0.97)
+
+
+def run(cmd: list[str]) -> str:
+  result = subprocess.run(cmd, check=True, capture_output=True, text=True)
+  return result.stdout
+
+
+def ffprobe_video(path: Path) -> dict:
+  payload = run([
+    "ffprobe",
+    "-v", "error",
+    "-print_format", "json",
+    "-show_streams",
+    "-show_format",
+    str(path),
+  ])
+  return json.loads(payload)
+
+
+def parse_fraction(value: str) -> float:
+  if "/" in value:
+    num, den = value.split("/", 1)
+    return float(num) / float(den)
+  return float(value)
+
+
+def extract_frames(video_path: Path, output_dir: Path) -> list[Path]:
+  output_dir.mkdir(parents=True, exist_ok=True)
+  run([
+    "ffmpeg",
+    "-loglevel", "error",
+    "-i", str(video_path),
+    "-vsync", "0",
+    str(output_dir / "frame_%06d.png"),
+    "-y",
+  ])
+  return sorted(output_dir.glob("frame_*.png"))
+
+
+def file_hash(path: Path) -> str:
+  digest = hashlib.sha256()
+  with path.open("rb") as f:
+    for chunk in iter(lambda: f.read(1024 * 1024), b""):
+      digest.update(chunk)
+  return digest.hexdigest()
+
+
+def crop_box(width: int, height: int, roi: tuple[float, float, float, float]) -> tuple[int, int, int, int]:
+  left = int(width * roi[0])
+  top = int(height * roi[1])
+  right = int(width * roi[2])
+  bottom = int(height * roi[3])
+  return left, top, right, bottom
+
+
+def rms_diff(prev_img, cur_img) -> float:
+  diff = ImageChops.difference(prev_img, cur_img)
+  return float(ImageStat.Stat(diff).rms[0])
+
+
+def summarize(values: list[float]) -> dict[str, float]:
+  if not values:
+    return {"count": 0, "avg": 0.0, "max": 0.0, "min": 0.0}
+  return {
+    "count": len(values),
+    "avg": sum(values) / len(values),
+    "max": max(values),
+    "min": min(values),
+  }
+
+
+def write_contact_sheet(flagged: list[Path], output_path: Path, *, columns: int = 3) -> None:
+  if Image is None or not flagged:
+    return
+
+  images = []
+  for path in flagged:
+    with Image.open(path) as img:
+      images.append(img.convert("RGB").copy())
+
+  thumb_w = min(img.width for img in images)
+  thumb_h = min(img.height for img in images)
+  rows = math.ceil(len(images) / columns)
+  sheet = Image.new("RGB", (thumb_w * columns, thumb_h * rows), color=(0, 0, 0))
+
+  for idx, img in enumerate(images):
+    thumb = img.resize((thumb_w, thumb_h))
+    x = (idx % columns) * thumb_w
+    y = (idx // columns) * thumb_h
+    sheet.paste(thumb, (x, y))
+
+  sheet.save(output_path)
+
+
+def audit_video(video_path: Path, output_dir: Path, roi: tuple[float, float, float, float]) -> dict:
+  output_dir.mkdir(parents=True, exist_ok=True)
+  temp_root = Path(tempfile.mkdtemp(prefix="iqpilot_video_audit_"))
+  try:
+    frames = extract_frames(video_path, temp_root / "frames")
+    probe = ffprobe_video(video_path)
+    streams = probe.get("streams", [])
+    video_stream = next((stream for stream in streams if stream.get("codec_type") == "video"), {})
+    fps = parse_fraction(video_stream.get("avg_frame_rate", "0")) if video_stream.get("avg_frame_rate") else 0.0
+    duration = float(probe.get("format", {}).get("duration", 0.0) or 0.0)
+
+    records: list[dict] = []
+    duplicate_runs: list[int] = []
+    current_duplicate_run = 0
+    exact_duplicate_indices: list[int] = []
+    low_motion_indices: list[int] = []
+    suspicious_paths: list[Path] = []
+    full_rms_values: list[float] = []
+    planner_rms_values: list[float] = []
+
+    prev_hash = None
+    prev_img = None
+    prev_planner = None
+
+    for idx, frame_path in enumerate(frames):
+      frame_hash = file_hash(frame_path)
+      exact_duplicate = prev_hash == frame_hash
+
+      full_rms = 0.0
+      planner_rms = 0.0
+      if Image is not None:
+        with Image.open(frame_path) as img:
+          current = img.convert("L")
+          planner = current.crop(crop_box(current.width, current.height, roi))
+          if prev_img is not None:
+            full_rms = rms_diff(prev_img, current)
+            planner_rms = rms_diff(prev_planner, planner)
+            full_rms_values.append(full_rms)
+            planner_rms_values.append(planner_rms)
+          prev_img = current.copy()
+          prev_planner = planner.copy()
+
+      if exact_duplicate:
+        current_duplicate_run += 1
+        exact_duplicate_indices.append(idx)
+      elif current_duplicate_run:
+        duplicate_runs.append(current_duplicate_run)
+        current_duplicate_run = 0
+
+      if idx > 0 and (exact_duplicate or planner_rms < 1.2 or full_rms < 1.0):
+        low_motion_indices.append(idx)
+        suspicious_paths.append(frame_path)
+
+      records.append({
+        "frame": idx,
+        "exact_duplicate": exact_duplicate,
+        "full_rms": round(full_rms, 4),
+        "planner_rms": round(planner_rms, 4),
+      })
+      prev_hash = frame_hash
+
+    if current_duplicate_run:
+      duplicate_runs.append(current_duplicate_run)
+
+    csv_path = output_dir / f"{video_path.stem}_frame_metrics.csv"
+    with csv_path.open("w", newline="") as f:
+      writer = csv.DictWriter(f, fieldnames=["frame", "exact_duplicate", "full_rms", "planner_rms"])
+      writer.writeheader()
+      writer.writerows(records)
+
+    flagged_dir = output_dir / f"{video_path.stem}_flagged_frames"
+    flagged_dir.mkdir(parents=True, exist_ok=True)
+    for path in suspicious_paths[:24]:
+      shutil.copy2(path, flagged_dir / path.name)
+
+    write_contact_sheet(sorted(flagged_dir.glob("*.png"))[:12], output_dir / f"{video_path.stem}_contact_sheet.png")
+
+    summary = {
+      "video": str(video_path),
+      "frame_count": len(frames),
+      "fps": fps,
+      "duration_s": duration,
+      "exact_duplicate_frames": len(exact_duplicate_indices),
+      "max_duplicate_run": max(duplicate_runs) if duplicate_runs else 0,
+      "duplicate_runs": duplicate_runs,
+      "low_motion_frames": len(low_motion_indices),
+      "full_rms": summarize(full_rms_values),
+      "planner_rms": summarize(planner_rms_values),
+      "planner_roi": {
+        "left": roi[0],
+        "top": roi[1],
+        "right": roi[2],
+        "bottom": roi[3],
+      },
+      "artifacts": {
+        "metrics_csv": str(csv_path),
+        "flagged_frames_dir": str(flagged_dir),
+        "contact_sheet": str(output_dir / f"{video_path.stem}_contact_sheet.png"),
+      },
+    }
+
+    summary_path = output_dir / f"{video_path.stem}_summary.json"
+    summary_path.write_text(json.dumps(summary, indent=2, sort_keys=True) + "\n")
+    return summary
+  finally:
+    shutil.rmtree(temp_root, ignore_errors=True)
+
+
+def main() -> None:
+  parser = argparse.ArgumentParser(description="Audit an MP4 for duplicate/low-motion frame runs.")
+  parser.add_argument("video", type=Path, help="Input MP4")
+  parser.add_argument("--output-dir", type=Path, default=Path("video_audit"), help="Artifact output directory")
+  parser.add_argument(
+    "--planner-roi",
+    default="0.22,0.54,0.78,0.97",
+    help="ROI fractions left,top,right,bottom for planner-focused RMS stats",
+  )
+  args = parser.parse_args()
+
+  roi = tuple(float(part.strip()) for part in args.planner_roi.split(","))
+  if len(roi) != 4:
+    raise ValueError("planner-roi must have four comma-separated floats")
+
+  summary = audit_video(args.video, args.output_dir, roi)
+  print(json.dumps(summary, indent=2, sort_keys=True))
+
+
+if __name__ == "__main__":
+  main()