SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(planning): consolidate recorded-route localization and spatial scene

Browse Source 
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
2026-09-21 08:47:19 +03:00
parent be58d589e2
commit e515ab1b8c
189 changed files with 19074 additions and 758 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/check_planning_live_bootstrap.py
+599
View File
@@ -0,0 +1,599 @@
"""Bounded archive qualification of the actual live planning service, without devices."""
import argparse
import json
import shutil
import threading
import time
from dataclasses import replace
from pathlib import Path
from types import SimpleNamespace
import numpy as np
from check_stationary_bootstrap import code_hashes, write
from planning_archive_camera import ArchiveCamera
from planning_archive_source import ReceiptQueueArchiveSource
from k1link.artifacts import utc_now_iso
from k1link.device_plugins.xgrids_k1.localization_source import extract_scene_submap, extract_submap
from k1link.device_plugins.xgrids_k1.planning_replay import iter_planning_events
from k1link.missions.causal_replay import digest
from k1link.missions.live_limits import live_route_limits
from k1link.missions.live_scene_delta import decode_cursor
from k1link.missions.live_tests import PlanningLiveTests
from k1link.missions.reference_map import build_reference_map
from k1link.missions.stationary_bootstrap import BOOTSTRAP_POLICY
from k1link.missions.stationary_entry import STATIONARY_POLICY
class ArchiveSource:
"""One pending receipt, original intervals, no synthetic device authority."""
def __init__(self, raw, session, maximum_seconds=65, *, after_monotonic_ns=None):
if not 0 < maximum_seconds <= 600:
raise ValueError("This offline probe supports up to ten minutes of recorded receipts.")
self.maximum_seconds = maximum_seconds
self.iterator = iter(iter_planning_events(raw, session))
self.pending = next(self.iterator)
while after_monotonic_ns is not None and self.pending.monotonic_ns < after_monotonic_ns:
self.pending = next(self.iterator)
self.origin = self.pending.monotonic_ns
self.started = None
self.session = session
self.owner = None
self.deliveries = []
self.active = False
def snapshot(self):
return dict(
active=self.active,
session_id=self.session if self.started else None,
session_generation=1 if self.started else 0,
)
def open(self, owner):
if self.owner:
raise RuntimeError("An archive consumer already exists.")
self.owner = owner
def close(self, owner):
assert self.owner == owner
self.owner = None
self.iterator.close()
def activate(self):
self.started = time.monotonic_ns()
self.active = True
def take(self, owner):
assert self.owner == owner
if self.started is None:
time.sleep(0.02)
return None
if self.pending is None:
self.active = False
return None
delay = self.pending.monotonic_ns - self.origin
if delay > self.maximum_seconds * 1e9:
self.active = False
return None
stamp = self.started + delay
now = time.monotonic_ns()
if now < stamp:
time.sleep(min(0.02, (stamp - now) / 1e9))
return None
original = self.pending
self.pending = next(self.iterator, None)
self.deliveries.append(
dict(
sequence=original.sequence,
kind=original.kind,
original_monotonic_ns=original.monotonic_ns,
mapped_monotonic_ns=stamp,
delivered_monotonic_ns=now,
lag_s=(now - stamp) / 1e9,
)
)
return replace(original, monotonic_ns=stamp)
def main():
parser = argparse.ArgumentParser(description=__doc__)
group = parser.add_mutually_exclusive_group(required=True)
group.add_argument("--predecessor", type=Path)
group.add_argument("--physical-diagnosis", type=Path)
group.add_argument("--live-run", type=Path)
parser.add_argument("--reference-capture", type=Path)
parser.add_argument("--query-capture", type=Path)
parser.add_argument("--planning-source", type=Path)
parser.add_argument(
"--route-length-m",
type=float,
help="Select a longer reference for this archive probe only.",
)
parser.add_argument(
"--reference-kind", choices=["correct-entry", "wrong-region"], default="correct-entry"
)
parser.add_argument("--queue-ingress", action="store_true")
parser.add_argument(
"--drop-interval-s",
nargs=2,
type=float,
metavar=("START", "END"),
help="Omit derived receipts only; no source edits or retiming.",
)
parser.add_argument(
"--spatial-stop-monotonic-ns",
type=int,
help="Model admitted STOP at a retained receipt, with capture active until planner ends.",
)
parser.add_argument(
"--profile-planning",
action="store_true",
help="Profile the isolated planning consumer, not the canonical runtime.",
)
parser.add_argument(
"--after-monotonic-ns",
type=int,
help="Start at a retained operator retry boundary; never selects a reference position.",
)
parser.add_argument(
"--maximum-seconds",
type=float,
default=None,
help="Explicit offline replay wall budget; never a product-session limit.",
)
parser.add_argument(
"--fast-scene",
action="store_true",
help="Qualify production delta route in-process at 10 Hz, without a server.",
)
parser.add_argument("--camera-epoch", type=Path)
parser.add_argument("--ffmpeg", type=Path)
parser.add_argument("--output", required=True, type=Path)
args = parser.parse_args()
if args.spatial_stop_monotonic_ns is not None and not args.queue_ingress:
parser.error("--spatial-stop-monotonic-ns requires --queue-ingress")
if args.drop_interval_s is not None and not args.queue_ingress:
parser.error("--drop-interval-s requires --queue-ingress")
if args.route_length_m is not None:
live_route_limits(args.route_length_m)
if not args.live_run:
parser.error("--route-length-m requires --live-run and its frozen planning source.")
maximum_seconds = args.maximum_seconds or (120 if args.live_run else 65)
scene_cadence = 0.5 if args.live_run else 2
if args.fast_scene:
scene_cadence = 0.1
reference_provenance = None
scene_reference = scene_provenance = None
if args.camera_epoch and (not args.ffmpeg or not args.ffmpeg.is_file()):
parser.error("A retained camera requires an explicit local FFmpeg binary.")
if args.live_run:
if not all(
p is not None and p.is_file()
for p in [args.reference_capture, args.query_capture, args.planning_source]
):
parser.error(
"A live-run replay requires both raw captures and the frozen planning source."
)
original = json.loads((args.live_run / "report.json").read_text())
assert all(digest(args.live_run / p) == sha for p, sha in original["artifacts"].items())
assert (
digest(args.reference_capture)
== original["reference"]["source_digests"]["raw-transport-primary"]
)
planning = json.loads(args.planning_source.read_text())
assert planning["generation"] == original["draft"]["zone"]["generation"]
start, end = (
original["draft"]["route"]["start_index"],
original["draft"]["route"]["end_index"],
)
if args.reference_kind == "wrong-region":
start = next(i for i, p in enumerate(planning["poses"]) if p["distance_m"] >= 130)
end = next(i for i, p in enumerate(planning["poses"]) if p["distance_m"] >= 160)
if args.route_length_m is not None:
target = planning["poses"][start]["distance_m"] + args.route_length_m
if target > planning["poses"][-1]["distance_m"]:
parser.error("The reference recording does not cover the requested route.")
end = max(i for i, p in enumerate(planning["poses"]) if p["distance_m"] <= target)
path = np.array([p["position"] for p in planning["poses"][start : end + 1]])
def submap(session, generation, first, last, *, presentation=False):
extractor = extract_scene_submap if presentation else extract_submap
points, provenance = extractor(args.reference_capture, planning, first, last)
return points, {
**provenance,
**{k: planning[k] for k in ["session_id", "generation", "source_digests"]},
}
prepared = SimpleNamespace(bound=lambda *a: planning, submap=submap)
reference, reference_provenance = build_reference_map(
prepared, planning["session_id"], planning["generation"], start, end
)
scene_reference, scene_provenance = build_reference_map(
prepared, planning["session_id"], planning["generation"], start, end, presentation=True
)
files = [
args.live_run / "report.json",
args.reference_capture,
args.query_capture,
args.planning_source,
args.query_capture.with_name("mqtt.metadata.jsonl"),
args.reference_capture.with_name("mqtt.metadata.jsonl"),
]
inputs = {str(p): digest(p) for p in files}
previous = dict(
query_raw=str(args.query_capture),
query_session=original["query_session_id"],
reference_session=planning["session_id"],
)
elif args.physical_diagnosis:
if args.reference_kind != "correct-entry":
parser.error("A physical diagnosis uses its exact frozen reference.")
previous = json.loads((args.physical_diagnosis / "run/report.json").read_text())
sealed = json.loads((args.physical_diagnosis / "manifest.redacted.json").read_text())
inputs = {
str(args.physical_diagnosis / item["path"]): item["sha256"] for item in sealed["files"]
}
reference = np.load(args.physical_diagnosis / "run/reference.npy", allow_pickle=False)
path = np.array([p["position"] for p in previous["draft"]["route"]["points"]])
previous = dict(
query_raw=str(args.physical_diagnosis / "capture/captures/mqtt_live/mqtt.raw.k1mqtt"),
query_session=previous["query_session_id"],
reference_session=previous["reference"]["session_id"],
)
else:
previous = json.loads((args.predecessor / "manifest.json").read_text())
inputs = {
**previous["input_digests"],
str(args.predecessor / "manifest.json"): digest(args.predecessor / "manifest.json"),
}
with np.load(previous["references"][args.reference_kind], allow_pickle=False) as data:
reference, path = data["reference"], data["reference_path"]
assert all(digest(Path(p)) == sha for p, sha in inputs.items())
root = args.output
root.mkdir(parents=True, exist_ok=False)
camera = (
ArchiveCamera(args.camera_epoch, root / "camera", args.ffmpeg)
if args.camera_epoch
else None
)
if camera:
inputs.update(camera.inputs)
implementation = {
**code_hashes(),
"scripts/check_planning_live_bootstrap.py": digest(Path(__file__)),
"scripts/planning_archive_source.py": digest(Path("scripts/planning_archive_source.py")),
"scripts/planning_archive_camera.py": digest(Path("scripts/planning_archive_camera.py")),
"src/k1link/device_plugins/xgrids_k1/camera.py": digest(
Path("src/k1link/device_plugins/xgrids_k1/camera.py")
),
"src/k1link/web/camera_archive.py": digest(Path("src/k1link/web/camera_archive.py")),
"tests/test_planning_live.py": digest(Path("tests/test_planning_live.py")),
}
for name in [
"src/k1link/missions/live_display_buffer.py",
"src/k1link/compute/live_perception.py",
"src/k1link/device_plugins/xgrids_k1/facade.py",
"tests/test_planning_stop_lifecycle.py",
"src/k1link/missions/live_limits.py",
"src/k1link/missions/reference_map.py",
"src/k1link/missions/reference_window.py",
"src/k1link/missions/live_buffer.py",
"src/k1link/missions/live_scene_delta.py",
"src/k1link/web/planning_live_api.py",
"tests/test_planning_fast_display.py",
"apps/control-station/src/core/missions/planningSceneStream.ts",
"apps/control-station/src/components/missions/PlanningLiveScene.tsx",
]:
implementation[name] = digest(Path(name))
for name in implementation:
target = root / "executed-source" / name
target.parent.mkdir(parents=True, exist_ok=True)
shutil.copyfile(name, target)
write(
root / "manifest.json",
dict(
schema_version="missioncore.live-bootstrap-qualification/v1",
created_at_utc=utc_now_iso(),
created_monotonic_ns=time.monotonic_ns(),
input_digests=inputs,
implementation_sha256=implementation,
protocol=BOOTSTRAP_POLICY,
entry_policy=STATIONARY_POLICY,
queue_ingress=args.queue_ingress,
drop_interval_s=args.drop_interval_s,
profile_planning=args.profile_planning,
spatial_stop_monotonic_ns=args.spatial_stop_monotonic_ns,
after_monotonic_ns=args.after_monotonic_ns,
camera_replay=bool(camera),
reference_kind=args.reference_kind,
scene_cadence_s=scene_cadence,
fast_scene=args.fast_scene,
archive_maximum_seconds=maximum_seconds,
**live_route_limits(float(np.linalg.norm(np.diff(path, axis=0), axis=1).sum())),
pace=1,
expectation=(
"wrong-region: reject without tracking; correct-entry: complete prior -> "
"three disjoint fresh fits -> tracking -> end"
),
clock=(
"original receipt deltas rebased to this process monotonic start; "
"original epoch retained"
),
authority=(
"isolated archive adapter; optional in-process ASGI GET only; "
"no listening server, capture, device commands or vehicle control"
),
limitations=(
"recorded startup/motion only; stationary physical wait and scanner UI "
"require field acceptance; optional camera uses real parser/archive/queue "
"and local FFmpeg decoder, not RTSP, browser MSE or acquisition authority"
),
),
)
np.save(root / "qualified-reference.npy", reference, allow_pickle=False)
if reference_provenance is not None:
write(root / "reference-provenance.json", reference_provenance)
draft = dict(
id="archive-probe",
name="Stationary live integration · archive qualification",
revision=1,
zone=dict(session_id=previous["reference_session"], generation="frozen-archive"),
route=dict(
length_m=float(np.linalg.norm(np.diff(path, axis=0), axis=1).sum()),
start_index=0,
end_index=len(path) - 1,
points=[dict(position=p.tolist()) for p in path],
),
)
(root / "runtime").mkdir()
sources = SimpleNamespace(
store=SimpleNamespace(
get_session=lambda _: SimpleNamespace(plugin_id="archive-qualification")
),
reference_map=lambda *args, **kwargs: (
reference,
reference_provenance
or dict(session_id=previous["reference_session"], source_digests=inputs),
),
)
if scene_reference is not None:
sources.scene_reference_map = lambda *args, **kwargs: (scene_reference, scene_provenance)
drafts = SimpleNamespace(
database=root / "runtime" / "drafts.json", sources=sources, get=lambda _: draft
)
source_type = ReceiptQueueArchiveSource if args.queue_ingress else ArchiveSource
source = source_type(
Path(previous["query_raw"]),
previous["query_session"],
maximum_seconds,
after_monotonic_ns=args.after_monotonic_ns,
**(
{"spatial_stop_monotonic_ns": args.spatial_stop_monotonic_ns}
| {"drop_interval_s": args.drop_interval_s}
if args.queue_ingress
else {}
),
)
lock = threading.Lock()
service = PlanningLiveTests(drafts, {"archive-qualification": source}, lock)
if args.profile_planning:
import cProfile
original_work = service.work
def profiled_work(*work_args):
profile = cProfile.Profile()
try:
return profile.runcall(original_work, *work_args)
finally:
profile.dump_stats(str(root / "planning-consumer.prof"))
service.work = profiled_work
observations = []
started_utc = utc_now_iso()
run = service.start(draft["id"], 1)
scene_saved = False
first_tracking_state = None
last_scene = 0
scene_count = 0
scene_measurements = []
cursor = ""
client = None
if args.fast_scene:
from fastapi import FastAPI
from fastapi.testclient import TestClient
from k1link.web.planning_live_api import build_planning_live_router
app = FastAPI()
app.include_router(build_planning_live_router(service))
client = TestClient(app)
client.__enter__()
try:
deadline = time.monotonic() + 10
while service.get()["state"] == "preparing" and time.monotonic() < deadline:
time.sleep(0.02)
assert service.get()["state"] == "waiting"
source.activate()
if camera:
camera.start()
while service.thread.is_alive():
now = time.monotonic_ns()
assert now - source.started < (maximum_seconds + 40) * 1e9, (
"Functional probe wall bound exceeded."
)
state = service.get()
key = (state["state"], state.get("planning_phase"), state.get("result_source_sequence"))
if not observations or key != observations[-1]["key"]:
observations.append(
dict(
key=key,
at_s=(now - source.started) / 1e9,
frame_age_s=state["frame_age_s"],
result_age_s=state["result_age_s"],
tracking_state=state["tracking_state"],
message=state["message"],
source_active=source.snapshot().get("active", False),
recovery_attempt=state.get("recovery_attempt", 0),
accepted_sample=service.accepted_sample is not None,
)
)
print(json.dumps(observations[-1], ensure_ascii=False), flush=True)
if state["tracking_state"] == "tracking" and not scene_saved:
first_tracking_state = state
(root / "tracking.rrd").write_bytes(service.scene(run["id"], True))
scene_saved = True
if now - last_scene >= scene_cadence * 1e9:
before = time.monotonic()
extra = {}
if client:
response = client.get(
f"/api/v1/mission-planner/live-tests/{run['id']}/scene-delta.rrd",
params=dict(cursor=cursor, base=scene_count == 0),
)
assert response.status_code in (200, 204)
cursor = response.headers["X-Planning-Scene-Cursor"]
token = decode_cursor(cursor)
extra = dict(
cloud_revision=token["cloud"],
pose_sequence=token["pose"],
delta_live=token["live"],
display_cloud_age_s=float(response.headers["X-Planning-Cloud-Age"]),
at_s=(now - source.started) / 1e9,
)
payload = response.content
else:
payload = service.scene(run["id"], scene_count == 0)
scene_measurements.append(
dict(
**extra,
seconds=time.monotonic() - before,
bytes=len(payload),
presentation_state=state.get("presentation_state"),
frame_age_s=state["frame_age_s"],
result_age_s=state["result_age_s"],
pose_age_s=state.get("pose_age_s"),
)
)
last_scene, scene_count = now, scene_count + 1
time.sleep(0.01 if client else 0.05)
finally:
if client:
client.__exit__(None, None, None)
service.close()
if camera:
camera.close()
write(root / "deliveries.json", source.deliveries)
write(root / "observations.json", observations)
if args.drop_interval_s is not None:
write(root / "dropped-receipts.json", source.dropped_receipts)
result = service.get()
(root / "terminal.rrd").write_bytes(service.scene(run["id"], True))
write(root / "scene-measurements.json", scene_measurements)
directory = service.directory(run["id"])
steps = []
seen = set()
for step in sorted(directory.glob("step-*")):
sample = json.loads((step / "source.json").read_text())
decision = json.loads((step / "decision.json").read_text())
if sample["role"] == "fresh-validation":
ids = {e["sequence"] for e in sample["events"]}
assert ids and not seen.intersection(ids)
assert all(
sample["fresh_floor_ns"] < e["monotonic_ns"] <= sample["requested_monotonic_ns"]
for e in sample["events"]
)
seen.update(ids)
steps.append(dict(role=sample["role"], **decision))
checks = dict(
completed=result["state"] == "completed",
expected_tracking=scene_saved == (args.reference_kind == "correct-entry"),
prior_not_accepted=result.get("initialization_temporal", {}).get("accepted") is False,
complete_search=(
result.get("initialization_result", {}).get("initialization", {}).get("complete")
is True
and len(result["initialization_result"]["initialization"]["attempts"])
== result["initialization_result"]["initialization"]["expected_attempts"]
),
producer_ok=getattr(source, "error", None) is None,
no_live_authority_after_end=service.accepted_sample is None
and result["tracking_state"] == "lost",
lease_released=source.owner is None and not lock.locked(),
inputs_unchanged=all(digest(Path(p)) == sha for p, sha in inputs.items()),
code_unchanged=all(digest(Path(p)) == sha for p, sha in implementation.items()),
no_green_after_end=result.get("presentation_state") != "live",
terminal_transform_retained=(not scene_saved or service.presentation.result is not None),
)
if camera:
camera_report = camera.report()
write(root / "camera-report.json", camera_report)
checks["camera_passed"] = all(camera_report["checks"].values())
if args.spatial_stop_monotonic_ns is not None:
checks["commanded_stop_not_loss"] = result[
"termination_reason"
] == "spatial-stop-requested" and (
args.drop_interval_s is not None
or (
result.get("recovery_attempt", 0) == 0
and not any(t["phase"] == "lost" for t in result.get("phase_transitions", []))
)
)
checks["capture_retained_at_stop"] = bool(
source.stopping_snapshot and source.stopping_snapshot["active"]
)
if args.drop_interval_s is not None:
start, end = args.drop_interval_s
# The fault probe requires successful acquisition BEFORE the declared
# fault. Recovery may remain unconfirmed if the recorded operator kept
# walking; a synthetic stationary interval must not be manufactured.
prior = (
(first_tracking_state or {}).get("initialization_result", {}).get("initialization", {})
)
checks["complete_search"] = (
prior.get("complete") is True and len(prior["attempts"]) == prior["expected_attempts"]
)
checks["prior_not_accepted"] = (first_tracking_state or {}).get(
"initialization_temporal", {}
).get("accepted") is False
checks["tracking_before_fault"] = any(
o["tracking_state"] == "tracking" and o["at_s"] < start for o in observations
)
checks["fault_exercised"] = bool(source.dropped_receipts)
recovery = [o for o in observations if o["key"][1] == "recovering"]
checks["recovery_keeps_capture"] = bool(recovery) and all(
o["key"][0] == "running" and o["source_active"] for o in recovery
)
checks["recovery_revokes_old_authority"] = bool(recovery) and all(
o["tracking_state"] != "tracking" and not o["accepted_sample"] for o in recovery
)
write(
root / "summary.json",
dict(
started_at_utc=started_utc,
finished_at_utc=utc_now_iso(),
started_monotonic_ns=source.started,
run_id=run["id"],
checks=checks,
steps=steps,
event_count=len(source.deliveries),
scene_count=scene_count,
ingress=source.snapshot().get("queues", {}),
display=result.get("display"),
producer_error=getattr(source, "error", None),
maximum_delivery_lag_s=max(d["lag_s"] for d in source.deliveries),
vehicle_control=False,
localization_confirmed=False,
),
)
write(
root / "seal.json",
{str(p.relative_to(root)): digest(p) for p in root.rglob("*") if p.is_file()},
)
print(json.dumps(checks), flush=True)
assert all(checks.values()), "See retained integration evidence."
if __name__ == "__main__":
main()