| |
| """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: |
| 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" |
| 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() |
|
|
