feat(planning): consolidate recorded-route localization and spatial scene
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.
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,175 @@
|
||||
"""Qualify preparation on an existing complete reference, without a live service."""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import time
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
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_source import export_planning_source
|
||||
from k1link.missions.causal_replay import digest
|
||||
from k1link.missions.reference_window import ReferenceWindowIndex, reference_window
|
||||
from k1link.missions.sources import PlanningSources
|
||||
from k1link.sessions.models import ReplayArtifact, ReplayCommand
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--previous-manifest", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
previous = json.loads(args.previous_manifest.read_text())
|
||||
inputs = {Path(p): sha for p, sha in previous["input_digests"].items()}
|
||||
assert all(digest(p) == sha for p, sha in inputs.items())
|
||||
run_report = next(p for p in inputs if p.name == "report.json")
|
||||
physical = json.loads(run_report.read_text())
|
||||
session = physical["reference"]["session_id"]
|
||||
raw = next(p for p in inputs if session in str(p) and p.name == "mqtt.raw.k1mqtt")
|
||||
index = raw.with_name("mqtt.metadata.jsonl")
|
||||
root = args.output
|
||||
root.mkdir(parents=True, exist_ok=False)
|
||||
code_paths = [
|
||||
Path(__file__),
|
||||
*Path("src/k1link/missions").glob("*.py"),
|
||||
Path("src/k1link/device_plugins/xgrids_k1/localization_source.py"),
|
||||
Path("src/k1link/device_plugins/xgrids_k1/planning_source.py"),
|
||||
]
|
||||
code = {str(p): digest(p) for p in code_paths}
|
||||
artifacts = tuple(
|
||||
ReplayArtifact(
|
||||
artifact_id=name,
|
||||
path=p.resolve(),
|
||||
media_type=media,
|
||||
file_byte_length=p.stat().st_size,
|
||||
replay_byte_length=p.stat().st_size,
|
||||
expected_sha256=inputs[p],
|
||||
)
|
||||
for name, p, media in [
|
||||
("raw-transport-primary", raw, "application/x-k1mqtt"),
|
||||
("raw-transport-index", index, "application/x-ndjson"),
|
||||
]
|
||||
)
|
||||
command = ReplayCommand(
|
||||
session_id=session,
|
||||
plugin_id="archive-qualification",
|
||||
allowed_root=raw.parent.resolve(),
|
||||
session_root=raw.parent.resolve(),
|
||||
primary_artifact_id="raw-transport-primary",
|
||||
artifacts=artifacts,
|
||||
timeline_origin_epoch_ns=0,
|
||||
timeline_origin_monotonic_ns=0,
|
||||
speed=1.0,
|
||||
loop=False,
|
||||
)
|
||||
detail = SimpleNamespace(
|
||||
plugin_id=command.plugin_id,
|
||||
summary=SimpleNamespace(replayable=True, lab=None),
|
||||
as_dict=lambda: {"display_name": "Private archive preparation"},
|
||||
)
|
||||
store = SimpleNamespace(
|
||||
data_dir=root, prepare_replay=lambda _: command, get_session=lambda _: detail
|
||||
)
|
||||
sources = PlanningSources(
|
||||
store, {command.plugin_id: export_planning_source}, {command.plugin_id: extract_submap}
|
||||
)
|
||||
t0 = time.monotonic()
|
||||
doc = sources.get(session)
|
||||
export_s = time.monotonic() - t0
|
||||
draft = physical["draft"]["route"]
|
||||
measurements = []
|
||||
for label, first, last in [
|
||||
("physical-selected", draft["start_index"], draft["end_index"]),
|
||||
("complete-recorded-reference", 0, len(doc["poses"]) - 1),
|
||||
]:
|
||||
t0 = time.monotonic()
|
||||
points, provenance = sources.reference_map(session, doc["generation"], first, last)
|
||||
seconds = time.monotonic() - t0
|
||||
if label == "physical-selected":
|
||||
original = run_report.parent / "reference.npy"
|
||||
assert digest(original) == physical["artifacts"]["reference.npy"]
|
||||
assert np.array_equal(points, np.load(original, allow_pickle=False))
|
||||
t1 = time.monotonic()
|
||||
spatial_index = ReferenceWindowIndex(points)
|
||||
indexed_s = time.monotonic() - t1
|
||||
windows = []
|
||||
for step in sorted(run_report.parent.glob("step-*")):
|
||||
source_file = step / "source.json"
|
||||
input_file = step / "registration-input.npz"
|
||||
if not input_file.is_file():
|
||||
continue
|
||||
assert (
|
||||
digest(source_file)
|
||||
== physical["artifacts"][str(source_file.relative_to(run_report.parent))]
|
||||
)
|
||||
assert (
|
||||
digest(input_file)
|
||||
== physical["artifacts"][str(input_file.relative_to(run_report.parent))]
|
||||
)
|
||||
saved = json.loads(source_file.read_text())
|
||||
with np.load(input_file, allow_pickle=False) as sample_input:
|
||||
sample = dict(points=sample_input["query"], path=np.asarray(saved["query_path"]))
|
||||
t2 = time.monotonic()
|
||||
full, old = reference_window(points, sample, sample_input["initial"])
|
||||
t3 = time.monotonic()
|
||||
local, new = reference_window(
|
||||
points, sample, sample_input["initial"], index=spatial_index
|
||||
)
|
||||
t4 = time.monotonic()
|
||||
assert np.array_equal(full, local)
|
||||
windows.append(
|
||||
dict(
|
||||
step=step.name,
|
||||
full_s=t3 - t2,
|
||||
indexed_s=t4 - t3,
|
||||
examined=new["examined_points"],
|
||||
target=len(local),
|
||||
)
|
||||
)
|
||||
measurement = dict(
|
||||
label=label,
|
||||
seconds=seconds,
|
||||
points=len(points),
|
||||
bytes=points.nbytes,
|
||||
route_m=doc["poses"][last]["distance_m"] - doc["poses"][first]["distance_m"],
|
||||
tiles=len(provenance["tiles"]),
|
||||
index_s=indexed_s,
|
||||
index_array_bytes=spatial_index.order.nbytes,
|
||||
spatial_cells=len(spatial_index.slices),
|
||||
provenance=provenance,
|
||||
exact_windows=len(windows),
|
||||
median_window_full_s=float(np.median([w["full_s"] for w in windows])),
|
||||
median_window_indexed_s=float(np.median([w["indexed_s"] for w in windows])),
|
||||
median_window_examined=float(np.median([w["examined"] for w in windows])),
|
||||
window_measurements=windows,
|
||||
)
|
||||
measurements.append(measurement)
|
||||
print(json.dumps({k: v for k, v in measurement.items() if k != "provenance"}), flush=True)
|
||||
del points, spatial_index
|
||||
checks = dict(
|
||||
inputs_unchanged=all(digest(p) == sha for p, sha in inputs.items()),
|
||||
code_unchanged=all(digest(Path(p)) == sha for p, sha in code.items()),
|
||||
staging_removed=not list(sources.root.glob(".source.*")),
|
||||
selected_reference_exact=True,
|
||||
)
|
||||
result = dict(
|
||||
created_at_utc=utc_now_iso(),
|
||||
inputs={str(p): sha for p, sha in inputs.items()},
|
||||
implementation_sha256=code,
|
||||
export_s=export_s,
|
||||
pose_count=len(doc["poses"]),
|
||||
trajectory_cache_bytes=(sources.root / (doc["generation"] + ".json")).stat().st_size,
|
||||
checks=checks,
|
||||
measurements=measurements,
|
||||
limitation="Preparation of existing reference only, not independent long traversal.",
|
||||
)
|
||||
(root / "report.json").write_text(json.dumps(result, indent=2, allow_nan=False))
|
||||
print(json.dumps(checks), flush=True)
|
||||
assert all(checks.values())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user