#!/usr/bin/env python """Plan one decision with SimForge Pilot-4B v1.5. python inference.py --example examples/sf-slowing # Pilot as benchmarked: plan selector + brake-only veto python inference.py --example examples/sf-slowing --planner-only Loads the model through the same code and driver profile (driver_profile.txt) that produced the published results (pilot/driver/qd_driver.py), then plans once from three cameras x four frames (2 Hz, oldest first), 16 ego poses at 10 Hz and the route, all in the rig frame at t0 (x forward, y left, z up). Output: a 5 s plan, 50 poses (x, y, heading) at 10 Hz in the same frame. Two differences from closed-loop serving, both inherent to a single decision: * the veto perceives the current frames (closed loop it uses the previous prefill's detections, 0.2 s old); * plan-select has no previous plan for its consistency term. For closed loop, run the full service: python pilot/serve.py (AlpaSim egodriver gRPC; see README). """ from __future__ import annotations import argparse import json import math import shlex import sys import time from pathlib import Path import numpy as np REPO = Path(__file__).resolve().parent for sub in ("pilot/driver", "pilot/lib", "pilot/qd", "pilot/train", "pilot"): sys.path.insert(0, str(REPO / sub)) def driver_args(model_dir: Path) -> argparse.Namespace: """The benchmarked driver profile, parsed by the driver's own CLI (no flag is re-implemented here).""" import qd_driver profile = (REPO / "driver_profile.txt").read_text().strip() profile = profile.replace("/app/weights", str(model_dir)) argv = ["--model", str(model_dir), "--stats-file", "/tmp/pilot-inference-sessions.jsonl", *shlex.split(profile)] class Captured(Exception): pass orig = argparse.ArgumentParser.parse_args def capture(self, args=None, namespace=None): raise Captured(orig(self, argv, namespace)) argparse.ArgumentParser.parse_args = capture try: qd_driver.main() except Captured as c: return c.args[0] finally: argparse.ArgumentParser.parse_args = orig raise RuntimeError("driver CLI did not parse") def load_pilot(model_dir: Path, planner_only: bool): import qd_driver import qd_scene args = driver_args(model_dir) if planner_only: # weights only: no plan selector, no perception head, no veto args.obstacle_veto = False args.plan_select = False args.prog_select = False qd_scene.DYNAMICS_MODE = args.v2_dynamics qd_scene.DYNAMICS_WINDOW = int(args.v2_dynamics_window) model, load_s = qd_driver.load_model(args) warm_s = qd_driver.warmup(model) if getattr(model, "veto", None) is not None: model.veto.warmup() model.veto.pipeline = "sync" # one decision: perceive this decision's own frames return model, args, load_s, warm_s def camera_cals(cameras: dict) -> dict: import qd_perception cals = {} for name, c in cameras.items(): R = qd_perception.view_rotation(math.degrees(c["yaw_rad"]), math.degrees(c["pitch_rad"])) cals[name] = qd_perception.cal_from_arrays(name, (int(c["width"]), int(c["height"])), tuple(c["principal_point"]), tuple(c["angle_to_pixeldist_poly"]), float(c["max_angle_rad"]), tuple(c["linear_cde"]), R, np.asarray(c["position_flu_m"], dtype=np.float64)) return cals def plan(model, args, example_dir: Path) -> dict: import torch import qd_scene inp = json.loads((example_dir / "input.json").read_text()) t0_us = int(round(float(inp["t0_s"]) * 1e6)) frames = {cam: [(t0_us + int(round(f["t_s"] * 1e6)), (example_dir / f["file"]).read_bytes()) for f in entries] for cam, entries in inp["frames"].items()} hxyz = np.asarray(inp["ego_history_xyz"], dtype=np.float64)[None, None] hrot = np.asarray(inp["ego_history_rot"], dtype=np.float64)[None, None] route_xyz = np.asarray(inp["route_xyz"], dtype=np.float64).reshape(-1, 3) route_xy = route_xyz[np.isfinite(route_xyz).all(axis=1)][:, :2] nav = qd_scene.nav_command_from_route(route_xyz, lookahead_m=args.nav_lookahead_m, from_ego=args.nav_from_ego) if args.nav == "route" else qd_scene.NAV_STRAIGHT seed = int(inp.get("seed", 0)) t = time.perf_counter() with torch.inference_mode(): scene = qd_scene.build_scene(frames, hxyz, hrot, nav_command=nav, lazy_frames=model.lazy_frames) ego = qd_scene.build_ego(hxyz, hrot, nav_command=nav) cond = model.prefill([scene])[0] info = None if getattr(model, "veto", None) is not None: frames_now = {cam: entries[-1][1] for cam, entries in frames.items()} xy, hd, info = model.veto.plan(cond, ego, seed, route_xy, frames_now, camera_cals(inp["cameras"]), "example", prev_job=None, anchor=None, t0_us=t0_us, psel={"prev": None, "gt_rig": None} if model.veto.selector is not None else None) else: xy, hd, _ = model.fast_plan(cond, ego, seed, route_xy=route_xy) torch.cuda.synchronize() ms = (time.perf_counter() - t) * 1e3 xy, hd = np.asarray(xy, dtype=np.float64), np.asarray(hd, dtype=np.float64) out = { "model": getattr(model, "name", None), "nav_command": {qd_scene.NAV_STRAIGHT: "straight", qd_scene.NAV_LEFT: "left", qd_scene.NAV_RIGHT: "right"}[nav], "seed": seed, "plan_ms": round(ms, 1), "trajectory": np.round(np.concatenate([xy, hd[:, None]], axis=1), 4).tolist(), "selection": None if info is None else {k: info.get(k) for k in ("k", "pick", "brake", "n_det", "err")}, } ref = inp.get("reference") if ref: rxy = np.asarray(ref["trajectory_xyz"], dtype=np.float64)[:, :2] d = np.linalg.norm(rxy - xy, axis=1) out["vs_reference"] = {"max_m": round(float(d.max()), 4), "mean_m": round(float(d.mean()), 4), "reference_end_xy": np.round(rxy[-1], 2).tolist()} return out def main() -> None: p = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) p.add_argument("--example", type=Path, default=REPO / "examples" / "sf-slowing", help="directory with input.json and frames/") p.add_argument("--model-dir", type=Path, default=REPO, help="this repository (weights at its root)") p.add_argument("--planner-only", action="store_true", help="the network alone: no plan selector, no brake-only veto") p.add_argument("--json", type=Path, default=None, help="also write the result here") a = p.parse_args() model, args, load_s, warm_s = load_pilot(a.model_dir.resolve(), a.planner_only) res = plan(model, args, a.example.resolve()) res.update(load_s=round(load_s, 1), warmup_s=round(warm_s, 1)) if a.json: a.json.write_text(json.dumps(res, indent=1)) traj = np.asarray(res["trajectory"]) print(json.dumps({k: v for k, v in res.items() if k != "trajectory"}, indent=1)) print("plan (x, y, heading) at 1 s steps:", np.round(traj[9::10], 2).tolist()) if __name__ == "__main__": main()