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.
99 lines
4.4 KiB
Python
99 lines
4.4 KiB
Python
"""Candidate fallback through the real service loop; synthetic fit outcomes only."""
|
|
|
|
import json
|
|
from types import SimpleNamespace
|
|
|
|
import numpy as np
|
|
import pytest
|
|
from test_planning_live import event, fixture_service, until
|
|
|
|
from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
|
|
|
|
|
|
@pytest.mark.parametrize("first_stage", ["route", "dense-start"])
|
|
def test_service_confirms_next_hypothesis_without_capture_restart(
|
|
tmp_path, monkeypatch, first_stage,
|
|
):
|
|
import k1link.missions.live_tests as module
|
|
|
|
service, source, lock, _ = fixture_service(tmp_path, monkeypatch)
|
|
clock = [100.0]
|
|
monkeypatch.setattr(module, "time", SimpleNamespace(
|
|
monotonic=lambda: clock[0], monotonic_ns=lambda: int(clock[0] * 1e9)))
|
|
initial_calls, fit_seeds = [], []
|
|
alternative = np.eye(4)
|
|
alternative[0, 3] = 10
|
|
|
|
def initialize(directory, ref, path, query, anchor, **kwargs):
|
|
initial_calls.append(kwargs)
|
|
matrix = alternative if kwargs.get("route_only") else np.eye(4)
|
|
info = dict(complete=True, scope="selected-route", policy=ROUTE_RELOCALIZATION_POLICY,
|
|
expected_attempts=1, attempts=[{}], selected_candidate_index=0)
|
|
if first_stage == "dense-start" and len(initial_calls) == 1:
|
|
info["stages"] = [dict(name="dense-start")]
|
|
elif len(initial_calls) == 1:
|
|
info["candidate_queue"] = [
|
|
dict(candidate_index=0, T_reference_query=matrix.tolist(), ambiguous=False),
|
|
dict(candidate_index=2, T_reference_query=alternative.tolist(), ambiguous=False),
|
|
]
|
|
return dict(status="candidate", T_reference_query=matrix.tolist(), overlap=.95,
|
|
inlier_rmse_m=.1, reasons=[], matched_query_indices=[], initialization=info)
|
|
|
|
def calculate(directory, ref, query, hint):
|
|
fit_seeds.append(hint.copy())
|
|
if len(fit_seeds) == 1:
|
|
return dict(status="rejected", T_reference_query=hint.tolist(), overlap=.5,
|
|
inlier_rmse_m=.3, reasons=["fixture rejection"], matched_query_indices=[])
|
|
return dict(status="candidate", T_reference_query=hint.tolist(), overlap=.95,
|
|
inlier_rmse_m=.1, reasons=[], matched_query_indices=[0])
|
|
|
|
monkeypatch.setattr(module, "run_route_relocalization", initialize)
|
|
monkeypatch.setattr(module, "run_registration", calculate)
|
|
run = service.start("draft", 1)
|
|
points = np.random.default_rng(11).uniform([-1, -3, -1], [8, 3, 3], (1500, 3))
|
|
sequence = 0
|
|
|
|
def frame(stamp):
|
|
nonlocal sequence
|
|
clock[0] = stamp + .002
|
|
sequence += 1
|
|
source.queue.put(event("pose", t=stamp, sequence=sequence))
|
|
sequence += 1
|
|
source.queue.put(event("points", t=stamp + .001, sequence=sequence, points=points))
|
|
until(source.queue.empty)
|
|
|
|
try:
|
|
until(lambda: service.get()["state"] == "waiting")
|
|
source.state.update(active=True, session_id="B", session_generation=2)
|
|
for i in range(140):
|
|
frame(100 + i * .5)
|
|
if service.get()["tracking_state"] == "tracking":
|
|
break
|
|
until(lambda: service.get()["tracking_state"] == "tracking")
|
|
assert service.get()["initialization_attempt"] == 1
|
|
assert service.get()["reinitialization_count"] == 0
|
|
assert source.state["active"] and source.owner == "planning-" + run["id"]
|
|
assert all(np.allclose(seed, alternative) for seed in fit_seeds[1:])
|
|
assert len(fit_seeds) >= 4 # Rejection plus three independent accepted windows.
|
|
if first_stage == "dense-start":
|
|
assert initial_calls == [{}, {"route_only": True}]
|
|
else:
|
|
assert initial_calls == [{}]
|
|
seen = set()
|
|
sources = sorted(service.directory(run["id"]).glob("step-*/source.json"))
|
|
for path in sources:
|
|
sample = json.loads(path.read_text())
|
|
if sample["role"] != "fresh-validation":
|
|
continue
|
|
ids = {item["sequence"] for item in sample["events"]}
|
|
assert not seen.intersection(ids)
|
|
assert all(item["monotonic_ns"] > sample["fresh_floor_ns"]
|
|
for item in sample["events"])
|
|
seen.update(ids)
|
|
source.state["active"] = False
|
|
until(lambda: service.get()["state"] == "completed")
|
|
assert service.accepted_sample is None
|
|
finally:
|
|
service.close()
|
|
assert not lock.locked() and source.owner is None
|