File size: 7,255 Bytes
042382e | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 | #!/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()
|