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.
123 lines
5.0 KiB
Python
123 lines
5.0 KiB
Python
"""Small causal fixtures for fault injection and heading-free entry."""
|
|
|
|
from dataclasses import replace
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from k1link.missions.causal_tracking import CausalTracking
|
|
from k1link.missions.entry_acquisition import choose_entry, entry_seeds
|
|
from k1link.missions.replay_faults import drop_receipts
|
|
from k1link.missions.stationary_entry import STATIONARY_POLICY, stationary_prefix
|
|
from k1link.sessions.live_planning import PlanningLiveEvent
|
|
|
|
|
|
def test_receipt_fault_keeps_identity_time_and_payload_of_survivors():
|
|
e = PlanningLiveEvent("B", 1, 1, 1_000_000_000, 1, "pose", position=(0, 0, 0))
|
|
events = [
|
|
replace(e, sequence=i + 1, monotonic_ns=int((t + 1) * 1e9))
|
|
for i, t in enumerate([0, 43.9, 44, 45, 46.99, 47, 48])
|
|
]
|
|
audit = {}
|
|
kept = list(drop_receipts(iter(events), 44, 47, audit))
|
|
assert kept == [events[i] for i in [0, 1, 5, 6]]
|
|
assert all(x is events[x.sequence - 1] for x in kept)
|
|
assert [x["sequence"] for x in audit["dropped"]] == [3, 4, 5]
|
|
for start, end in [(0, 2), (4, 3), (1, 121), (1, float("nan"))]:
|
|
with pytest.raises(ValueError):
|
|
list(drop_receipts(iter(events), start, end, {}))
|
|
|
|
|
|
def stationary_events():
|
|
points = np.random.default_rng(23).uniform([-4, -4, -1], [4, 4, 3], (1500, 3))
|
|
e = PlanningLiveEvent("B", 1, 1, 1_000_000_000, 1, "pose", position=(0, 0, 0))
|
|
for i in range(21):
|
|
t = i * 0.5
|
|
yield replace(
|
|
e, sequence=2 * i + 1, monotonic_ns=int((t + 1) * 1e9), position=(0.001 * i / 20, 0, 0)
|
|
)
|
|
yield replace(
|
|
e, sequence=2 * i + 2, monotonic_ns=int((t + 1.001) * 1e9), kind="points", points=points
|
|
)
|
|
yield replace(e, sequence=100, monotonic_ns=12_000_000_000, position=(999, 999, 999))
|
|
|
|
|
|
def test_stationary_prefix_has_no_future_motion_or_heading():
|
|
path = np.array([[10, 20, 0], [14, 20, 0]])
|
|
sample, initial, basis, meta = stationary_prefix(stationary_events(), path)
|
|
assert meta["maximum_motion_m"] == pytest.approx(0.001)
|
|
assert max(x["time_s"] for x in meta["source_events"]) <= 10
|
|
assert np.allclose(initial[:3, :3], np.eye(3))
|
|
assert np.allclose(initial[:3, 3], [10, 20, 0])
|
|
assert len(sample["path"]) == 1 and len(sample["points"]) >= 300
|
|
assert np.allclose(basis, [4, 0, 0])
|
|
|
|
|
|
def test_stationary_prefix_rejects_motion_even_if_buffer_would_thin_it():
|
|
events = list(stationary_events())
|
|
events[2] = replace(events[2], position=(0.11, 0, 0))
|
|
with pytest.raises(ValueError, match="not stationary"):
|
|
stationary_prefix(events, np.array([[0, 0, 0], [4, 0, 0]]))
|
|
with pytest.raises(ValueError, match="Incomplete"):
|
|
stationary_prefix(events[:2], np.array([[0, 0, 0], [4, 0, 0]]))
|
|
|
|
|
|
def test_stationary_all_yaws_rotate_at_entry_and_require_complete_search():
|
|
anchor = np.array([40.0, 30.0, 2.0])
|
|
initial = np.eye(4)
|
|
initial[:3, 3] = [7, 8, 0]
|
|
seeds = list(entry_seeds(initial, anchor, [1, 0, 0], policy=STATIONARY_POLICY))
|
|
assert len(seeds) == 108 and set(x["yaw_deg"] for x in seeds) == set(range(0, 360, 30))
|
|
attempts = []
|
|
for seed in seeds:
|
|
matrix = seed.pop("matrix")
|
|
assert np.allclose(
|
|
(matrix @ np.r_[anchor, 1])[:3], anchor + [7 + seed["along_m"], 8 + seed["across_m"], 0]
|
|
)
|
|
# All fits converge to the same half-turn solution at the entry anchor.
|
|
final = initial.copy()
|
|
final[:2, :2] = -np.eye(2)
|
|
final[:3, 3] = (initial @ np.r_[anchor, 1])[:3] - final[:3, :3] @ anchor
|
|
attempts.append(
|
|
{
|
|
**seed,
|
|
"result": dict(
|
|
status="candidate",
|
|
T_reference_query=final.tolist(),
|
|
overlap=0.95,
|
|
inlier_rmse_m=0.1,
|
|
matched_query_indices=[0],
|
|
),
|
|
}
|
|
)
|
|
assert (
|
|
choose_entry(attempts, initial, anchor, policy=STATIONARY_POLICY)["status"] == "candidate"
|
|
)
|
|
assert choose_entry(attempts[:27], initial, anchor, policy=STATIONARY_POLICY)["reasons"] == [
|
|
"incomplete-search"
|
|
]
|
|
|
|
|
|
def test_post_tracking_gap_requires_three_new_segment_windows():
|
|
gate = CausalTracking()
|
|
fit = dict(status="candidate", T_reference_query=np.eye(4).tolist())
|
|
|
|
def sample(t, segment):
|
|
return dict(
|
|
monotonic_ns=int(t * 1e9), segment=segment, path=np.array([[0, 0, 0], [25, 0, 0]])
|
|
)
|
|
|
|
for t in (30, 35, 40):
|
|
assert gate.accept(fit, sample(t, 0), int((t + 0.2) * 1e9), 0)["accepted"]
|
|
assert gate.state == "tracking"
|
|
gate.tick(44_000_000_000, 1)
|
|
assert gate.state == "lost" and gate.matrix is None
|
|
stale = gate.accept(fit, sample(41, 0), 44_100_000_000, 1)
|
|
assert not stale["accepted"] and gate.streak == 0 and gate.matrix is None
|
|
for i, t in enumerate((45, 50, 55)):
|
|
assert gate.accept(fit, sample(t, 1), int((t + 0.2) * 1e9), 1)["accepted"]
|
|
assert gate.streak == i + 1
|
|
assert gate.state == ("tracking" if i == 2 else "acquiring")
|
|
gate.clear("input-ended")
|
|
assert gate.matrix is None and gate.state == "lost"
|