DCCONSTRUCTIONS
e515ab1b8c
Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
229 lines
9.2 KiB
Python
229 lines
9.2 KiB
Python
"""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()
|