"""Fixed recovery/stationary functional probes; no device or application access.""" import argparse import json import platform from pathlib import Path import numpy as np from k1link.artifacts import utc_now_iso from k1link.device_plugins.xgrids_k1.localization_source import extract_submap from k1link.device_plugins.xgrids_k1.planning_replay import iter_planning_events from k1link.missions.causal_replay import digest, replay from k1link.missions.entry_acquisition_worker import run_entry_acquisition from k1link.missions.replay_faults import drop_receipts from k1link.missions.stationary_entry import STATIONARY_POLICY, stationary_prefix def write(path, data): path.write_text(json.dumps(data, allow_nan=False, indent=2)) def code_hashes(): root = Path(__file__).resolve().parents[1] paths = [ Path(__file__).resolve(), *sorted((root / "src/k1link/missions").glob("*.py")), root / "src/k1link/device_plugins/xgrids_k1/planning_replay.py", root / "src/k1link/device_plugins/xgrids_k1/planning_live.py", root / "src/k1link/device_plugins/xgrids_k1/localization_source.py", ] return {str(p.relative_to(root)): digest(p) for p in paths} def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--stage", choices=["prepare", "baseline", "drop", "stationary"], required=True ) parser.add_argument("--output", type=Path, required=True) parser.add_argument("--reference-raw", type=Path) parser.add_argument("--reference-planning", type=Path) parser.add_argument("--query-raw", type=Path) parser.add_argument("--query-planning", type=Path) parser.add_argument("--negative-controls", type=Path) args = parser.parse_args() root = args.output if args.stage == "prepare": if any( x is None for x in ( args.reference_raw, args.reference_planning, args.query_raw, args.query_planning, args.negative_controls, ) ): parser.error("Preparation requires all five source arguments.") a = json.loads(args.reference_planning.read_text()) b = json.loads(args.query_planning.read_text()) if a["session_id"] == b["session_id"]: raise ValueError("Independent query required.") files = { args.reference_planning: digest(args.reference_planning), args.query_planning: digest(args.query_planning), } for raw, p in ((args.reference_raw, a), (args.query_raw, b)): files[raw] = p["source_digests"]["raw-transport-primary"] files[raw.with_name("mqtt.metadata.jsonl")] = p["source_digests"]["raw-transport-index"] negative = json.loads((args.negative_controls / "report.json").read_text()) negative_file = args.negative_controls / "wrong-region/registration-input.npz" files[negative_file] = negative["artifacts"]["wrong-region/registration-input.npz"] if any(digest(p) != h for p, h in files.items()): raise ValueError("Input digest mismatch.") root.mkdir(parents=True, exist_ok=False) end = max(i for i, p in enumerate(a["poses"]) if p["distance_m"] <= 40) reference, extraction = extract_submap(args.reference_raw, a, 0, end) path = np.array([p["position"] for p in a["poses"][: end + 1]]) wrong_start = next(i for i, p in enumerate(a["poses"]) if p["distance_m"] >= 130) wrong_entry = np.array(a["poses"][wrong_start]["position"]) wrong_forward = next( np.array(p["position"]) - wrong_entry for p in a["poses"][wrong_start + 1 :] if np.linalg.norm((np.array(p["position"]) - wrong_entry)[:2]) >= 3 ) np.savez_compressed(root / "reference.npz", reference=reference, reference_path=path) write( root / "manifest.json", dict( schema_version="missioncore.recovery-probe/v1", created_at_utc=utc_now_iso(), reference_session=a["session_id"], query_session=b["session_id"], reference_length_m=a["poses"][end]["distance_m"], extraction=extraction, query_raw=str(args.query_raw), negative_file=str(negative_file), negative_entry=wrong_entry.tolist(), negative_forward=wrong_forward.tolist(), input_digests={str(p): h for p, h in files.items()}, reference_sha256=digest(root / "reference.npz"), implementation_sha256=code_hashes(), vehicle_control=False, localization_confirmed=False, ), ) print( json.dumps( dict( stage="prepared", reference_length_m=a["poses"][end]["distance_m"], points=len(reference), ) ), flush=True, ) return manifest = json.loads((root / "manifest.json").read_text()) inputs = { **manifest["input_digests"], str(root / "reference.npz"): manifest["reference_sha256"], } if any(digest(Path(p)) != h for p, h in inputs.items()): raise ValueError("Input changed before probe.") code = code_hashes() with np.load(root / "reference.npz", allow_pickle=False) as data: reference, path = data["reference"], data["reference_path"] events = iter_planning_events(Path(manifest["query_raw"]), manifest["query_session"]) destination = root / args.stage if args.stage in {"baseline", "drop"}: fault = {} if args.stage == "drop": baseline = json.loads((root / "baseline/report.json").read_text()) if baseline["first_tracking_s"] is None or baseline["first_tracking_s"] >= 44: raise ValueError( "Baseline did not establish tracking before frozen fault interval." ) events = drop_receipts(events, 44.0, 47.0, fault) report = replay(events, reference, path, destination, mode="acquisition") report["fault_injection"] = fault or None else: destination.mkdir(exist_ok=False) report = dict( schema_version="missioncore.stationary-probe/v1", created_at_utc=utc_now_iso(), policy=STATIONARY_POLICY, results={}, ) sample, initial, forward, prefix = stationary_prefix(events, path) events.close() report["prefix"] = prefix np.savez_compressed( destination / "prefix.npz", points=sample["points"], path=sample["path"], initial=initial, forward=forward, ) with np.load(manifest["negative_file"], allow_pickle=False) as data: wrong_reference = data["reference"] # Only fixed A geometry is reused. No previous B seed or fit is read. wrong_entry = np.array(manifest["negative_entry"]) wrong_forward = np.array(manifest["negative_forward"]) wrong_initial = np.eye(4) wrong_initial[:3, 3] = wrong_entry - sample["path"][0] for name, ref, hint, basis in [ ("correct-entry", reference, initial, forward), ("wrong-region", wrong_reference, wrong_initial, wrong_forward), ]: job = destination / name job.mkdir() result = run_entry_acquisition( job, ref, sample["points"], hint, sample["path"][0], basis, mode="stationary" ) report["results"][name] = { k: v for k, v in result.items() if k != "matched_query_indices" } print( json.dumps( dict( control=name, status=result["status"], reasons=result["reasons"], attempts=len(result["initialization"]["attempts"]), clusters=result["initialization"]["clusters"], seconds=result["initialization"]["elapsed_s"], ) ), flush=True, ) report["artifacts"] = { str(p.relative_to(destination)): digest(p) for p in destination.rglob("*") if p.is_file() } report.update( input_digests=inputs, source_integrity_verified=all(digest(Path(p)) == h for p, h in inputs.items()), implementation_sha256=code, runtime=dict( system=platform.system(), machine=platform.machine(), python=platform.python_version() ), vehicle_control=False, localization_confirmed=False, finished_at_utc=utc_now_iso(), ) write(destination / "report.json", report) print( json.dumps( { k: report.get(k) for k in ( "mode", "state", "first_candidate_s", "first_tracking_s", "source_integrity_verified", "transitions", ) } ), flush=True, ) if __name__ == "__main__": main()