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:
@@ -0,0 +1,160 @@
|
||||
"""Tiny deterministic fixtures: temporal authority and causality, not load."""
|
||||
|
||||
import json
|
||||
from dataclasses import replace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from k1link.missions.causal_replay import replay
|
||||
from k1link.missions.causal_tracking import CausalTracking
|
||||
from k1link.sessions.live_planning import PlanningLiveEvent
|
||||
|
||||
|
||||
def sample(t, segment=0):
|
||||
return dict(monotonic_ns=int(t * 1e9), segment=segment, path=np.array([[0.0, 0, 0], [3, 0, 0]]))
|
||||
|
||||
|
||||
def result(x=0, status="candidate"):
|
||||
matrix = np.eye(4)
|
||||
matrix[0, 3] = x
|
||||
return dict(status=status, T_reference_query=matrix.tolist())
|
||||
|
||||
|
||||
def test_three_consistent_candidates_and_timeout():
|
||||
gate = CausalTracking()
|
||||
for t in (1, 6, 11):
|
||||
assert gate.accept(result(), sample(t), int((t + 1) * 1e9), 0)["accepted"]
|
||||
assert gate.state == "tracking" and gate.streak == 3
|
||||
gate.tick(20_000_000_000, 0)
|
||||
assert gate.state == "lost" and gate.matrix is None and gate.reason == "stale"
|
||||
|
||||
|
||||
def test_inconsistent_candidate_is_not_seeded_or_counted():
|
||||
gate = CausalTracking()
|
||||
gate.accept(result(), sample(1), 2_000_000_000, 0)
|
||||
rejection = gate.accept(result(0.6), sample(6), 7_000_000_000, 0)
|
||||
assert not rejection["accepted"] and rejection["reason"] == "inconsistent-candidate"
|
||||
assert gate.matrix is None and gate.streak == 0
|
||||
gate.accept(result(0.6), sample(11), 12_000_000_000, 0)
|
||||
assert gate.streak == 1 and gate.state == "acquiring"
|
||||
|
||||
|
||||
def test_old_segment_result_does_not_override_current_candidate():
|
||||
gate = CausalTracking()
|
||||
gate.accept(result(), sample(1), 2_000_000_000, 0)
|
||||
gate.tick(3_000_000_000, 1)
|
||||
assert gate.matrix is None
|
||||
gate.accept(result(0.1), sample(4, 1), 5_000_000_000, 1)
|
||||
old = gate.accept(result(10), sample(2), 6_000_000_000, 1)
|
||||
assert old["reason"] == "old-segment" and not old["accepted"]
|
||||
assert gate.streak == 1 and gate.matrix[0, 3] == pytest.approx(0.1)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"status,stamp,now,reason",
|
||||
[
|
||||
("rejected", 1, 2, "registration-rejected"),
|
||||
("candidate", 1, 10, "stale-result"),
|
||||
("candidate", 3, 2, "stale-result"),
|
||||
],
|
||||
)
|
||||
def test_bad_or_stale_fit_cannot_establish_tracking(status, stamp, now, reason):
|
||||
gate = CausalTracking()
|
||||
assert not gate.accept(result(status=status), sample(stamp), int(now * 1e9), 0)["accepted"]
|
||||
assert gate.reason == reason and gate.matrix is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize("mode", ["baseline", "acquisition"])
|
||||
def test_replay_fit_never_contains_future_points(tmp_path, mode):
|
||||
points = np.random.default_rng(11).uniform([0, -3, -1], [8, 3, 3], (1500, 3))
|
||||
start = PlanningLiveEvent("B", 1, 1, 1_000_000_000, 1, "pose", position=(0, 0, 0))
|
||||
events = [
|
||||
start,
|
||||
replace(start, sequence=2, monotonic_ns=1_001_000_000, position=(3, 0, 0)),
|
||||
replace(start, kind="points", sequence=3, monotonic_ns=1_002_000_000, points=points),
|
||||
replace(start, sequence=4, monotonic_ns=1_100_000_000, position=(3.1, 0, 0)),
|
||||
replace(start, kind="points", sequence=5, monotonic_ns=1_110_000_000, points=points + 100),
|
||||
]
|
||||
calls = []
|
||||
|
||||
def calculate(directory, ref, query, hint):
|
||||
calls.append(query.copy())
|
||||
return result()
|
||||
|
||||
report = replay(
|
||||
iter(events),
|
||||
points,
|
||||
np.array([[0, 0, 0], [30, 0, 0]]),
|
||||
tmp_path / "replay",
|
||||
calculate=calculate,
|
||||
initialize=lambda directory, ref, query, hint, anchor, forward: calculate(
|
||||
directory, ref, query, hint
|
||||
),
|
||||
mode=mode,
|
||||
max_seconds=0.2,
|
||||
)
|
||||
assert len(calls) == 1 and calls[0].max() < 20
|
||||
assert report["steps"][0]["sequence"] == 3
|
||||
assert max(e["sequence"] for e in report["steps"][0]["source_events"]) == 3
|
||||
assert report["transitions"][-1]["reason"] == "input-ended"
|
||||
assert json.loads((tmp_path / "replay/report.json").read_text())["vehicle_control"] is False
|
||||
|
||||
|
||||
def test_empty_or_oversized_replay_fails(tmp_path):
|
||||
with pytest.raises(ValueError, match="bounds"):
|
||||
replay([], [], [], tmp_path / "too-long", max_seconds=121)
|
||||
with pytest.raises(ValueError, match="Empty"):
|
||||
replay([], [], [], tmp_path / "empty")
|
||||
|
||||
|
||||
def test_job_finishing_after_input_end_is_historical_only(tmp_path):
|
||||
import time
|
||||
|
||||
points = np.random.default_rng(21).uniform([0, -3, -1], [8, 3, 3], (1500, 3))
|
||||
start = PlanningLiveEvent("B", 1, 1, 1_000_000_000, 1, "pose", position=(0, 0, 0))
|
||||
events = [
|
||||
start,
|
||||
replace(start, sequence=2, monotonic_ns=1_001_000_000, position=(3, 0, 0)),
|
||||
replace(start, kind="points", sequence=3, monotonic_ns=1_002_000_000, points=points),
|
||||
]
|
||||
|
||||
def delayed(directory, reference, query, hint):
|
||||
time.sleep(0.05)
|
||||
return result()
|
||||
|
||||
report = replay(
|
||||
iter(events),
|
||||
points,
|
||||
np.array([[0, 0, 0], [30, 0, 0]]),
|
||||
tmp_path / "ended",
|
||||
calculate=delayed,
|
||||
max_seconds=0.2,
|
||||
)
|
||||
assert report["first_candidate_s"] is None
|
||||
assert report["steps"][0]["temporal"]["reason"] == "input-ended"
|
||||
assert not report["steps"][0]["temporal"]["accepted"]
|
||||
assert report["transitions"][-1]["time_s"] < report["steps"][0]["completed_s"]
|
||||
|
||||
|
||||
def test_archived_adapter_requires_real_clock_and_reuses_live_decoder(tmp_path):
|
||||
from test_stream_summary import _pose_payload
|
||||
from test_viewer_replay import _write_native
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.planning_replay import iter_planning_events
|
||||
|
||||
source = tmp_path / "mqtt.raw.k1mqtt"
|
||||
_write_native(source, "x/lio_pose", _pose_payload((5, 1, 2)))
|
||||
with pytest.raises(ValueError, match="metadata"):
|
||||
list(iter_planning_events(source, "B"))
|
||||
meta = dict(
|
||||
record_type="message", sequence=1, received_at_epoch_ns=10, received_monotonic_ns=20
|
||||
)
|
||||
source.with_name("mqtt.metadata.jsonl").write_text(json.dumps(meta) + "\n")
|
||||
events = list(iter_planning_events(source, "B"))
|
||||
assert len(events) == 1 and events[0].position == (5.0, 1.0, 2.0)
|
||||
assert events[0].monotonic_ns == 20 and events[0].epoch_ns == 10
|
||||
del meta["received_monotonic_ns"]
|
||||
source.with_name("mqtt.metadata.jsonl").write_text(json.dumps(meta) + "\n")
|
||||
with pytest.raises(ValueError, match="clock"):
|
||||
list(iter_planning_events(source, "B"))
|
||||
@@ -0,0 +1,158 @@
|
||||
"""Deterministic entry hypotheses, ambiguity and bounded numeric qualification."""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from k1link.missions.entry_acquisition import acquire_entry, choose_entry, entry_seeds
|
||||
from k1link.missions.registration import angle_deg, transform
|
||||
|
||||
|
||||
def attempts(matrix=None):
|
||||
matrix = np.eye(4) if matrix is None else matrix
|
||||
return [
|
||||
dict(
|
||||
index=i,
|
||||
along_m=(i // 9 - 1) * 3,
|
||||
across_m=(i // 3 % 3 - 1) * 3,
|
||||
yaw_deg=(i % 3 - 1) * 15,
|
||||
result=dict(
|
||||
status="candidate",
|
||||
T_reference_query=matrix.tolist(),
|
||||
overlap=0.95,
|
||||
inlier_rmse_m=0.1,
|
||||
matched_query_indices=[0],
|
||||
registration_seconds=0.01,
|
||||
),
|
||||
)
|
||||
for i in range(27)
|
||||
]
|
||||
|
||||
|
||||
def test_seeds_rotate_about_query_entry_and_span_route_basis():
|
||||
initial = np.eye(4)
|
||||
initial[:3, 3] = [10, 20, 1]
|
||||
anchor = np.array([100, 200, 3.0])
|
||||
seeds = list(entry_seeds(initial, anchor, [0, 4, 0]))
|
||||
assert len(seeds) == 27
|
||||
for seed in seeds:
|
||||
expected = transform(anchor[None], initial)[0] + [-seed["across_m"], seed["along_m"], 0]
|
||||
assert np.allclose(transform(anchor[None], seed["matrix"])[0], expected)
|
||||
assert np.linalg.det(seed["matrix"][:3, :3]) == pytest.approx(1)
|
||||
with pytest.raises(ValueError):
|
||||
list(entry_seeds(initial, anchor, [0, 0, 0]))
|
||||
|
||||
|
||||
def test_single_supported_solution_does_not_relax_local_rejection():
|
||||
data = attempts()
|
||||
data[0]["result"]["status"] = "rejected"
|
||||
result = choose_entry(data, np.eye(4), [0, 0, 0])
|
||||
assert result["status"] == "candidate"
|
||||
assert result["initialization"]["clusters"][0]["support"] == 26
|
||||
assert not result["initialization"]["attempts"][0]["entry_admitted"]
|
||||
|
||||
|
||||
def test_two_near_equal_place_solutions_are_ambiguous_even_with_unequal_support():
|
||||
data = attempts()
|
||||
alternative = np.eye(4)
|
||||
alternative[0, 3] = 2
|
||||
data[-1]["result"].update(
|
||||
T_reference_query=alternative.tolist(), overlap=0.94, inlier_rmse_m=0.11
|
||||
)
|
||||
result = choose_entry(data, np.eye(4), [0, 0, 0])
|
||||
assert result["status"] == "rejected" and result["reasons"] == ["ambiguous-entry"]
|
||||
assert result["matched_query_indices"] == []
|
||||
|
||||
|
||||
def test_cluster_is_pairwise_not_a_chain_between_distant_places():
|
||||
data = attempts()
|
||||
for i, item in enumerate(data):
|
||||
matrix = np.eye(4)
|
||||
matrix[0, 3] = (i % 3) * 0.4
|
||||
item["result"]["T_reference_query"] = matrix.tolist()
|
||||
result = choose_entry(data, np.eye(4), [0, 0, 0])
|
||||
assert len(result["initialization"]["clusters"]) == 2
|
||||
assert result["reasons"] == ["ambiguous-entry"]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("offset", [[6, 0, 0], [0, 0, 1.1]])
|
||||
def test_solution_outside_entry_region_is_rejected(offset):
|
||||
matrix = np.eye(4)
|
||||
matrix[:3, 3] = offset
|
||||
result = choose_entry(attempts(matrix), np.eye(4), [0, 0, 0])
|
||||
assert result["reasons"] == ["no-admissible-entry"]
|
||||
|
||||
|
||||
def test_angles_and_multiple_translation_starts_required():
|
||||
data = attempts()
|
||||
for item in data[3:]:
|
||||
item["result"]["status"] = "rejected"
|
||||
assert choose_entry(data, np.eye(4), [0, 0, 0])["reasons"] == [
|
||||
"insufficient-multistart-support"
|
||||
]
|
||||
matrix = np.eye(4)
|
||||
a = np.radians(31)
|
||||
matrix[:2, :2] = [[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]
|
||||
assert choose_entry(attempts(matrix), np.eye(4), [0, 0, 0])["status"] == "rejected"
|
||||
|
||||
|
||||
def test_partial_search_and_deadline_cannot_claim_unique_solution():
|
||||
assert choose_entry(attempts(), np.eye(4), [0, 0, 0], complete=False)["reasons"] == [
|
||||
"incomplete-search"
|
||||
]
|
||||
times = iter([0, 1, 26, 26])
|
||||
points = np.random.default_rng(1).normal(size=(400, 3))
|
||||
output = acquire_entry(
|
||||
points,
|
||||
points,
|
||||
np.eye(4),
|
||||
[0, 0, 0],
|
||||
[1, 0, 0],
|
||||
fitter=lambda *args: attempts()[0]["result"],
|
||||
clock=lambda: next(times),
|
||||
)
|
||||
assert len(output["initialization"]["attempts"]) == 1
|
||||
assert output["reasons"] == ["incomplete-search"]
|
||||
|
||||
|
||||
def test_recovers_known_transform_from_several_starts():
|
||||
pytest.importorskip("small_gicp")
|
||||
from test_mission_registration import geometry
|
||||
|
||||
ref = geometry()
|
||||
truth = np.eye(4)
|
||||
a = np.radians(10)
|
||||
truth[:2, :2] = [[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]
|
||||
truth[:3, 3] = [0.5, 2.6, 0.1]
|
||||
query = transform(ref, np.linalg.inv(truth))
|
||||
result = acquire_entry(ref, query, np.eye(4), [0, 0, 0], [1, 0, 0])
|
||||
assert result["status"] == "candidate", result["reasons"]
|
||||
found = np.asarray(result["T_reference_query"])
|
||||
assert np.linalg.norm(found[:3, 3] - truth[:3, 3]) < 0.03
|
||||
assert angle_deg(found[:3, :3] @ truth[:3, :3].T) < 0.3
|
||||
assert result["policy"]["maximum_correction_m"] == 3
|
||||
assert not result["localization_confirmed"] and not result["vehicle_control"]
|
||||
|
||||
|
||||
def test_centre_first_search_preserves_all_seed_identities():
|
||||
from k1link.missions.stationary_entry import STATIONARY_POLICY
|
||||
seeds = list(entry_seeds(np.eye(4), [0, 0, 0], [1, 0, 0], policy=STATIONARY_POLICY))
|
||||
assert (seeds[0]["along_m"], seeds[0]["across_m"], seeds[0]["yaw_deg"]) == (0, 0, 0)
|
||||
assert {s["index"] for s in seeds} == set(range(108))
|
||||
assert len({(s["along_m"], s["across_m"], s["yaw_deg"]) for s in seeds}) == 108
|
||||
|
||||
|
||||
def test_prepared_target_is_identical_across_rejected_and_accepted_fits():
|
||||
from k1link.missions.registration import PreparedReference, register
|
||||
from test_mission_registration import geometry
|
||||
ref = geometry()
|
||||
prepared = PreparedReference(ref)
|
||||
query = ref + [.7, -.5, .2]
|
||||
first = prepared.register(query, np.eye(4))
|
||||
assert prepared.register(query + [100, 0, 0], np.eye(4))["status"] == "rejected"
|
||||
second = prepared.register(query, np.eye(4))
|
||||
standalone = register(ref, query, np.eye(4))
|
||||
for result in (first, second, standalone):
|
||||
result.pop("registration_seconds")
|
||||
assert first == second == standalone
|
||||
assert first["status"] == "candidate"
|
||||
assert np.allclose(np.asarray(first["T_reference_query"])[:3, 3], [-.7, .5, -.2], atol=.02)
|
||||
@@ -181,6 +181,10 @@ from k1link.device_plugins.xgrids_k1.node_bridge import NodeBridge
|
||||
assert "k1link.laboratory.execution" not in sys.modules
|
||||
assert "k1link.device_plugins.xgrids_k1.legacy_api" not in sys.modules
|
||||
assert "k1link.compute.jobs" not in sys.modules
|
||||
assert "k1link.missions.live_tests" not in sys.modules
|
||||
assert "k1link.device_plugins.xgrids_k1.planning_live" not in sys.modules
|
||||
from k1link.sessions.live_planning import PlanningLiveEvent
|
||||
assert PlanningLiveEvent("fixture", 1, 1, 1, 1, "ignored").points is None
|
||||
import k1link
|
||||
assert pathlib.Path(k1link.__file__).is_relative_to(stage)
|
||||
bridge = NodeBridge(stage)
|
||||
|
||||
@@ -31,7 +31,7 @@ def test_live_ingress_keeps_modalities_separately_bounded_and_ordered() -> None:
|
||||
received_monotonic_ns=200 + sequence,
|
||||
payload=f"camera-{sequence}".encode(),
|
||||
)
|
||||
for sequence in range(9):
|
||||
for sequence in range(33):
|
||||
assert ingress.publish(
|
||||
modality="lidar",
|
||||
source_id="lixel/application/report/lio_pcl",
|
||||
@@ -40,7 +40,7 @@ def test_live_ingress_keeps_modalities_separately_bounded_and_ordered() -> None:
|
||||
received_monotonic_ns=400 + sequence,
|
||||
payload=f"lidar-{sequence}".encode(),
|
||||
)
|
||||
for sequence in range(20):
|
||||
for sequence in range(36):
|
||||
assert ingress.publish(
|
||||
modality="pose",
|
||||
source_id="lixel/application/report/lio_pose",
|
||||
@@ -53,9 +53,9 @@ def test_live_ingress_keeps_modalities_separately_bounded_and_ordered() -> None:
|
||||
snapshot = ingress.snapshot()
|
||||
assert snapshot["queues"]["camera-frame"]["depth"] == 2
|
||||
assert snapshot["queues"]["camera-frame"]["dropped_overflow"] == 3
|
||||
assert snapshot["queues"]["lidar"]["depth"] == 8
|
||||
assert snapshot["queues"]["lidar"]["depth"] == 32
|
||||
assert snapshot["queues"]["lidar"]["dropped_overflow"] == 1
|
||||
assert snapshot["queues"]["pose"]["depth"] == 16
|
||||
assert snapshot["queues"]["pose"]["depth"] == 32
|
||||
assert snapshot["queues"]["pose"]["dropped_overflow"] == 4
|
||||
|
||||
events = []
|
||||
@@ -70,10 +70,10 @@ def test_live_ingress_keeps_modalities_separately_bounded_and_ordered() -> None:
|
||||
4,
|
||||
]
|
||||
assert [event.source_sequence for event in events if event.modality == "lidar"] == list(
|
||||
range(1, 9)
|
||||
range(1, 33)
|
||||
)
|
||||
assert [event.source_sequence for event in events if event.modality == "pose"] == list(
|
||||
range(4, 20)
|
||||
range(4, 36)
|
||||
)
|
||||
|
||||
|
||||
@@ -105,6 +105,27 @@ def test_live_ingress_wire_is_self_delimiting_and_explicitly_non_authoritative()
|
||||
assert encoded[4 + header_bytes :] == b"init"
|
||||
|
||||
|
||||
def test_spatial_stop_is_identity_bound_idempotent_and_keeps_capture_active() -> None:
|
||||
ingress = LivePerceptionIngress()
|
||||
ingress.open_consumer("planning")
|
||||
ingress.begin_session("A")
|
||||
assert not ingress.request_spatial_stop("other", 1)
|
||||
assert not ingress.request_spatial_stop("A", 2)
|
||||
assert not ingress.snapshot()["spatial_stop_requested"]
|
||||
assert ingress.request_spatial_stop("A", 1)
|
||||
assert ingress.request_spatial_stop("A", 1)
|
||||
assert ingress.snapshot()["active"]
|
||||
assert ingress.snapshot()["queues"]["control"]["published"] == 2
|
||||
assert ingress.publish(modality="pose", source_id="pose", source_sequence=1,
|
||||
captured_at_epoch_ns=1, received_monotonic_ns=1, payload=b"raw")
|
||||
ingress.end_session("A")
|
||||
ingress.begin_session("A") # Same name, new generation cannot inherit STOP.
|
||||
assert not ingress.request_spatial_stop("A", 1)
|
||||
assert not ingress.snapshot()["spatial_stop_requested"]
|
||||
assert ingress.snapshot()["active"]
|
||||
ingress.close()
|
||||
|
||||
|
||||
def test_live_ingress_rejects_oversize_without_affecting_other_modalities() -> None:
|
||||
ingress = LivePerceptionIngress()
|
||||
ingress.begin_session("session-1")
|
||||
|
||||
@@ -58,6 +58,8 @@ def test_launch_agent_plan_disables_sync_and_enables_watchdog(tmp_path: Path) ->
|
||||
assert desired["RunAtLoad"] is True
|
||||
assert desired["AbandonProcessGroup"] is False
|
||||
assert desired["ExitTimeOut"] == 20
|
||||
assert desired["ProcessType"] == "Interactive"
|
||||
assert plan.to_dict()["desired_process_type"] == "Interactive"
|
||||
assert plan.current_sha256 != plan.desired_sha256
|
||||
assert plan.current_working_directory == repository
|
||||
assert plan.desired_working_directory == repository
|
||||
@@ -68,6 +70,28 @@ def test_launch_agent_plan_disables_sync_and_enables_watchdog(tmp_path: Path) ->
|
||||
assert plan.to_dict()["changes"]["local_observatory_worker_enabled"] is False
|
||||
|
||||
|
||||
def test_background_migration_changes_scheduling_without_changing_operator_state(tmp_path):
|
||||
repository = tmp_path / "repo"
|
||||
repository.mkdir()
|
||||
uv = tmp_path / "uv"
|
||||
uv.write_text("#!/bin/sh\n")
|
||||
agent = tmp_path / "agent.plist"
|
||||
environment = {"PATH": "/usr/bin:/bin", "MISSIONCORE_DATA_DIR": "/existing/evidence"}
|
||||
_write_agent(path=agent, repository=repository, uv_entrypoint=uv, environment=environment)
|
||||
previous = plistlib.loads(agent.read_bytes())
|
||||
previous["ProcessType"] = "Background"
|
||||
agent.write_bytes(plistlib.dumps(previous))
|
||||
original = agent.read_bytes()
|
||||
plan = plan_mission_core_launch_agent(repository_root=repository, agent_path=agent)
|
||||
desired = plistlib.loads(plan.desired_payload)
|
||||
assert plan.current_process_type == "Background"
|
||||
assert desired["ProcessType"] == "Interactive"
|
||||
assert desired["EnvironmentVariables"] == {**environment, "MISSIONCORE_SERVICE_WATCHDOG": "1"}
|
||||
assert desired["AbandonProcessGroup"] is False
|
||||
assert "Nice" not in desired and "HardResourceLimits" not in desired
|
||||
assert agent.read_bytes() == original # A plan never changes the running service.
|
||||
|
||||
|
||||
def test_launch_agent_plan_explicitly_enables_local_observatory_worker(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
|
||||
@@ -0,0 +1,110 @@
|
||||
from types import SimpleNamespace
|
||||
from pathlib import Path
|
||||
import json
|
||||
import pytest
|
||||
from fastapi import FastAPI
|
||||
from fastapi.testclient import TestClient
|
||||
from test_stream_summary import _write_capture, _pcl_payload, _pose_payload
|
||||
from test_session_recording import _command
|
||||
from k1link.device_plugins.xgrids_k1.planning_source import export_planning_source
|
||||
from k1link.missions.sources import PlanningSources
|
||||
from k1link.missions.drafts import MissionDrafts, DraftConflict, route_from_source
|
||||
from k1link.web.mission_planner_api import DraftRequest, build_mission_planner_router
|
||||
|
||||
|
||||
def source_doc():
|
||||
return {'session_id': 'session-a', 'generation': 'a'*64, 'label': 'Route A', 'units': 'm',
|
||||
'frame_id': 'session/session-a', 'source_digests': {'primary': 'b'*64}, 'decode_errors': 0,
|
||||
'poses': [{'index': i, 'position': [i*3, i*4, 0]} for i in range(4)]}
|
||||
|
||||
|
||||
class Sources:
|
||||
changed = False
|
||||
def bound(self, id, generation):
|
||||
if self.changed or id != 'session-a' or generation != 'a'*64:
|
||||
raise ValueError('source changed')
|
||||
return source_doc()
|
||||
verify = bound
|
||||
|
||||
|
||||
def request(**values):
|
||||
return DraftRequest(**dict({'name': 'Route', 'session_id': 'session-a', 'generation': 'a'*64,
|
||||
'start_index': 0, 'end_index': 2}, **values))
|
||||
|
||||
|
||||
def test_export_preserves_all_pose_indices_and_does_not_transform_twice(tmp_path):
|
||||
src = tmp_path / 'mqtt.raw.k1mqtt'
|
||||
_write_capture(src, [('lixel/application/report/lio_pcl', _pcl_payload(scaler=1000, point_count=4)),
|
||||
*[('lixel/application/report/lio_pose', _pose_payload(xyz)) for xyz in [(0, 0, 0), (3, 4, 0), (0, 0, 0)]]])
|
||||
path = tmp_path / 'planning.json'
|
||||
export_planning_source(src, path)
|
||||
doc = json.loads(path.read_text())
|
||||
assert [p['index'] for p in doc['poses']] == [0, 1, 2]
|
||||
assert doc['poses'][1]['position'] == [3, 4, 0]
|
||||
assert doc['poses'][1]['distance_m'] == 5
|
||||
assert doc['path_m'] == 10
|
||||
assert all(p['elapsed_s'] is None for p in doc['poses'])
|
||||
|
||||
|
||||
def test_export_rejects_camera_or_pose_only_recording(tmp_path):
|
||||
src = tmp_path / 'mqtt.raw.k1mqtt'
|
||||
_write_capture(src, [('lixel/application/report/lio_pose', _pose_payload((0, 0, 0)))] * 2)
|
||||
with pytest.raises(ValueError): export_planning_source(src, tmp_path / 'out.json')
|
||||
|
||||
|
||||
def test_draft_persists_full_route_and_optimistic_revision(tmp_path):
|
||||
service = MissionDrafts(tmp_path, Sources())
|
||||
first = service.save(request(direction='reverse'))
|
||||
assert first['vehicle_id'] is None and first['revision'] == 1
|
||||
assert [p['source_index'] for p in first['route']['points']] == [2, 1, 0]
|
||||
assert first['route']['length_m'] == 10
|
||||
restarted = MissionDrafts(tmp_path, Sources())
|
||||
assert restarted.get(first['id']) == first
|
||||
next = restarted.save(request(id=first['id'], revision=1, name='Changed'))
|
||||
assert next['revision'] == 2
|
||||
with pytest.raises(DraftConflict): service.save(request(id=first['id'], revision=1))
|
||||
assert service.get(first['id']) == next
|
||||
|
||||
|
||||
@pytest.mark.parametrize('start,end', [(2, 2), (3, 1), (-1, 2), (0, 4)])
|
||||
def test_route_rejects_out_of_bounds(start, end):
|
||||
with pytest.raises(ValueError): route_from_source(source_doc(), start, end, 'forward')
|
||||
|
||||
|
||||
def test_check_is_saved_revision_bound_and_never_claims_localization(tmp_path):
|
||||
sources = Sources(); service = MissionDrafts(tmp_path, sources)
|
||||
draft = service.save(request())
|
||||
result = service.check(draft['id'], 1)
|
||||
assert result['localization'] == 'not_run' and result['vehicle_control'] is False
|
||||
assert result['warnings'] and result['source_verified']
|
||||
with service.connect() as db: assert db.execute('SELECT COUNT(*) FROM checks').fetchone()[0] == 1
|
||||
sources.changed = True
|
||||
with pytest.raises(ValueError): service.check(draft['id'], 1)
|
||||
assert service.get(draft['id']) == draft # unavailable evidence never discards the draft
|
||||
|
||||
|
||||
def test_api_forbids_vehicle_authority_and_checks_revision(tmp_path):
|
||||
app = FastAPI(); app.include_router(build_mission_planner_router(MissionDrafts(tmp_path, Sources())))
|
||||
with TestClient(app) as client:
|
||||
body = request().model_dump(mode='json')
|
||||
assert client.post('/api/v1/mission-planner/drafts', json={**body, 'vehicle_id': 'rover'}).status_code == 422
|
||||
draft = client.post('/api/v1/mission-planner/drafts', json=body).json()
|
||||
assert client.post('/api/v1/mission-planner/drafts/'+draft['id']+'/checks', json={'revision': 2}).status_code == 409
|
||||
assert client.get('/api/v1/mission-planner/drafts/'+draft['id']).json() == draft
|
||||
|
||||
|
||||
def test_source_cache_is_bound_to_validated_archive_and_rejects_lab(tmp_path):
|
||||
command = _command(tmp_path / 'source')
|
||||
detail = SimpleNamespace(plugin_id=command.plugin_id, summary=SimpleNamespace(replayable=True, lab=None), as_dict=lambda: {'display_name': 'A'})
|
||||
store = SimpleNamespace(data_dir=tmp_path / 'data', get_session=lambda _: detail, prepare_replay=lambda _: command)
|
||||
count = []
|
||||
def export(source, dest):
|
||||
count.append(1); dest.write_text(json.dumps({'poses': [], 'path_m': 0}))
|
||||
service = PlanningSources(store, {command.plugin_id: export})
|
||||
first = service.get(command.session_id)
|
||||
assert service.get(command.session_id) == first and len(count) == 1
|
||||
assert service.verify(command.session_id, first['generation']) == first
|
||||
command.primary_artifact.path.write_bytes(b'X' * command.primary_artifact.file_byte_length)
|
||||
with pytest.raises(ValueError): service.bound(command.session_id, first['generation'])
|
||||
detail.summary.lab = object()
|
||||
with pytest.raises(ValueError): service.get(command.session_id)
|
||||
@@ -0,0 +1,114 @@
|
||||
import json
|
||||
import threading
|
||||
from types import SimpleNamespace
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
pytest.importorskip('small_gicp')
|
||||
from k1link.missions.registration import register, transform, rigid, path_hint, angle_deg
|
||||
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.registration_runs import RegistrationRuns
|
||||
from test_stream_summary import _write_capture, _pcl_payload, _pose_payload
|
||||
|
||||
|
||||
def geometry():
|
||||
r = np.random.default_rng(41)
|
||||
return np.vstack([r.normal([0,0,0],[4,3,.1],(3000,3)),
|
||||
r.normal([3,2,2],[.1,2,2],(1500,3)),
|
||||
r.normal([-2,-1,2],[2,.1,1],(1500,3))])
|
||||
|
||||
|
||||
def test_recovers_known_rigid_transform_in_source_to_target_convention():
|
||||
p = geometry(); t = np.eye(4); a = .1
|
||||
t[:2,:2] = [[np.cos(a),-np.sin(a)],[np.sin(a),np.cos(a)]]
|
||||
t[:3,3] = [.7,-.4,.2]
|
||||
result = register(p, transform(p, np.linalg.inv(t)), np.eye(4))
|
||||
found = np.array(result['T_reference_query'])
|
||||
assert result['status'] == 'candidate'
|
||||
assert np.linalg.norm(found[:3,3]-t[:3,3]) < .01
|
||||
assert angle_deg(found[:3,:3] @ t[:3,:3].T) < .1
|
||||
assert not result['localization_confirmed'] and not result['vehicle_control']
|
||||
|
||||
|
||||
def test_no_overlap_and_uninformative_plane_are_rejected():
|
||||
p = geometry()
|
||||
assert register(p, p+[100,100,100], np.eye(4))['status'] == 'rejected'
|
||||
p[:,2] = 0
|
||||
result = register(p, p, np.eye(4))
|
||||
assert result['status'] == 'rejected' and result['shape_ratio'] == 0
|
||||
|
||||
|
||||
def test_nonrigid_and_nonfinite_inputs_rejected():
|
||||
with pytest.raises(ValueError): rigid(np.ones((4,4)))
|
||||
p = geometry(); p[0,0] = np.nan
|
||||
with pytest.raises(ValueError): register(p, geometry(), np.eye(4))
|
||||
with pytest.raises(ValueError): register(geometry()[:20], geometry(), np.eye(4))
|
||||
|
||||
|
||||
def test_path_hint_maps_query_entry_and_heading_to_selected_route():
|
||||
t = path_hint([[8,9,1],[8,14,1]], [[-1,-2,0],[4,-2,0]])
|
||||
assert np.allclose(transform(np.array([[-1,-2,0],[4,-2,0]]),t), [[8,9,1],[8,14,1]])
|
||||
with pytest.raises(ValueError): path_hint([[0,0,0],[0,0,0]], [[0,0,0],[4,0,0]])
|
||||
|
||||
|
||||
def test_extractor_uses_only_selected_interval_and_no_second_pose_transform(tmp_path):
|
||||
raw = tmp_path/'mqtt.raw.k1mqtt'
|
||||
_write_capture(raw, [('x/lio_pose', _pose_payload((5,0,0))),
|
||||
('x/lio_pcl', _pcl_payload(scaler=1000, point_count=4)),
|
||||
('x/lio_pose', _pose_payload((8,0,0))),
|
||||
('x/lio_pcl', b'bad-future-cloud')])
|
||||
out = tmp_path/'planning.json'; export_planning_source(raw,out)
|
||||
points, meta = extract_submap(raw,json.loads(out.read_text()),0,1)
|
||||
assert len(points) >= 1 and np.allclose(points[0], [1,-2,.5])
|
||||
assert meta['available_frames'] == 1 and meta['frames'][0]['message_index'] == 1
|
||||
assert meta['message_interval'] == [0,2]
|
||||
|
||||
|
||||
def test_run_rejects_overlapping_self_comparison_and_revision_race(tmp_path):
|
||||
source = {'poses':[{'position':[i,0,0],'distance_m':i} for i in range(31)]}
|
||||
draft = {'id':'a', 'revision':1, 'zone':{'session_id':'A'},
|
||||
'route':{'length_m':30,'start_index':0,'end_index':30}}
|
||||
drafts = SimpleNamespace(database=tmp_path/'drafts.sqlite', get=lambda _:draft,
|
||||
sources=SimpleNamespace(bound=lambda *_:source))
|
||||
runs = RegistrationRuns(drafts)
|
||||
req = {'revision':1,'session_id':'A','generation':'x','start_index':0,'end_index':30}
|
||||
try:
|
||||
with pytest.raises(ValueError, match='не должны пересекаться'): runs.start('a',req)
|
||||
with pytest.raises(ValueError, match='изменён'): runs.start('a',{**req,'revision':2})
|
||||
assert not runs.lock.locked() and not list(runs.root.glob('*/report.json'))
|
||||
finally: runs.close()
|
||||
|
||||
|
||||
def test_run_is_single_owner_persisted_and_bound_to_submitted_revision(tmp_path, monkeypatch):
|
||||
import k1link.missions.registration_runs as module
|
||||
entered, release = threading.Event(), threading.Event()
|
||||
source = {'poses':[{'position':[i,0,0], 'distance_m':i} for i in range(31)]}
|
||||
draft = {'id':'draft', 'revision':1, 'zone':{'session_id':'A','generation':'a'},
|
||||
'route':{'length_m':30,'start_index':0,'end_index':30,
|
||||
'points':[{'position':[i,0,0]} for i in range(31)]}}
|
||||
def submap(*args):
|
||||
entered.set(); assert release.wait(5)
|
||||
return geometry(), {'session_id':args[0]}
|
||||
drafts = SimpleNamespace(database=tmp_path/'db', get=lambda _:draft,
|
||||
sources=SimpleNamespace(bound=lambda *_:source, submap=submap))
|
||||
monkeypatch.setattr(module, 'write_scene', lambda p,*a: p.write_bytes(b'test-rrd'))
|
||||
runs = RegistrationRuns(drafts)
|
||||
req = {'revision':1,'session_id':'B','generation':'b','start_index':0,'end_index':30}
|
||||
first = runs.start('draft',req)
|
||||
try:
|
||||
assert entered.wait(5)
|
||||
with pytest.raises(ValueError, match='ещё выполняется'): runs.start('draft',req)
|
||||
# Real draft reads are decoded snapshots. A later edit does not relabel an existing report.
|
||||
draft = {**draft, 'revision':2}
|
||||
finally:
|
||||
release.set(); runs.close()
|
||||
report = runs.get(first['id'])
|
||||
assert report['state'] == 'ready' and report['revision'] == 1
|
||||
assert report['reference']['session_id'] == 'A' and report['query']['session_id'] == 'B'
|
||||
assert not report['localization_confirmed'] and not report['vehicle_control']
|
||||
restarted = RegistrationRuns(drafts)
|
||||
try:
|
||||
assert restarted.get(first['id']) == report
|
||||
assert restarted.list('draft')[0]['id'] == first['id']
|
||||
finally: restarted.close()
|
||||
@@ -0,0 +1,64 @@
|
||||
"""The archive fault adapter omits receipts without changing surviving clocks."""
|
||||
|
||||
import importlib.util
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
|
||||
def adapter_module():
|
||||
path = Path(__file__).resolve().parents[1] / "scripts/planning_archive_source.py"
|
||||
spec = importlib.util.spec_from_file_location("archive_fault_probe", path)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
spec.loader.exec_module(module)
|
||||
return module
|
||||
|
||||
|
||||
def test_fault_preserves_survivor_identity_payload_and_receipt_offsets(monkeypatch):
|
||||
module = adapter_module()
|
||||
original = [
|
||||
SimpleNamespace(
|
||||
received_monotonic_ns=int((100 + t) * 1e9),
|
||||
received_at_epoch_ns=int((1000 + t) * 1e9),
|
||||
topic="fixture/lio_pcl" if i % 2 else "fixture/lio_pose",
|
||||
sequence=i + 1,
|
||||
payload=bytes([i]),
|
||||
)
|
||||
for i, t in enumerate([0, 1, 2, 3, 3.9, 4, 5])
|
||||
]
|
||||
monkeypatch.setattr(module, "iter_replay_messages", lambda _: iter(original))
|
||||
clock = [int(500e9)]
|
||||
|
||||
def wait(delay):
|
||||
clock[0] += int(delay * 1e9)
|
||||
return False
|
||||
|
||||
monkeypatch.setattr(module, "time", SimpleNamespace(monotonic_ns=lambda: clock[0]))
|
||||
source = module.ReceiptQueueArchiveSource(Path("unused"), "B", 10, drop_interval_s=(2, 4))
|
||||
source.stop = SimpleNamespace(wait=wait)
|
||||
source.started = clock[0]
|
||||
published = []
|
||||
source.ingress = SimpleNamespace(
|
||||
publish=lambda **kw: published.append(kw) or True,
|
||||
end_session=lambda session: None,
|
||||
)
|
||||
source.publish()
|
||||
kept = [original[i] for i in [0, 1, 5, 6]]
|
||||
assert source.error is None
|
||||
assert [x["sequence"] for x in source.dropped_receipts] == [3, 4, 5]
|
||||
for out, entry in zip(published, kept, strict=True):
|
||||
assert out["source_sequence"] == entry.sequence
|
||||
assert out["payload"] is entry.payload
|
||||
assert out["captured_at_epoch_ns"] == entry.received_at_epoch_ns
|
||||
assert out["received_monotonic_ns"] == source.started + entry.received_monotonic_ns - int(
|
||||
100e9
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("interval", [(2, 2), (4, 2), (0, 2), (1, 11), (1, float("nan"))])
|
||||
def test_invalid_fault_interval_is_rejected(interval):
|
||||
with pytest.raises(ValueError, match="fault interval"):
|
||||
adapter_module().ReceiptQueueArchiveSource(
|
||||
Path("unused"), "B", 10, drop_interval_s=interval
|
||||
)
|
||||
@@ -0,0 +1,98 @@
|
||||
"""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
|
||||
@@ -0,0 +1,247 @@
|
||||
"""Small failure fixtures; no device, network, live singleton or native search."""
|
||||
|
||||
from concurrent.futures import Future
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from test_planning_live import event, fixture_service, until
|
||||
from test_stream_summary import _pose_payload
|
||||
|
||||
from k1link.compute.live_perception import LivePerceptionIngress
|
||||
from k1link.device_plugins.xgrids_k1.planning_live import K1PlanningLiveSource
|
||||
from k1link.missions.stationary_live import run_stationary_live
|
||||
|
||||
|
||||
@pytest.mark.parametrize("failure", ["persist", "thread-constructor", "thread-start", "open"])
|
||||
def test_failed_start_releases_resources_and_allows_retry(tmp_path, monkeypatch, failure):
|
||||
import k1link.missions.live_tests as module
|
||||
|
||||
service, source, compute_lock, _ = fixture_service(tmp_path, monkeypatch)
|
||||
|
||||
def fail(*args, **kwargs):
|
||||
raise OSError("injected startup failure")
|
||||
|
||||
with monkeypatch.context() as patch:
|
||||
if failure == "persist":
|
||||
patch.setattr(service, "persist", fail)
|
||||
elif failure == "thread-constructor":
|
||||
patch.setattr(module.threading, "Thread", fail)
|
||||
elif failure == "thread-start":
|
||||
patch.setattr(module.threading.Thread, "start", fail)
|
||||
else:
|
||||
patch.setattr(source, "open", fail)
|
||||
try:
|
||||
with pytest.raises(OSError, match="injected startup failure"):
|
||||
service.start("draft", 1)
|
||||
assert not compute_lock.locked()
|
||||
assert source.owner is None
|
||||
assert service.thread is None
|
||||
if failure != "open":
|
||||
assert service.get()["state"] == "error"
|
||||
assert service.get()["tracking_state"] == "lost"
|
||||
assert service.source is None
|
||||
finally:
|
||||
# Also isolate the pre-fix red run, whose leaked resources are fixtures.
|
||||
if compute_lock.locked():
|
||||
compute_lock.release()
|
||||
source.owner = None
|
||||
|
||||
run = service.start("draft", 1)
|
||||
try:
|
||||
until(lambda: service.get()["state"] == "waiting")
|
||||
service.stop(run["id"])
|
||||
finally:
|
||||
service.close()
|
||||
assert not compute_lock.locked() and source.owner is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize("failure", ["executor", "report", "close"])
|
||||
def test_worker_cleanup_keeps_failure_terminal_and_unlocks(tmp_path, monkeypatch, failure):
|
||||
import k1link.missions.live_tests as module
|
||||
|
||||
service, source, compute_lock, _ = fixture_service(tmp_path, monkeypatch)
|
||||
original_persist = service.persist
|
||||
original_close = source.close
|
||||
|
||||
def fail(*args, **kwargs):
|
||||
raise OSError("injected worker failure")
|
||||
|
||||
if failure == "executor":
|
||||
monkeypatch.setattr(module, "ThreadPoolExecutor", fail)
|
||||
elif failure == "report":
|
||||
|
||||
def persist():
|
||||
if service.run["state"] != "preparing":
|
||||
fail()
|
||||
original_persist()
|
||||
|
||||
monkeypatch.setattr(service, "persist", persist)
|
||||
else:
|
||||
|
||||
def close(owner):
|
||||
original_close(owner)
|
||||
fail()
|
||||
|
||||
monkeypatch.setattr(source, "close", close)
|
||||
|
||||
run = service.start("draft", 1)
|
||||
try:
|
||||
if failure == "close":
|
||||
until(lambda: service.get()["state"] == "waiting")
|
||||
service.stop(run["id"])
|
||||
until(lambda: not service.thread.is_alive())
|
||||
assert not compute_lock.locked()
|
||||
assert source.owner is None
|
||||
assert service.get()["state"] == "error"
|
||||
assert service.accepted_sample is None and service.last_result_ns == 0
|
||||
finally:
|
||||
service.close()
|
||||
if compute_lock.locked():
|
||||
compute_lock.release()
|
||||
source.owner = None
|
||||
|
||||
|
||||
def test_partial_report_write_is_recovered_as_terminal_before_retry(tmp_path, monkeypatch):
|
||||
import k1link.missions.live_tests as module
|
||||
|
||||
service, source, compute_lock, _ = fixture_service(tmp_path, monkeypatch)
|
||||
replace_file = module.os.replace
|
||||
failures = []
|
||||
|
||||
def fail_first_active_replace(src, dst):
|
||||
if dst == service.root / "active.json" and not failures:
|
||||
failures.append(dst)
|
||||
raise OSError("injected partial report commit")
|
||||
replace_file(src, dst)
|
||||
|
||||
monkeypatch.setattr(module.os, "replace", fail_first_active_replace)
|
||||
with pytest.raises(OSError, match="partial report commit"):
|
||||
service.start("draft", 1)
|
||||
assert not compute_lock.locked() and source.owner is None
|
||||
assert service.thread is None
|
||||
restored = module.PlanningLiveTests(service.drafts, service.sources, compute_lock)
|
||||
assert restored.get()["state"] == "error"
|
||||
assert restored.get()["tracking_state"] == "lost"
|
||||
assert restored.get()["scene_available"] is False
|
||||
assert (
|
||||
"partial report commit"
|
||||
in (service.directory(service.run["id"]) / "failure.txt").read_text()
|
||||
)
|
||||
run = service.start("draft", 1)
|
||||
try:
|
||||
until(lambda: service.get()["state"] == "waiting")
|
||||
service.stop(run["id"])
|
||||
finally:
|
||||
service.close()
|
||||
assert not compute_lock.locked() and source.owner is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize("modality", ["camera-init", "camera-frame"])
|
||||
def test_auxiliary_receipt_is_not_an_empty_queue(modality):
|
||||
ingress = LivePerceptionIngress()
|
||||
source = K1PlanningLiveSource(ingress)
|
||||
source.open("fixture")
|
||||
ingress.begin_session("B")
|
||||
try:
|
||||
assert source.take("fixture").kind == "session-start"
|
||||
ingress.publish(
|
||||
modality=modality,
|
||||
source_id="camera",
|
||||
source_sequence=1,
|
||||
captured_at_epoch_ns=10,
|
||||
received_monotonic_ns=20,
|
||||
payload=b"fixture",
|
||||
)
|
||||
ingress.publish(
|
||||
modality="pose",
|
||||
source_id="x/lio_pose",
|
||||
source_sequence=2,
|
||||
captured_at_epoch_ns=11,
|
||||
received_monotonic_ns=21,
|
||||
payload=_pose_payload((0, 0, 0)),
|
||||
)
|
||||
auxiliary = source.take("fixture")
|
||||
assert auxiliary is not None and auxiliary.kind == "ignored"
|
||||
assert auxiliary.session_id == "B" and auxiliary.generation == 1
|
||||
assert auxiliary.monotonic_ns == 20 and auxiliary.epoch_ns == 10
|
||||
assert auxiliary.points is None and auxiliary.position is None
|
||||
assert ingress.snapshot()["queues"]["pose"]["depth"] == 1
|
||||
pose = source.take("fixture")
|
||||
assert pose.kind == "pose" and pose.sequence > auxiliary.sequence
|
||||
finally:
|
||||
source.close("fixture")
|
||||
ingress.close()
|
||||
|
||||
|
||||
def test_delayed_auxiliary_receipt_does_not_freeze_before_queued_prefix(tmp_path, monkeypatch):
|
||||
service, _, _, _ = fixture_service(tmp_path, monkeypatch)
|
||||
service.reference_path = np.array([[0.0, 0, 0], [20.0, 0, 0]])
|
||||
points = np.random.default_rng(501).uniform([-2, -2, -1], [2, 2, 2], (700, 3))
|
||||
service.reference = points
|
||||
service.run = dict(
|
||||
baseline_generation=0,
|
||||
baseline_session_id="old",
|
||||
draft=dict(zone=dict(session_id="A")),
|
||||
maximum_distance_m=20,
|
||||
)
|
||||
updates = []
|
||||
service.update = lambda **values: updates.append(values)
|
||||
service.directory = lambda _: tmp_path
|
||||
clock_value = [100.0]
|
||||
pending = []
|
||||
for t in range(100, 111):
|
||||
pending.extend([event("pose", t=t), event("points", t=t + 0.001, points=points)])
|
||||
if t == 108:
|
||||
# Use the production adapter, not an invented None/ignored value.
|
||||
ingress = LivePerceptionIngress()
|
||||
adapter = K1PlanningLiveSource(ingress)
|
||||
adapter.open("fixture")
|
||||
ingress.begin_session("B")
|
||||
adapter.take("fixture")
|
||||
ingress.publish(
|
||||
modality="camera-frame",
|
||||
source_id="camera",
|
||||
source_sequence=1,
|
||||
captured_at_epoch_ns=1,
|
||||
received_monotonic_ns=108_100_000_000,
|
||||
payload=b"fixture",
|
||||
)
|
||||
pending.append(adapter.take("fixture"))
|
||||
adapter.close("fixture")
|
||||
ingress.close()
|
||||
# Preserve the same session/generation/order across modality boundaries.
|
||||
from dataclasses import replace
|
||||
|
||||
pending = [
|
||||
(108.1, None)
|
||||
if item is None
|
||||
else (item.monotonic_ns / 1e9, replace(item, generation=1, sequence=i + 1))
|
||||
for i, item in enumerate(pending)
|
||||
]
|
||||
|
||||
class Source:
|
||||
def snapshot(self):
|
||||
return dict(active=True, session_id="B", session_generation=1)
|
||||
|
||||
def take(self, owner):
|
||||
stamp, item = pending.pop(0)
|
||||
clock_value[0] = stamp + 2
|
||||
return item
|
||||
|
||||
searches = []
|
||||
|
||||
class Executor:
|
||||
def submit(self, *args, **kwargs):
|
||||
searches.append((args, kwargs))
|
||||
service.cancel.set()
|
||||
future = Future()
|
||||
future.set_result({})
|
||||
return future
|
||||
|
||||
clock = SimpleNamespace(
|
||||
monotonic=lambda: clock_value[0], monotonic_ns=lambda: int(clock_value[0] * 1e9)
|
||||
)
|
||||
run_stationary_live(service, Source(), "fixture", Executor(), clock, None, None)
|
||||
assert len(searches) == 1 and not pending
|
||||
assert any(update.get("planning_phase") == "searching" for update in updates)
|
||||
@@ -0,0 +1,312 @@
|
||||
"""Bounded functional tests of display/registration separation and delta fences."""
|
||||
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
from fastapi import FastAPI
|
||||
from fastapi.testclient import TestClient
|
||||
from test_planning_live import event, fixture_service
|
||||
from test_planning_stabilization import fit, sample
|
||||
|
||||
from k1link.missions.live_buffer import LiveCloudBuffer
|
||||
from k1link.missions.live_display_buffer import LiveDisplayBuffer
|
||||
from k1link.missions.live_scene_delta import decode_cursor, scene_delta
|
||||
from k1link.web.planning_live_api import build_planning_live_router
|
||||
|
||||
|
||||
def ingest(display, numeric, t, sequence, points=None):
|
||||
pose = event("pose", t=t, sequence=sequence, p=(t / 100, 0, 0))
|
||||
cloud = event(
|
||||
"points",
|
||||
t=t + 0.001,
|
||||
sequence=sequence + 1,
|
||||
points=np.array([[t / 100, 1, 0]]) if points is None else points,
|
||||
)
|
||||
for e in (pose, cloud):
|
||||
numeric.ingest(e)
|
||||
display.ingest(e, numeric)
|
||||
return cloud
|
||||
|
||||
|
||||
def test_display_preserves_high_points_independently_from_tracking_filter():
|
||||
numeric, display = LiveCloudBuffer([[0,0,0],[10,0,0]]), LiveDisplayBuffer()
|
||||
points = np.array([[1,1,1],[1,0,40],[1,0,79.9],[1,0,80.1]])
|
||||
ingest(display, numeric, 100, 1, points)
|
||||
np.testing.assert_allclose(display.snapshot()["points"], points[:3])
|
||||
np.testing.assert_allclose(numeric.snapshot()["points"], points[:1])
|
||||
|
||||
|
||||
def test_native_packets_are_not_numerical_sampling_and_snapshots_are_immutable():
|
||||
numeric, display = LiveCloudBuffer([[0, 0, 0], [10, 0, 0]]), LiveDisplayBuffer()
|
||||
ingest(display, numeric, 100, 1)
|
||||
first = display.snapshot()
|
||||
for i in range(1, 10):
|
||||
ingest(display, numeric, 100 + i / 10, i * 2 + 1)
|
||||
assert display.frames == 10
|
||||
assert len(numeric.chunks) == 2 # registration's 500 ms policy is unchanged
|
||||
assert display.snapshot()["sequence"] == 20
|
||||
assert first["sequence"] == 2 and first["cloud_revision"] == 1
|
||||
np.testing.assert_equal(first["points"], [[1, 1, 0]])
|
||||
assert len(display.chunks) == 1 # The current half-second receipt stays live.
|
||||
np.testing.assert_allclose(display.snapshot()["current_points"], [[1.009, 1, 0]])
|
||||
|
||||
|
||||
def test_budgets_eviction_tombstones_old_receipts_and_segment_reset():
|
||||
numeric, display = LiveCloudBuffer([[0, 0, 0], [10, 0, 0]]), LiveDisplayBuffer()
|
||||
points = np.array([[x * 0.3, y * 0.3, 0] for x in range(35) for y in range(35)])
|
||||
for i in range(60):
|
||||
ingest(display, numeric, 100 + i * 0.5, 2 * i, points)
|
||||
assert len(display.chunks) <= 40
|
||||
assert sum(map(len, display.chunks.values())) <= 40_000
|
||||
before = display.frames
|
||||
display.ingest(event("points", t=100, points=points), numeric)
|
||||
assert display.frames == before
|
||||
assert any(p is None and revision > 0 for _, revision, p in display.snapshot()["chunks"])
|
||||
ingest(display, numeric, 133, 200, points)
|
||||
assert numeric.segment == 1 and len(display.chunks) == 0
|
||||
assert len(display.snapshot()["current_points"]) == len(points)
|
||||
|
||||
|
||||
def test_delta_sends_only_changed_chunks_pose_and_transform(monkeypatch):
|
||||
import k1link.missions.live_scene_delta as module
|
||||
|
||||
logs = []
|
||||
|
||||
class Recording:
|
||||
def __init__(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
def binary_stream(self):
|
||||
return SimpleNamespace(read=lambda: b"RRF2")
|
||||
|
||||
def log(self, name, value, **kwargs):
|
||||
logs.append((name, value, kwargs))
|
||||
|
||||
def set_time(self, *args, **kwargs):
|
||||
logs.append(("time", args, kwargs))
|
||||
|
||||
def flush(self):
|
||||
pass
|
||||
|
||||
def disconnect(self):
|
||||
pass
|
||||
|
||||
monkeypatch.setattr(module.rr, "RecordingStream", Recording)
|
||||
monkeypatch.setattr(module, "log_base", lambda *args: None)
|
||||
monkeypatch.setattr(module, "log_view", lambda *args: None)
|
||||
monkeypatch.setattr(module.rr, "Points3D", lambda points, **kwargs: np.asarray(points))
|
||||
monkeypatch.setattr(module.rr, "Transform3D", lambda **kwargs: kwargs)
|
||||
numeric, display = LiveCloudBuffer([[0, 0, 0], [10, 0, 0]]), LiveDisplayBuffer()
|
||||
ingest(display, numeric, 100, 1)
|
||||
|
||||
def render(cursor="", result=None, live=True, evidence=None, options=None):
|
||||
return scene_delta(
|
||||
"run",
|
||||
display.epoch,
|
||||
np.zeros((3, 3)),
|
||||
np.zeros((2, 3)),
|
||||
display.snapshot(),
|
||||
result or fit(),
|
||||
evidence,
|
||||
live,
|
||||
cursor=cursor,
|
||||
options=options,
|
||||
)
|
||||
|
||||
_, cursor = render()
|
||||
assert all(
|
||||
kwargs.get("static")
|
||||
for name, _, kwargs in logs
|
||||
if name == "world/query" or "/cloud/" in name
|
||||
)
|
||||
np.testing.assert_equal(dict((n, v) for n, v, _ in logs)["world/query/live"], [[1, 1, 0]])
|
||||
logs.clear()
|
||||
render(options={"ceiling_m": -0.1})
|
||||
assert len(dict((n, v) for n, v, _ in logs)["world/query/live"]) == 0
|
||||
logs.clear()
|
||||
assert render(cursor)[0] == b""
|
||||
assert logs == []
|
||||
ingest(display, numeric, 100.1, 3)
|
||||
_, cursor = render(cursor)
|
||||
assert [n for n, _, _ in logs if "/cloud/" in n] == []
|
||||
assert "world/query/live" in [n for n, _, _ in logs]
|
||||
assert "world/query" not in [n for n, _, _ in logs]
|
||||
logs.clear()
|
||||
adjusted = fit()
|
||||
adjusted["T_reference_query"][0][3] = 9
|
||||
_, cursor = render(cursor, result=adjusted)
|
||||
assert [n for n, _, _ in logs] == ["world/query", "time", "world/query/live"]
|
||||
assert logs[0][1]["translation"][0] == 9
|
||||
logs.clear()
|
||||
_, cursor = render(cursor, result=adjusted, evidence=(sample(), fit()))
|
||||
assert "world/validated_query" in [n for n, _, _ in logs]
|
||||
logs.clear()
|
||||
render(cursor, result=adjusted, live=False)
|
||||
assert "world/validated_query" in [n for n, _, _ in logs]
|
||||
assert not any("/cloud/" in n for n, _, _ in logs)
|
||||
|
||||
|
||||
def test_presentation_changes_and_geometry_repairs_never_reset_camera(monkeypatch):
|
||||
import k1link.missions.live_scene_delta as module
|
||||
|
||||
views = []
|
||||
monkeypatch.setattr(module, "log_view", lambda *args: views.append(args[-1].copy()))
|
||||
reference = np.array([[0., 0., -1.], [20., 1., 40.]])
|
||||
cursor = ""
|
||||
|
||||
def render(options=None, **kwargs):
|
||||
nonlocal cursor
|
||||
payload, cursor = scene_delta(
|
||||
"camera-test", kwargs.pop("epoch", "first"), reference, reference,
|
||||
None, None, None, False, cursor=cursor, options=options, **kwargs,
|
||||
)
|
||||
return payload
|
||||
|
||||
assert render().startswith(b"RRF2")
|
||||
assert len(views) == 1
|
||||
for options in ({"ceiling_m": 3}, {"reference": False}, {"query": False},
|
||||
{"trajectory": False}, {"point_size": 4}, {"grid": False}, {}):
|
||||
assert render(options).startswith(b"RRF2")
|
||||
assert len(views) == 1
|
||||
assert render(base=True).startswith(b"RRF2")
|
||||
assert render(epoch="reconnected").startswith(b"RRF2")
|
||||
assert len(views) == 1
|
||||
render({"mode": "top"})
|
||||
render({"mode": "top", "reset": 1})
|
||||
assert len(views) == 3
|
||||
assert render({"mode": "top", "reset": 1}) == b""
|
||||
assert len(views) == 3
|
||||
|
||||
|
||||
def test_grid_is_display_geometry_and_never_a_camera_command():
|
||||
from k1link.missions.live_scene import grid_lines, log_base
|
||||
|
||||
reference = np.array([[0., 0., -1.], [20., 1., 40.]])
|
||||
logs = {}
|
||||
recording = SimpleNamespace(log=lambda path, value, **kw: logs.update({path: value}))
|
||||
log_base(recording, reference, reference, {"ceiling_m": 3})
|
||||
assert "world/grid" in logs
|
||||
assert len(logs["world/reference"].positions.as_arrow_array()) == 1
|
||||
assert len(grid_lines(reference)) > 0
|
||||
log_base(recording, reference, reference, {"grid": False})
|
||||
assert len(logs["world/grid"].strips.as_arrow_array()) == 0
|
||||
|
||||
|
||||
def test_service_freezes_after_loss_fences_identity_and_expires_without_packets(
|
||||
tmp_path, monkeypatch
|
||||
):
|
||||
import k1link.missions.live_tests as module
|
||||
|
||||
service, source, _, _ = fixture_service(tmp_path, monkeypatch)
|
||||
run_id = "cedf3261-a703-453d-a1c8-aac71c6ce5e3"
|
||||
service.run = dict(
|
||||
id=run_id,
|
||||
state="running",
|
||||
query_session_id="B",
|
||||
query_generation=2,
|
||||
vehicle_control=False,
|
||||
)
|
||||
service.source = source
|
||||
source.state.update(active=True, session_id="B", session_generation=2)
|
||||
service.reference, service.reference_path = np.zeros((3, 3)), np.zeros((2, 3))
|
||||
summary = service.get()
|
||||
assert summary["scene_height_min_m"] == 0.0
|
||||
assert summary["scene_height_max_m"] == 80.0
|
||||
clock = [int(100.1e9)]
|
||||
monkeypatch.setattr(module, "time", SimpleNamespace(monotonic_ns=lambda: clock[0]))
|
||||
monkeypatch.setattr(service, "persist", lambda: None)
|
||||
numeric = LiveCloudBuffer(service.reference_path)
|
||||
for e in (event("pose", t=100), event("points", t=100.01, points=np.ones((2, 3)))):
|
||||
numeric.ingest(e)
|
||||
service.observe_display(e, numeric, clock[0])
|
||||
assert service.presentation.sample is None # no hint fallback
|
||||
service.commit_result(
|
||||
fit(),
|
||||
sample(),
|
||||
{"accepted": True},
|
||||
"tracking",
|
||||
phase="tracking",
|
||||
message="tracking",
|
||||
tracking_established=True,
|
||||
)
|
||||
app = FastAPI()
|
||||
app.include_router(build_planning_live_router(service))
|
||||
with TestClient(app) as client:
|
||||
url = f"/api/v1/mission-planner/live-tests/{run_id}/scene-delta.rrd"
|
||||
first = client.get(url)
|
||||
assert first.status_code == 200 and first.content.startswith(b"RRF2")
|
||||
cursor = first.headers["X-Planning-Scene-Cursor"]
|
||||
assert first.headers["X-Planning-Cloud-Revision"] == "1"
|
||||
assert first.headers["X-Planning-Cloud-Sequence"] == "1"
|
||||
assert first.headers["X-Planning-Height-Min"] == "0.0"
|
||||
assert first.headers["X-Planning-Height-Max"] == "80.0"
|
||||
observation = {
|
||||
"schema_version": "missioncore.planning-browser-presentation/v1",
|
||||
"samples": [
|
||||
{
|
||||
"cloud_revision": 1,
|
||||
"cloud_sequence": 1,
|
||||
"display_epoch": service.display.epoch,
|
||||
"request_ms": 40.0,
|
||||
"rerun_admission_ms": 2.0,
|
||||
"first_animation_frame_ms": 12.0,
|
||||
"second_animation_frame_ms": 28.0,
|
||||
"frame_timeout": False,
|
||||
"source_to_second_animation_frame_upper_bound_ms": 268.0,
|
||||
}
|
||||
],
|
||||
}
|
||||
report_url = (
|
||||
f"/api/v1/mission-planner/live-tests/{run_id}/presentation-observations"
|
||||
)
|
||||
assert client.post(report_url, json=observation).status_code == 204
|
||||
presentation = service.get()["browser_presentation"]
|
||||
assert presentation["reported_sample_count"] == 1
|
||||
assert presentation["second_animation_frame_ms"]["p50"] == 28.0
|
||||
assert service.get()["vehicle_control"] is False
|
||||
observation["samples"][0]["cloud_revision"] = 2
|
||||
assert client.post(report_url, json=observation).status_code == 204
|
||||
assert service.get()["browser_presentation"]["rejected_sample_count"] == 1
|
||||
assert client.get(url, params={"cursor": cursor}).status_code == 204
|
||||
assert client.get(url, params={"cursor": "corrupt"}).status_code == 200
|
||||
assert client.get(url, params={"cursor": "a" * 2049}).status_code == 422
|
||||
clock[0] = int(102.2e9)
|
||||
expired = client.get(url, params={"cursor": cursor})
|
||||
assert expired.status_code == 200
|
||||
assert decode_cursor(expired.headers["X-Planning-Scene-Cursor"])["evidence"] is None
|
||||
frozen = service.presentation.sample
|
||||
service.accepted_sample = None
|
||||
fresh = event("points", t=102.3, points=np.ones((2, 3)))
|
||||
service.observe_display(replace(fresh, session_id="other"), numeric, int(102.3e9))
|
||||
assert service.display.frames == 1
|
||||
numeric.ingest(event("pose", t=102.2))
|
||||
service.observe_display(event("pose", t=102.2), numeric, int(102.2e9))
|
||||
service.observe_display(fresh, numeric, int(102.3e9))
|
||||
assert service.presentation.sample is frozen
|
||||
source.state["session_generation"] = 3
|
||||
assert not service._view_live(int(102.3e9))
|
||||
|
||||
|
||||
def test_reinitialize_endpoint_discards_only_a_failed_initialization(tmp_path, monkeypatch):
|
||||
service, _, _, _ = fixture_service(tmp_path, monkeypatch)
|
||||
run_id = "cedf3261-a703-453d-a1c8-aac71c6ce5e4"
|
||||
service.run = dict(
|
||||
id=run_id,
|
||||
state="running",
|
||||
planning_phase="lost",
|
||||
tracking_established=False,
|
||||
initialization_attempt=1,
|
||||
reinitialization_count=0,
|
||||
)
|
||||
service.persist = lambda: None
|
||||
app = FastAPI()
|
||||
app.include_router(build_planning_live_router(service))
|
||||
with TestClient(app) as client:
|
||||
response = client.post(f"/api/v1/mission-planner/live-tests/{run_id}/reinitialize")
|
||||
assert response.status_code == 200
|
||||
body = response.json()
|
||||
assert body["state"] == "running"
|
||||
assert body["planning_phase"] == "waiting-cloud"
|
||||
assert body["initialization_attempt"] == 2
|
||||
assert body["reinitialization_count"] == 1
|
||||
@@ -0,0 +1,441 @@
|
||||
"""Bounded functional fixtures; no device, socket, capture or load generation."""
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
from queue import Queue, Empty
|
||||
from types import SimpleNamespace
|
||||
import numpy as np
|
||||
import pytest
|
||||
from k1link.sessions.live_planning import PlanningLiveEvent
|
||||
from k1link.missions.live_buffer import LiveCloudBuffer
|
||||
from k1link.missions.live_tests import PlanningLiveTests
|
||||
from k1link.missions.registration_colors import query_colors
|
||||
from k1link.missions.live_scene import scene_bytes
|
||||
from k1link.compute.live_perception import LivePerceptionIngress
|
||||
from k1link.device_plugins.xgrids_k1.planning_live import K1PlanningLiveSource
|
||||
from test_stream_summary import _pose_payload, _pcl_payload
|
||||
|
||||
|
||||
def event(kind, *, t=1, p=(0,0,0), points=None, sequence=1):
|
||||
return PlanningLiveEvent('B',2,sequence,int(t*1e9),int(t*1e9),kind,points,p)
|
||||
|
||||
|
||||
def test_causal_pose_and_bounded_point_window():
|
||||
b=LiveCloudBuffer([[10,10,0],[14,10,0]])
|
||||
cloud=np.array([[1,1,1],[1.01,1,1],[99,1,1],[1,1,20]])
|
||||
b.ingest(event('points',points=cloud));assert len(b.snapshot()['points'])==0
|
||||
b.ingest(event('pose',t=2))
|
||||
b.ingest(event('points',t=1.9,points=cloud));assert len(b.snapshot()['points'])==0
|
||||
b.ingest(event('points',t=2.6,points=cloud));assert len(b.snapshot()['points'])==0
|
||||
b.ingest(event('points',t=2.1,points=cloud));assert len(b.snapshot()['points'])==1
|
||||
# No double pose transform: query remains in its native local K1 frame.
|
||||
assert np.allclose(b.snapshot()['points'][0],[1,1,1])
|
||||
for i in range(1,80):
|
||||
b.ingest(event('pose',t=3+i,p=(i*.05,0,0)))
|
||||
b.ingest(event('points',t=3.1+i,points=cloud,sequence=i))
|
||||
assert len(b.chunks)==40 and len(b.events)==40
|
||||
assert len(b.snapshot()['points'])<=40000
|
||||
with pytest.raises(ValueError,match='Разрыв координат'):b.ingest(event('pose',t=200,p=(100,0,0)))
|
||||
|
||||
|
||||
def test_route_initialization_can_request_an_explicit_wider_k1_scene():
|
||||
b=LiveCloudBuffer([[0,0,0],[10,0,0]],point_radius_m=80)
|
||||
b.ingest(event('pose',t=1,p=(0,0,0)))
|
||||
b.ingest(event('points',t=1.1,points=np.array([[79.9,0,1],[80.1,0,1]])))
|
||||
sample=b.snapshot()
|
||||
assert sample['point_radius_m']==80
|
||||
assert len(sample['points'])==1
|
||||
|
||||
|
||||
def test_only_accepted_correspondences_are_green():
|
||||
p=np.array([[0,0,0],[0,0,1],[0,0,2]])
|
||||
green=np.array([154,235,75])
|
||||
assert not (query_colors(p)==green).all(axis=1).any()
|
||||
assert not (query_colors(p,{'status':'rejected','matched_query_indices':[0,1,2]})==green).all(axis=1).any()
|
||||
c=query_colors(p,{'status':'candidate','matched_query_indices':[1]})
|
||||
assert (c==green).all(axis=1).tolist()==[False,True,False]
|
||||
|
||||
|
||||
def test_plugin_adapter_reads_existing_committed_ingress_only():
|
||||
ingress=LivePerceptionIngress();adapter=K1PlanningLiveSource(ingress)
|
||||
adapter.open('test');ingress.begin_session('B')
|
||||
assert adapter.take('test').kind=='session-start'
|
||||
ingress.publish(modality='pose',source_id='x/lio_pose',source_sequence=3,
|
||||
captured_at_epoch_ns=4,received_monotonic_ns=5,payload=_pose_payload((5,0,0)))
|
||||
p=adapter.take('test');assert p.position==(5.,0.,0.) and p.generation==1
|
||||
ingress.publish(modality='lidar',source_id='x/lio_pcl',source_sequence=4,
|
||||
captured_at_epoch_ns=5,received_monotonic_ns=6,payload=_pcl_payload(scaler=1000,point_count=4))
|
||||
frame=adapter.take('test');assert np.allclose(frame.points[0],[1,-2,.5])
|
||||
with pytest.raises(RuntimeError):adapter.open('other-profile')
|
||||
adapter.close('test');assert not ingress.snapshot()['consumer_connected']
|
||||
ingress.close()
|
||||
|
||||
|
||||
class Source:
|
||||
def __init__(self):self.state=dict(active=False,session_generation=1,session_id='old');self.queue=Queue();self.owner=None
|
||||
def snapshot(self):return dict(self.state)
|
||||
def open(self,id):
|
||||
if self.owner:raise RuntimeError('busy')
|
||||
self.owner=id
|
||||
def close(self,id):assert self.owner==id;self.owner=None
|
||||
def take(self,id):
|
||||
try:return self.queue.get(timeout=.02)
|
||||
except Empty:return None
|
||||
|
||||
|
||||
def fixture_service(tmp_path,monkeypatch):
|
||||
import k1link.missions.live_tests as module
|
||||
draft=dict(id='draft',name='Test route',revision=1,zone=dict(session_id='A',generation='a'),
|
||||
route=dict(length_m=20,start_index=0,end_index=20,points=[dict(position=[i,0,0]) for i in range(21)]))
|
||||
r=np.random.default_rng(19);points=r.normal(size=(1000,3))
|
||||
sources=SimpleNamespace(store=SimpleNamespace(get_session=lambda _:SimpleNamespace(plugin_id='test-plugin')),
|
||||
reference_map=lambda *args,**kwargs:(points,dict(session_id='A',source_digests={'raw':'a'*64})))
|
||||
drafts=SimpleNamespace(database=tmp_path/'db',sources=sources,get=lambda _:json.loads(json.dumps(draft)))
|
||||
source=Source();lock=threading.Lock();service=PlanningLiveTests(drafts,{'test-plugin':source},lock)
|
||||
def calculate(directory,ref,query,hint):
|
||||
# Real numeric regression is tested separately; this fixture verifies orchestration.
|
||||
return dict(status='candidate',T_reference_query=hint.tolist(),matched_query_indices=[0],
|
||||
overlap=.9,inlier_rmse_m=.1,reasons=[])
|
||||
monkeypatch.setattr(module,'run_registration',calculate)
|
||||
from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
|
||||
monkeypatch.setattr(module,'run_route_relocalization',
|
||||
lambda directory,ref,path,query,anchor:dict(
|
||||
status='candidate',T_reference_query=np.eye(4).tolist(),
|
||||
matched_query_indices=[0],overlap=.9,inlier_rmse_m=.1,reasons=[],
|
||||
initialization=dict(complete=True,scope='selected-route',
|
||||
policy=ROUTE_RELOCALIZATION_POLICY,expected_attempts=1,attempts=[{}])))
|
||||
return service,source,lock,draft
|
||||
|
||||
|
||||
def until(fn):
|
||||
deadline=time.monotonic()+5
|
||||
while time.monotonic()<deadline:
|
||||
if fn():return
|
||||
time.sleep(.01)
|
||||
raise AssertionError('condition did not become true')
|
||||
|
||||
|
||||
def test_profile_requires_new_session_free_lease_and_frozen_revision(tmp_path,monkeypatch):
|
||||
service,source,lock,draft=fixture_service(tmp_path,monkeypatch)
|
||||
with pytest.raises(ValueError,match='изменён'):service.start('draft',2)
|
||||
source.state['active']=True
|
||||
with pytest.raises(ValueError,match='завершите'):service.start('draft',1)
|
||||
source.state['active']=False;source.owner='AI'
|
||||
with pytest.raises(ValueError,match='другим'):service.start('draft',1)
|
||||
assert not lock.locked()
|
||||
source.owner=None
|
||||
run=service.start('draft',1)
|
||||
try:
|
||||
until(lambda:service.get()['state']=='waiting')
|
||||
assert run['profile']=='planning' and lock.locked()
|
||||
draft['revision']=2
|
||||
assert service.get()['draft']['revision']==1
|
||||
with pytest.raises(ValueError,match='ещё выполняется'):service.start('draft',1)
|
||||
# Old queued acquisition cannot be rebound to reference A or mistaken for B.
|
||||
source.queue.put(PlanningLiveEvent('old',1,1,time.monotonic_ns(),time.time_ns(),'pose',position=[0,0,0]))
|
||||
time.sleep(.05);assert service.get()['query_session_id'] is None
|
||||
source.state.update(active=True,session_id='B',session_generation=2)
|
||||
now=time.monotonic()
|
||||
source.queue.put(event('pose',t=now,sequence=10))
|
||||
source.queue.put(event('points',t=now+.001,sequence=11,
|
||||
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))))
|
||||
until(lambda:service.get().get('planning_phase')=='collecting')
|
||||
assert service.get()['query_session_id']=='B' and service.get()['state']=='running'
|
||||
assert service.get()['result'] is None
|
||||
assert service.scene(run['id'],True).startswith(b'RRF2')
|
||||
source.state.update(session_id='C',session_generation=3)
|
||||
until(lambda:service.get()['state']=='error')
|
||||
finally:service.close()
|
||||
assert not lock.locked() and source.owner is None
|
||||
restored=PlanningLiveTests(service.drafts,{'test-plugin':source},lock)
|
||||
assert restored.get()['query_session_id']=='B' and restored.get()['state']=='error'
|
||||
assert restored.get()['scene_available'] and restored.sample is not None
|
||||
assert restored.get()['stale'] and not lock.locked()
|
||||
assert restored.history()[0]['query_session_id']=='B'
|
||||
assert restored.scene(run['id'],True).startswith(b'RRF2')
|
||||
|
||||
|
||||
def test_cancel_retains_raw_and_releases_only_derived_lease(tmp_path,monkeypatch):
|
||||
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
||||
run=service.start('draft',1)
|
||||
until(lambda:service.get()['state']=='waiting')
|
||||
service.stop(run['id']);service.close()
|
||||
assert service.get()['state']=='cancelled' and not lock.locked() and source.owner is None
|
||||
assert service.get()['vehicle_control'] is False
|
||||
with pytest.raises(KeyError):service.stop('wrong-id')
|
||||
|
||||
|
||||
def test_native_rerun_base_and_incremental_update_are_valid():
|
||||
reference=np.random.default_rng(1).normal(size=(400,3));path=np.array([[0,0,0],[4,0,0]])
|
||||
sample=dict(points=reference,path=path,hint=np.eye(4))
|
||||
assert scene_bytes('test',reference,path,sample,base=True).startswith(b'RRF2')
|
||||
assert scene_bytes('test',reference,path,sample,base=False).startswith(b'RRF2')
|
||||
|
||||
|
||||
def test_receipt_gap_is_not_an_instantaneous_jump_and_clears_fit_window():
|
||||
b=LiveCloudBuffer([[0,0,0],[30,0,0]])
|
||||
b.ingest(event('pose',t=1,p=(0,0,0)))
|
||||
b.ingest(event('points',t=1.1,points=np.array([[0,0,1]])))
|
||||
b.ingest(event('pose',t=11,p=(10.4,0,0)))
|
||||
sample=b.snapshot()
|
||||
assert sample['segment']==1 and len(sample['points'])==0
|
||||
assert sample['gaps'][0]['seconds']==10
|
||||
assert sample['distance']==pytest.approx(10.4)
|
||||
# The same displacement over 100 ms still rejects; no threshold bypass.
|
||||
with pytest.raises(ValueError,match='Разрыв координат'):
|
||||
b.ingest(event('pose',t=11.1,p=(21,0,0)))
|
||||
|
||||
|
||||
@pytest.mark.parametrize("loss", ["stale", "reference-coverage", "fit-rejected", "worker-error", "pose-jump", "operator-stop", "operator-stop-pending"])
|
||||
def test_live_stationary_bootstrap_keeps_calibration_separate_and_requires_fresh_windows(tmp_path,monkeypatch,loss):
|
||||
import k1link.missions.live_tests as module
|
||||
from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
|
||||
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
||||
clock=[100.]
|
||||
monkeypatch.setattr(module,'time',SimpleNamespace(
|
||||
monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
|
||||
calls=[]
|
||||
fail_next=[None]
|
||||
entry_release=threading.Event()
|
||||
fresh_entered, fresh_release = threading.Event(), threading.Event()
|
||||
def fit(directory,ref,query,hint):
|
||||
failure,fail_next[0]=fail_next[0],None
|
||||
if failure=="worker-error":raise ValueError("fixture worker unavailable")
|
||||
if failure=="fit-rejected":return dict(status="rejected",reasons=["fixture bad fit"])
|
||||
if failure == "operator-stop-pending":
|
||||
fresh_entered.set()
|
||||
assert fresh_release.wait(5)
|
||||
calls.append('fresh')
|
||||
return dict(status='candidate',T_reference_query=hint.tolist(),matched_query_indices=[0],
|
||||
overlap=.9,inlier_rmse_m=.1,reasons=[])
|
||||
def entry(directory,ref,path,query,anchor,**kwargs):
|
||||
if calls:
|
||||
assert kwargs["reference_position"] == [0.,0.,0.]
|
||||
calls.append('entry')
|
||||
entry_release.wait(3)
|
||||
return dict(status='candidate',T_reference_query=np.eye(4).tolist(),matched_query_indices=[0],
|
||||
overlap=.9,inlier_rmse_m=.1,reasons=[],initialization=dict(
|
||||
complete=True,scope='selected-route',policy=ROUTE_RELOCALIZATION_POLICY,
|
||||
expected_attempts=1,attempts=[{}]))
|
||||
monkeypatch.setattr(module,'run_registration',fit)
|
||||
monkeypatch.setattr(module,'run_route_relocalization',entry)
|
||||
service.start('draft',1)
|
||||
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
|
||||
sequence=0
|
||||
def frame(t,p=(0,0,0)):
|
||||
nonlocal sequence
|
||||
sequence+=1;source.queue.put(event('pose',t=t,p=p,sequence=sequence))
|
||||
sequence+=1;source.queue.put(event('points',t=t+.001,points=points,sequence=sequence))
|
||||
return sequence
|
||||
try:
|
||||
until(lambda:service.get()['state']=='waiting')
|
||||
source.state.update(active=True,session_id='B',session_generation=2)
|
||||
# Session opens during hardware calibration. Neither control nor poses
|
||||
# start route initialization without a usable cloud.
|
||||
source.queue.put(event('session-start',t=100,sequence=0))
|
||||
until(lambda:service.get()['state']=='running')
|
||||
clock[0]=160
|
||||
source.queue.put(event('pose',t=159,sequence=0))
|
||||
until(lambda:source.queue.empty())
|
||||
assert service.get()['planning_phase']=='waiting-cloud' and not calls
|
||||
for i in range(20):frame(160+i*.5)
|
||||
until(lambda:source.queue.empty())
|
||||
assert service.get()['planning_phase']=='collecting'
|
||||
clock[0]=170.01
|
||||
until(lambda:calls==['entry'])
|
||||
# Worker submission and publication happen on separate threads; the
|
||||
# fit callback is not an acknowledgement that UI state was persisted.
|
||||
until(lambda:service.get()['planning_phase']=='searching')
|
||||
# Capture continues while the worker searches; an actual post-ready
|
||||
# receipt gap must invalidate the prior, not be hidden by this fixture.
|
||||
for i in range(40):frame(170+i*.5)
|
||||
until(lambda:source.queue.empty())
|
||||
clock[0]=190
|
||||
entry_release.set()
|
||||
until(lambda:service.get().get('initialization_result') is not None
|
||||
and service.get()['planning_phase']=='refreshing')
|
||||
assert service.get()['planning_phase']=='refreshing'
|
||||
assert service.get()['result'] is None and service.accepted_sample is None
|
||||
previous_result=None
|
||||
for j in range(3):
|
||||
begin=190.5+j*5
|
||||
for i in range(10):
|
||||
clock[0]=begin+i*.5+.002
|
||||
frame(begin+i*.5)
|
||||
until(lambda:source.queue.empty())
|
||||
until(lambda:service.get().get('result_source_sequence') not in (None,previous_result))
|
||||
previous_result=service.get()['result_source_sequence']
|
||||
assert service.get()['tracking_state']==('tracking' if j==2 else 'acquiring')
|
||||
assert service.get()['tracking_established'] == (j == 2)
|
||||
assert 'Кандидат совмещения' != service.get()['message']
|
||||
assert service._scene_result['matched_query_indices']==([0] if j==2 else [])
|
||||
assert calls==['entry','fresh','fresh','fresh']
|
||||
assert service.get()['planning_phase']=='tracking'
|
||||
if loss in {"operator-stop", "operator-stop-pending"}:
|
||||
if loss == "operator-stop-pending":
|
||||
fail_next[0] = loss
|
||||
begin = clock[0] + .5
|
||||
for i in range(10):
|
||||
clock[0] = begin + i*.5 + .002
|
||||
frame(begin+i*.5)
|
||||
until(source.queue.empty)
|
||||
until(fresh_entered.is_set)
|
||||
source.state['spatial_stop_requested'] = True
|
||||
# Model the recorded 51-second raw finalisation with a virtual clock.
|
||||
clock[0] += 51
|
||||
until(lambda: service.get()['state'] == 'completed')
|
||||
assert service.get()['termination_reason'] == 'spatial-stop-requested'
|
||||
assert service.get().get('recovery_attempt', 0) == 0
|
||||
assert service.get()['planning_phase'] == 'ended'
|
||||
assert service.accepted_sample is None and source.state['active']
|
||||
assert service.get()['result_source_sequence'] == previous_result
|
||||
assert not any(t['phase'] in {'lost', 'recovering'}
|
||||
for t in service.get()['phase_transitions'])
|
||||
fresh_release.set()
|
||||
until(lambda: not service.thread.is_alive())
|
||||
assert service.get()['result_source_sequence'] == previous_result
|
||||
assert service.accepted_sample is None
|
||||
return
|
||||
if loss=="stale":
|
||||
clock[0]+=9
|
||||
else:
|
||||
import k1link.missions.stationary_live as live_loop
|
||||
from k1link.missions.reference_window import ReferenceCoverageError
|
||||
original_window=live_loop.reference_window
|
||||
failed=[False]
|
||||
def window(*args,**kwargs):
|
||||
if not failed[0] and loss=="reference-coverage":
|
||||
failed[0]=True
|
||||
raise ReferenceCoverageError("fixture coverage failure")
|
||||
return original_window(*args,**kwargs)
|
||||
monkeypatch.setattr(live_loop,"reference_window",window)
|
||||
fail_next[0]=loss
|
||||
begin=clock[0]+.5
|
||||
for i in range(8):
|
||||
clock[0]=begin+i*.5+.002
|
||||
frame(begin+i*.5,p=(100,0,0) if loss=="pose-jump" else (0,0,0))
|
||||
until(source.queue.empty)
|
||||
if service.get()['planning_phase']=='recovering':break
|
||||
monkeypatch.setattr(live_loop,"reference_window",original_window)
|
||||
until(lambda:service.get()['tracking_state']=='lost')
|
||||
until(lambda:service.get()['planning_phase']=='recovering')
|
||||
assert service.accepted_sample is None
|
||||
assert service.get()['tracking_established'] is True
|
||||
assert service.get()['state']=='running' and source.state['active']
|
||||
assert service.get()['recovery_attempt']==1
|
||||
previous_fresh=calls.count('fresh')
|
||||
# A fresh stationary prefix, never replay of the previously accepted fit.
|
||||
begin=clock[0]+.5
|
||||
for i in range(20):
|
||||
clock[0]=begin+i*.5+.002
|
||||
frame(begin+i*.5)
|
||||
until(source.queue.empty)
|
||||
clock[0]=begin+10.01
|
||||
until(lambda:calls.count('entry')==2)
|
||||
until(lambda:service.run.get('initialization_temporal',{}).get('provisional') is True
|
||||
and service.run.get('planning_phase')=='recovering'
|
||||
and service.run.get('planning_reason')=='provisional-prior')
|
||||
assert service.accepted_sample is None
|
||||
begin=clock[0]+.5
|
||||
for j in range(3):
|
||||
for i in range(10):
|
||||
clock[0]=begin+j*5+i*.5+.002
|
||||
frame(begin+j*5+i*.5)
|
||||
until(source.queue.empty)
|
||||
until(lambda:calls.count('fresh')>=previous_fresh+1+j)
|
||||
assert service.get()['planning_phase']==('tracking' if j==2 else 'recovering')
|
||||
assert service.get()['tracking_state']=='tracking'
|
||||
source.state['active']=False
|
||||
until(lambda:service.get()['state']=='completed')
|
||||
finally:
|
||||
entry_release.set();fresh_release.set();service.close()
|
||||
assert not lock.locked() and source.owner is None
|
||||
|
||||
|
||||
def test_operator_can_retry_a_failed_route_identification_without_stopping_capture(tmp_path,monkeypatch):
|
||||
import k1link.missions.live_tests as module
|
||||
from k1link.missions.route_relocalization import ROUTE_RELOCALIZATION_POLICY
|
||||
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
||||
clock=[100.]
|
||||
monkeypatch.setattr(module,'time',SimpleNamespace(
|
||||
monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
|
||||
searches=[]
|
||||
def rejected_entry(*_args):
|
||||
searches.append('entry')
|
||||
return dict(status='rejected',T_reference_query=np.eye(4).tolist(),matched_query_indices=[],
|
||||
overlap=.6,inlier_rmse_m=.28,reasons=['Большое расстояние между поверхностями.'],
|
||||
initialization=dict(complete=True,scope='selected-route',
|
||||
policy=ROUTE_RELOCALIZATION_POLICY,expected_attempts=1,attempts=[{}],
|
||||
reason='no-route-location'))
|
||||
monkeypatch.setattr(module,'run_route_relocalization',rejected_entry)
|
||||
run=service.start('draft',1)
|
||||
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
|
||||
sequence=0
|
||||
def frame(t,p=(0,0,0)):
|
||||
nonlocal sequence
|
||||
sequence+=1;source.queue.put(event('pose',t=t,p=p,sequence=sequence))
|
||||
sequence+=1;source.queue.put(event('points',t=t+.001,points=points,sequence=sequence))
|
||||
try:
|
||||
until(lambda:service.get()['state']=='waiting')
|
||||
source.state.update(active=True,session_id='B',session_generation=2)
|
||||
for i in range(20):frame(100+i*.5)
|
||||
until(lambda:source.queue.empty());clock[0]=110.01
|
||||
until(lambda:service.get()['planning_phase']=='lost')
|
||||
assert searches==['entry'] and source.owner=='planning-'+run['id']
|
||||
retried=service.request_reinitialization(run['id'])
|
||||
assert retried['initialization_attempt']==2
|
||||
assert retried['reinitialization_count']==1
|
||||
assert retried['initialization_result'] is None
|
||||
until(lambda:not service.reinitialization_requested)
|
||||
# The scanner may be carried to a better point while the first failed
|
||||
# location stays recoverable; those receipts must not contaminate a
|
||||
# new stationary prefix or stop K1 capture.
|
||||
frame(111.0)
|
||||
frame(111.5,p=(1,0,0))
|
||||
until(lambda:source.queue.empty())
|
||||
assert service.get()['planning_phase']=='lost'
|
||||
assert service.get()['state']=='running'
|
||||
frame(112)
|
||||
frame(112.5)
|
||||
until(source.queue.empty)
|
||||
assert service.get()['planning_phase']=='lost' # No implicit retry after a moved prefix.
|
||||
retried=service.request_reinitialization(run['id'])
|
||||
assert retried['initialization_attempt']==3
|
||||
until(lambda:not service.reinitialization_requested)
|
||||
for i in range(20):frame(120+i*.5)
|
||||
until(lambda:source.queue.empty());clock[0]=130.01
|
||||
until(lambda:searches==['entry','entry'])
|
||||
until(lambda:service.get()['planning_phase']=='lost')
|
||||
assert service.get()['query_session_id']=='B'
|
||||
assert source.owner=='planning-'+run['id']
|
||||
finally:
|
||||
source.state['active']=False
|
||||
service.close()
|
||||
assert not lock.locked() and source.owner is None
|
||||
|
||||
|
||||
def test_stationary_cancel_while_searching_does_not_publish_late_prior(tmp_path,monkeypatch):
|
||||
import k1link.missions.live_tests as module
|
||||
service,source,lock,_=fixture_service(tmp_path,monkeypatch)
|
||||
clock=[100.]; entered=threading.Event();release=threading.Event()
|
||||
monkeypatch.setattr(module,'time',SimpleNamespace(monotonic=lambda:clock[0],monotonic_ns=lambda:int(clock[0]*1e9)))
|
||||
def search(*args,**kwargs):
|
||||
entered.set();release.wait(3)
|
||||
return dict(status='rejected',reasons=['test-cancel'])
|
||||
monkeypatch.setattr(module,'run_route_relocalization',search)
|
||||
run=service.start('draft',1)
|
||||
points=np.random.default_rng(11).uniform([-1,-3,-1],[8,3,3],(1500,3))
|
||||
try:
|
||||
until(lambda:service.get()['state']=='waiting')
|
||||
source.state.update(active=True,session_id='B',session_generation=2)
|
||||
for i in range(20):
|
||||
source.queue.put(event('pose',t=100+i*.5,sequence=i*2+1))
|
||||
source.queue.put(event('points',t=100+i*.5+.001,sequence=i*2+2,points=points))
|
||||
until(lambda:source.queue.empty());clock[0]=110.01
|
||||
until(entered.is_set)
|
||||
service.stop(run['id']);release.set()
|
||||
until(lambda:service.get()['state']=='cancelled')
|
||||
finally:
|
||||
release.set();service.close()
|
||||
assert service.accepted_sample is None and service.get()['result'] is None
|
||||
assert not lock.locked() and source.owner is None
|
||||
@@ -0,0 +1,196 @@
|
||||
"""Project browsing binds to existing evidence and has no acquisition authority."""
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
from uuid import uuid4
|
||||
import numpy as np
|
||||
import pytest
|
||||
from fastapi import FastAPI
|
||||
from fastapi.testclient import TestClient
|
||||
from k1link.missions.projects import PlanningProjects
|
||||
from k1link.web.mission_registration_api import build_mission_registration_router
|
||||
|
||||
|
||||
def fixture(tmp_path):
|
||||
draft = dict(id=str(uuid4()), revision=1, name='Reference experiment', updated_at_utc='2026-09-11T10:00:00Z',
|
||||
zone=dict(label='A'), route=dict(length_m=30, points=[dict(position=[0,0,0]), dict(position=[30,0,0])]))
|
||||
root=tmp_path/'recorded'; root.mkdir()
|
||||
live_root=tmp_path/'live'; live_root.mkdir()
|
||||
def read(identity): return json.loads((root/identity/'report.json').read_text())
|
||||
runs=SimpleNamespace(root=root,get=read,directory=lambda identity:root/identity,
|
||||
drafts=SimpleNamespace(list=lambda:[draft],get=lambda identity:draft))
|
||||
live=SimpleNamespace(root=live_root,directory=lambda identity:live_root/identity)
|
||||
return draft,runs,live,PlanningProjects(runs,live)
|
||||
|
||||
|
||||
def save(owner,doc):
|
||||
directory=owner.directory(doc['id']);directory.mkdir()
|
||||
(directory/'report.json').write_text(json.dumps(doc))
|
||||
return directory
|
||||
|
||||
|
||||
def test_each_result_is_a_project_and_uses_frozen_draft(tmp_path):
|
||||
draft,runs,live,projects=fixture(tmp_path)
|
||||
ids=[str(uuid4()),str(uuid4())]
|
||||
for n,identity in enumerate(ids):
|
||||
save(runs,dict(id=identity,draft=dict(draft),state='ready',revision=1,created_at_utc=f'2026-09-11T10:0{n}:00Z',
|
||||
scene_url=f'/recorded/{identity}.rrd',result=dict(status='candidate',matched_query_indices=[1,2])))
|
||||
draft.update(name='Later edited name',revision=8)
|
||||
items=projects.list()
|
||||
assert len(items)==2 and items[0]['key']=='recorded:'+ids[1]
|
||||
assert all(p['name']=='Reference experiment' and p['revision']==1 for p in items)
|
||||
detail=projects.get('recorded',ids[0])
|
||||
assert detail['draft']['revision']==1 and detail['scene_url']==f'/recorded/{ids[0]}.rrd'
|
||||
assert 'matched_query_indices' not in detail['result']
|
||||
assert not detail['vehicle_control'] and not detail['localization_confirmed']
|
||||
|
||||
|
||||
def test_unstarted_draft_and_failed_live_are_honest_states(tmp_path):
|
||||
draft,runs,live,projects=fixture(tmp_path)
|
||||
assert projects.list()[0]['kind']=='draft'
|
||||
identity=str(uuid4())
|
||||
save(live,dict(id=identity,draft=draft,state='error',query_session_id='B',created_at_utc='2026-09-11T11:00:00Z',message='No fit',result=None))
|
||||
assert len(projects.list())==1
|
||||
detail=projects.get('live',identity)
|
||||
assert detail['scene_url'] is None and detail['result'] is None and detail['message']=='No fit'
|
||||
with pytest.raises(ValueError): projects.live_scene(identity)
|
||||
|
||||
|
||||
def test_preparation_only_probe_is_not_a_passage_project(tmp_path):
|
||||
draft,runs,live,projects=fixture(tmp_path)
|
||||
save(live,dict(id=str(uuid4()),draft=draft,state='cancelled',query_session_id=None,created_at_utc='2026-09-11T11:00:00Z'))
|
||||
assert [p['kind'] for p in projects.list()]==['draft']
|
||||
|
||||
|
||||
def test_scene_hash_is_verified_and_browsing_router_never_starts_work(tmp_path):
|
||||
draft,runs,live,projects=fixture(tmp_path)
|
||||
identity=str(uuid4());payload=b'frozen-scene'
|
||||
directory=save(runs,dict(id=identity,draft=draft,state='ready',created_at_utc='2026-09-11T11:00:00Z',
|
||||
result={'status':'candidate'},artifacts={'scene.rrd':hashlib.sha256(payload).hexdigest()}))
|
||||
(directory/'scene.rrd').write_bytes(payload)
|
||||
app=FastAPI();app.include_router(build_mission_registration_router(runs,live))
|
||||
with TestClient(app) as client:
|
||||
assert client.get('/api/v1/mission-planner/projects').json()['items'][0]['key']=='recorded:'+identity
|
||||
assert client.get('/api/v1/mission-planner/projects/recorded/'+identity).status_code==200
|
||||
assert client.get('/api/v1/mission-planner/registration-runs/'+identity+'/scene.rrd').content==payload
|
||||
(directory/'scene.rrd').write_bytes(b'different-scene')
|
||||
assert client.get('/api/v1/mission-planner/registration-runs/'+identity+'/scene.rrd').status_code==409
|
||||
assert client.get('/api/v1/mission-planner/projects/unknown/'+identity).status_code==404
|
||||
|
||||
|
||||
def test_completed_live_uses_committed_fit_not_unregistered_terminal_preview(tmp_path,monkeypatch):
|
||||
import k1link.missions.registration_scene as renderer
|
||||
draft,runs,live,projects=fixture(tmp_path)
|
||||
identity=str(uuid4());transform=np.eye(4);transform[0,3]=3
|
||||
result={'status':'candidate','T_reference_query':transform.tolist(),'matched_query_indices':[0]}
|
||||
doc=dict(id=identity,draft=draft,state='completed',created_at_utc='2026-09-11T11:00:00Z',query_session_id='B',
|
||||
result=result,result_source_sequence=8,artifacts={})
|
||||
directory=save(live,doc);step=directory/'step-001';step.mkdir()
|
||||
(step/'source.json').write_text(json.dumps({'sequence':8,'query_path':[[1,2,3],[4,5,6]]}))
|
||||
(step/'registration-result.json').write_text(json.dumps(result))
|
||||
np.savez(step/'registration-input.npz',reference=np.zeros((2,3)),query=np.ones((2,3)),initial=np.eye(4))
|
||||
for path in step.iterdir(): doc['artifacts'][str(path.relative_to(directory))]=hashlib.sha256(path.read_bytes()).hexdigest()
|
||||
(directory/'report.json').write_text(json.dumps(doc))
|
||||
before=(directory/'report.json').read_bytes()
|
||||
captured=[]
|
||||
def writer(path,run_id,reference,query,actual,ref_path,query_path):
|
||||
captured.append((run_id,query.copy(),actual,query_path.copy()));path.write_bytes(b'scene')
|
||||
monkeypatch.setattr(renderer,'write_scene',writer)
|
||||
scene=projects.live_scene(identity)
|
||||
assert scene.read_bytes()==b'scene' and len(captured)==1
|
||||
assert captured[0][2]['T_reference_query']==transform.tolist()
|
||||
np.testing.assert_equal(captured[0][1],np.ones((2,3)))
|
||||
assert (directory/'report.json').read_bytes()==before
|
||||
projects.live_scene(identity);assert len(captured)==1
|
||||
(step/'registration-result.json').write_text('{}')
|
||||
with pytest.raises(ValueError,match='целостности'): projects.live_scene(identity)
|
||||
|
||||
|
||||
def test_spatial_renderer_transforms_query_cloud_and_path_once(tmp_path,monkeypatch):
|
||||
import k1link.missions.registration_scene as renderer
|
||||
logged={}
|
||||
class Recording:
|
||||
def __init__(self,*args,**kwargs): pass
|
||||
def log(self,name,value,**kwargs): logged[name]=value
|
||||
def save(self,*args): pass
|
||||
def send_blueprint(self,*args): pass
|
||||
def flush(self): pass
|
||||
def disconnect(self): pass
|
||||
monkeypatch.setattr(renderer.rr,'RecordingStream',Recording)
|
||||
monkeypatch.setattr(renderer.rr,'Points3D',lambda xyz,**kw:np.asarray(xyz))
|
||||
monkeypatch.setattr(renderer.rr,'LineStrips3D',lambda xyz,**kw:np.asarray(xyz))
|
||||
t=np.eye(4);t[:3,3]=[3,-2,1]
|
||||
points=np.array([[1.,2,3],[4,5,6]])
|
||||
renderer.write_scene(tmp_path/'scene.rrd','test',points,points,{'status':'rejected','T_reference_query':t.tolist()},points,points)
|
||||
np.testing.assert_equal(logged['world/reference'],points)
|
||||
np.testing.assert_equal(logged['world/query'],points+[3,-2,1])
|
||||
np.testing.assert_equal(logged['world/query_path'][0],points+[3,-2,1])
|
||||
|
||||
|
||||
@pytest.mark.parametrize('kind,state', [('recorded', 'ready'), ('recorded', 'error'),
|
||||
('live', 'completed'), ('live', 'cancelled'), ('live', 'error'), ('live', 'interrupted')])
|
||||
def test_delete_one_project_preserves_evidence_and_survives_restart(tmp_path, kind, state):
|
||||
draft, runs, live, projects = fixture(tmp_path)
|
||||
owner = runs if kind == 'recorded' else live
|
||||
ids = [str(uuid4()), str(uuid4())]
|
||||
preserved = {}
|
||||
for identity in ids:
|
||||
directory = save(owner, dict(id=identity, draft=draft, state=state,
|
||||
query_session_id='original-passage', created_at_utc='2026-09-11T11:00:00Z'))
|
||||
(directory / 'scene.rrd').write_bytes(b'original-derived-scene')
|
||||
for path in directory.iterdir(): preserved[path] = path.read_bytes()
|
||||
key = kind + ':' + ids[0]
|
||||
app = FastAPI(); app.include_router(build_mission_registration_router(runs, live))
|
||||
with TestClient(app) as client:
|
||||
url = '/api/v1/mission-planner/projects/' + kind + '/' + ids[0]
|
||||
for _ in range(2):
|
||||
reply = client.request('DELETE', url, json={'revision': 1})
|
||||
assert reply.status_code == 200 and reply.json() == {'key': key, 'deleted': True}
|
||||
assert client.get(url).status_code == 404
|
||||
assert [item['key'] for item in client.get('/api/v1/mission-planner/projects').json()['items']] == [kind + ':' + ids[1]]
|
||||
# Same-name sibling remains; the shared draft cannot resurface after the last deletion.
|
||||
projects.remove(kind, ids[1], 1)
|
||||
assert PlanningProjects(runs, live).list() == []
|
||||
assert all(path.read_bytes() == data for path, data in preserved.items())
|
||||
assert runs.drafts.get(draft['id']) == draft
|
||||
|
||||
|
||||
def test_delete_draft_revision_and_project_kind_are_exact(tmp_path):
|
||||
draft, runs, live, projects = fixture(tmp_path)
|
||||
with pytest.raises(ValueError, match='изменён'):
|
||||
projects.remove('draft', draft['id'], 9)
|
||||
with pytest.raises(KeyError): projects.remove('unknown', draft['id'], 1)
|
||||
assert len(projects.list()) == 1
|
||||
projects.remove('draft', draft['id'], 1)
|
||||
assert PlanningProjects(runs, live).list() == []
|
||||
assert runs.drafts.get(draft['id']) == draft
|
||||
# A later immutable run has an independent identity, not the draft tombstone.
|
||||
save(runs, dict(id=draft['id'], draft=draft, state='ready', created_at_utc='2026-09-11T11:00:00Z'))
|
||||
assert projects.list()[0]['kind'] == 'recorded'
|
||||
|
||||
|
||||
@pytest.mark.parametrize('kind,state', [('recorded', 'queued'), ('recorded', 'running'),
|
||||
('live', 'preparing'), ('live', 'waiting'), ('live', 'running'), ('live', 'unknown')])
|
||||
def test_delete_fails_closed_for_active_or_unknown_states(tmp_path, kind, state):
|
||||
draft, runs, live, projects = fixture(tmp_path)
|
||||
identity = str(uuid4())
|
||||
save(runs if kind == 'recorded' else live, dict(id=identity, draft=draft,
|
||||
state=state, created_at_utc='2026-09-11T11:00:00Z'))
|
||||
app = FastAPI(); app.include_router(build_mission_registration_router(runs, live))
|
||||
with TestClient(app) as client:
|
||||
url = '/api/v1/mission-planner/projects/' + kind + '/' + identity
|
||||
assert client.request('DELETE', url, json={'revision': 1}).status_code == 409
|
||||
assert client.request('DELETE', url, json={'revision': 1, 'delete_sources': True}).status_code == 422
|
||||
assert client.request('DELETE', '/api/v1/mission-planner/projects/draft/' + draft['id'], json={'revision': 1}).status_code == 409
|
||||
assert client.get(url).status_code == 200
|
||||
assert len(projects.list()) == 1
|
||||
|
||||
|
||||
def test_delete_api_rejects_missing_and_invalid_identity(tmp_path):
|
||||
_, runs, live, _ = fixture(tmp_path)
|
||||
app = FastAPI(); app.include_router(build_mission_registration_router(runs, live))
|
||||
with TestClient(app) as client:
|
||||
for kind in ['unknown', 'live']:
|
||||
assert client.request('DELETE', f'/api/v1/mission-planner/projects/{kind}/{uuid4()}', json={'revision': 1}).status_code == 404
|
||||
assert client.request('DELETE', '/api/v1/mission-planner/projects/live/not-a-uuid', json={'revision': 1}).status_code == 422
|
||||
@@ -0,0 +1,136 @@
|
||||
"""Shared recorded MQTT/camera ingress; no socket, device command or video process.
|
||||
|
||||
The producer-generation and camera-binding fixtures replace physical admission.
|
||||
The writers, facade observers, queue, decoder and planning owner are real.
|
||||
"""
|
||||
|
||||
import struct
|
||||
from dataclasses import replace
|
||||
|
||||
import paho.mqtt.client as mqtt
|
||||
from test_planning_live import fixture_service, until
|
||||
from test_stream_summary import _pcl_payload, _pose_payload
|
||||
|
||||
from k1link.device_plugins.xgrids_k1.camera import _CameraProducer
|
||||
from k1link.device_plugins.xgrids_k1.facade import XgridsK1CompatibilityService
|
||||
from k1link.device_plugins.xgrids_k1.mqtt.capture import _CaptureWriter
|
||||
from k1link.device_plugins.xgrids_k1.planning_live import K1PlanningLiveSource
|
||||
from k1link.device_plugins.xgrids_k1.viewer.messages import StreamMessage
|
||||
from k1link.viewer.metrics import BridgeMetrics
|
||||
from k1link.web.camera_archive import CameraArchiveWriter
|
||||
|
||||
|
||||
def box(kind, payload=b""):
|
||||
return struct.pack(">I4s", len(payload) + 8, kind) + payload
|
||||
|
||||
|
||||
def test_planning_cancel_preserves_both_recorders_and_generation_fences(tmp_path, monkeypatch):
|
||||
device = XgridsK1CompatibilityService(tmp_path / "device")
|
||||
ingress = device.live_perception_ingress
|
||||
adapter = K1PlanningLiveSource(ingress)
|
||||
planner, _, compute_lock, _ = fixture_service(tmp_path, monkeypatch)
|
||||
planner.sources["test-plugin"] = adapter
|
||||
observed = []
|
||||
take = adapter.take
|
||||
|
||||
def observe_take(owner):
|
||||
receipt = take(owner)
|
||||
if receipt is not None:
|
||||
observed.append(receipt)
|
||||
return receipt
|
||||
|
||||
monkeypatch.setattr(adapter, "take", observe_take)
|
||||
monkeypatch.setattr(
|
||||
device.runtime,
|
||||
"snapshot",
|
||||
lambda: {
|
||||
"source_mode": "live",
|
||||
"producer_generation": 7,
|
||||
},
|
||||
)
|
||||
mqtt_writer = _CaptureWriter(tmp_path / "mqtt", max_message_bytes=2 * 1024 * 1024)
|
||||
mqtt_writer.open()
|
||||
(tmp_path / "recording").mkdir()
|
||||
camera_writer = CameraArchiveWriter(tmp_path / "recording", "sensor.camera.right", 3)
|
||||
gateway = device.camera_preview
|
||||
# Inert producer: _publish_segment has no process operations. Detach it
|
||||
# before service.close, so this fixture can never spawn/stop an encoder.
|
||||
producer = _CameraProducer(3, "sensor.camera.right", None, camera_writer)
|
||||
gateway._producer = producer
|
||||
device._live_perception_camera_binding = ("B", "sensor.camera.right", 3)
|
||||
metrics = BridgeMetrics()
|
||||
|
||||
def mqtt_receipt(topic, payload):
|
||||
message = mqtt.MQTTMessage(topic=topic.encode())
|
||||
message.payload = payload
|
||||
recorded = mqtt_writer.record(message)
|
||||
# Raw-first, not a claim of per-message fsync: capture uses group commit.
|
||||
assert mqtt_writer.raw_path.read_bytes().endswith(payload)
|
||||
stream = StreamMessage(
|
||||
recorded.sequence,
|
||||
recorded.topic,
|
||||
recorded.payload,
|
||||
recorded.received_at_epoch_ns,
|
||||
recorded.received_monotonic_ns,
|
||||
"live_mqtt",
|
||||
7,
|
||||
)
|
||||
device._observe_runtime_message(stream, metrics)
|
||||
return stream
|
||||
|
||||
try:
|
||||
run = planner.start("draft", 1)
|
||||
until(lambda: planner.get()["state"] == "waiting")
|
||||
ingress.begin_session("B")
|
||||
until(lambda: any(item.kind == "session-start" for item in observed))
|
||||
init = box(b"ftyp", b"fixture") + box(b"moov", b"fixture")
|
||||
media = box(b"moof", b"fixture") + box(b"mdat", b"synthetic-media")
|
||||
assert gateway._publish_segment(producer, "init", init)
|
||||
assert camera_writer.init_path.read_bytes() == init
|
||||
pose = mqtt_receipt("lixel/application/report/lio_pose", _pose_payload((0, 0, 0)))
|
||||
assert gateway._publish_segment(producer, "media", media)
|
||||
assert (camera_writer.segments_dir / "1.m4s").read_bytes() == media
|
||||
mqtt_receipt("lixel/application/report/lio_pcl", _pcl_payload(scaler=1000, point_count=4))
|
||||
until(lambda: len(observed) == 5)
|
||||
assert [item.kind for item in observed] == [
|
||||
"session-start",
|
||||
"ignored",
|
||||
"pose",
|
||||
"ignored",
|
||||
"points",
|
||||
]
|
||||
assert [item.sequence for item in observed] == sorted(item.sequence for item in observed)
|
||||
assert all(item.session_id == "B" and item.generation == 1 for item in observed)
|
||||
assert gateway._committed_segment_observer_errors == 0
|
||||
|
||||
# Old producer callbacks cannot contaminate this same consumer.
|
||||
before = ingress.snapshot()["queues"]
|
||||
assert device._observe_runtime_message(replace(pose, producer_generation=6), metrics)
|
||||
device._live_perception_camera_binding = ("B", "sensor.camera.right", 4)
|
||||
assert gateway._publish_segment(producer, "media", media)
|
||||
after = ingress.snapshot()["queues"]
|
||||
assert after["pose"]["published"] == before["pose"]["published"]
|
||||
assert after["camera-frame"]["published"] == before["camera-frame"]["published"]
|
||||
# The stale camera epoch still belongs to its own archive, not the new ingress.
|
||||
assert (camera_writer.segments_dir / "2.m4s").read_bytes() == media
|
||||
|
||||
planner.stop(run["id"])
|
||||
planner.close()
|
||||
assert not compute_lock.locked() and not ingress.snapshot()["consumer_connected"]
|
||||
assert ingress.snapshot()["active"] # Planning did not stop acquisition.
|
||||
raw_before = mqtt_writer.raw_path.read_bytes()
|
||||
mqtt_receipt("lixel/application/report/lio_pose", _pose_payload((1, 0, 0)))
|
||||
assert mqtt_writer.raw_path.read_bytes().startswith(raw_before)
|
||||
assert len(mqtt_writer.raw_path.read_bytes()) > len(raw_before)
|
||||
assert gateway._publish_segment(producer, "media", media)
|
||||
assert (camera_writer.segments_dir / "3.m4s").read_bytes() == media
|
||||
assert camera_writer.init_path.read_bytes() == init
|
||||
assert planner.get()["state"] == "cancelled"
|
||||
assert planner.get()["vehicle_control"] is False
|
||||
finally:
|
||||
planner.close()
|
||||
gateway._producer = None
|
||||
camera_writer.close()
|
||||
mqtt_writer.close()
|
||||
ingress.end_session("B")
|
||||
device.close()
|
||||
@@ -0,0 +1,187 @@
|
||||
"""Selected route length drives live admission, reference coverage and termination."""
|
||||
|
||||
import threading
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from test_planning_live import event, fixture_service, until
|
||||
|
||||
from k1link.missions.live_buffer import LiveCloudBuffer
|
||||
from k1link.missions.live_limits import live_route_limits
|
||||
from k1link.missions.reference_map import build_reference_map, reference_intervals
|
||||
from k1link.missions.reference_window import reference_window
|
||||
|
||||
|
||||
@pytest.mark.parametrize("length", [3, 30, 50, 100, 200, 300, 10_000])
|
||||
def test_limits_come_from_selected_length(length):
|
||||
limits = live_route_limits(length)
|
||||
assert limits["maximum_distance_m"] == length
|
||||
assert limits["maximum_seconds"] is None
|
||||
assert limits["route_policy"]["maximum_m"] is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize("length", [0, 2.9, float("nan"), float("inf")])
|
||||
def test_invalid_route_fails_before_admission(length):
|
||||
with pytest.raises(ValueError):
|
||||
live_route_limits(length)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("start,end", [(0, 50), (0, 100), (20, 220), (70, 370)])
|
||||
def test_reference_tiles_cover_selected_route_and_context(start, end):
|
||||
poses = [dict(distance_m=i) for i in range(401)]
|
||||
tiles = reference_intervals(poses, start, end)
|
||||
assert tiles[0][0] == max(0, start - 20)
|
||||
assert tiles[-1][1] == min(400, end + 20)
|
||||
assert all(b - a <= 40 for a, b in tiles)
|
||||
assert all(b == c for (_, b), (c, _) in zip(tiles, tiles[1:], strict=False))
|
||||
|
||||
|
||||
def test_reference_window_keeps_local_density_and_moves_with_accepted_transform():
|
||||
local = np.random.default_rng(12).normal(size=(1500, 3))
|
||||
distant = np.tile([[300.0, 0, 0]], (100_001, 1))
|
||||
reference = np.concatenate([local, distant])
|
||||
sample = dict(points=local, path=np.array([[0.0, 0, 0], [1, 0, 0]]))
|
||||
selected, meta = reference_window(reference, sample, np.eye(4))
|
||||
np.testing.assert_array_equal(selected, local)
|
||||
assert meta["target_points"] == 1500 and meta["map_points"] == 101_501
|
||||
hint = np.eye(4)
|
||||
hint[0, 3] = 300
|
||||
dense, meta = reference_window(reference, sample, hint)
|
||||
np.testing.assert_array_equal(dense, distant) # Density is not loss of localisation.
|
||||
assert meta["target_points"] == 100_001
|
||||
hint[0, 3] = 150
|
||||
with pytest.raises(ValueError, match="покрытие"):
|
||||
reference_window(reference, sample, hint)
|
||||
unchanged, _ = reference_window(local, sample, np.eye(4))
|
||||
assert unchanged is local
|
||||
|
||||
|
||||
def test_long_path_does_not_freeze_tail_or_multiply_distance():
|
||||
buffer = LiveCloudBuffer([[0, 0, 0], [150, 0, 0]])
|
||||
for i in range(2100):
|
||||
buffer.ingest(event("pose", t=1 + i * 0.1, p=(i * 0.06, 0, 0)))
|
||||
assert buffer.distance == pytest.approx(2099 * 0.06)
|
||||
assert len(buffer.path) <= 2000
|
||||
np.testing.assert_allclose(buffer.path[0], [0, 0, 0])
|
||||
np.testing.assert_allclose(buffer.path[-1], [2099 * 0.06, 0, 0])
|
||||
|
||||
|
||||
@pytest.mark.parametrize("length", [20, 50, 100, 200, 300])
|
||||
def test_actual_live_loop_stops_at_selected_distance_not_forty(tmp_path, monkeypatch, length):
|
||||
import k1link.missions.stationary_live as module
|
||||
|
||||
# Isolate termination from the separately tested geometric state machine.
|
||||
class Bootstrap:
|
||||
def __init__(self, path, **_kwargs):
|
||||
self.phase, self.reason = "collecting", "fixture"
|
||||
self.tracking_established = False
|
||||
self.candidate_trial = 0
|
||||
self.candidate_index = None
|
||||
self.retry_route_search = False
|
||||
self.gate = SimpleNamespace(state="acquiring", reason="fixture", streak=0, matrix=None)
|
||||
self.origin = 0
|
||||
|
||||
def tick(self, *args):
|
||||
pass
|
||||
|
||||
def ingest(self, *args):
|
||||
pass
|
||||
|
||||
def start_search(self, *args):
|
||||
return None
|
||||
|
||||
def validation(self, *args):
|
||||
return None
|
||||
|
||||
def stop(self, reason):
|
||||
self.phase, self.reason = "lost", reason
|
||||
|
||||
monkeypatch.setattr(module, "StationaryBootstrap", Bootstrap)
|
||||
service, source, lock, draft = fixture_service(tmp_path, monkeypatch)
|
||||
import k1link.missions.live_tests as live_module
|
||||
|
||||
clock = [100.0]
|
||||
monkeypatch.setattr(
|
||||
live_module,
|
||||
"time",
|
||||
SimpleNamespace(monotonic=lambda: clock[0], monotonic_ns=lambda: int(clock[0] * 1e9)),
|
||||
)
|
||||
draft["route"]["length_m"] = length
|
||||
run = service.start("draft", 1)
|
||||
try:
|
||||
until(lambda: service.get()["state"] == "waiting")
|
||||
assert run["maximum_distance_m"] == length
|
||||
clock[0] = 4000.0 # Neither waiting nor an active run has an implicit time cap.
|
||||
source.state.update(active=True, session_id="B", session_generation=2)
|
||||
source.queue.put(event("pose", t=1))
|
||||
source.queue.put(
|
||||
event("points", t=1.001, points=np.random.default_rng(2).normal(size=(1500, 3)))
|
||||
)
|
||||
until(lambda: service.get().get("planning_phase") == "collecting")
|
||||
clock[0] = 8000.0
|
||||
# Queue up to just before the chosen boundary, then prove it remains live.
|
||||
for i in range(1, length):
|
||||
source.queue.put(event("pose", t=1 + i, p=(i, 0, 0), sequence=i + 2))
|
||||
until(source.queue.empty)
|
||||
assert service.get()["state"] == "running"
|
||||
source.queue.put(event("pose", t=1 + length, p=(length, 0, 0), sequence=length + 2))
|
||||
until(lambda: service.get()["state"] == "completed")
|
||||
assert service.get()["termination_reason"] == "distance-limit"
|
||||
assert service.get()["distance_m"] == length
|
||||
assert service.accepted_sample is None
|
||||
assert source.state["active"] is True # Calculation never stops capture.
|
||||
finally:
|
||||
service.close()
|
||||
assert not lock.locked() and source.owner is None
|
||||
|
||||
|
||||
def test_invalid_admission_does_not_take_compute_or_stream_lease(tmp_path, monkeypatch):
|
||||
service, source, lock, draft = fixture_service(tmp_path, monkeypatch)
|
||||
draft["route"]["length_m"] = 2
|
||||
with pytest.raises(ValueError, match="3"):
|
||||
service.start("draft", 1)
|
||||
assert source.owner is None and not lock.locked() and service.run is None
|
||||
|
||||
|
||||
def test_long_reference_preparation_can_cancel_between_tiles():
|
||||
cancel = threading.Event()
|
||||
calls = []
|
||||
|
||||
def submap(*args):
|
||||
calls.append(args)
|
||||
cancel.set()
|
||||
return np.zeros((300, 3)), {}
|
||||
|
||||
sources = SimpleNamespace(
|
||||
bound=lambda *a: dict(poses=[dict(distance_m=i) for i in range(500)]), submap=submap
|
||||
)
|
||||
with pytest.raises(InterruptedError):
|
||||
build_reference_map(sources, "A", "gen", 0, 300, cancel_event=cancel)
|
||||
assert len(calls) == 1
|
||||
|
||||
|
||||
def test_cancel_during_preparation_releases_leases_without_starting_live_loop(
|
||||
tmp_path, monkeypatch
|
||||
):
|
||||
service, source, lock, _ = fixture_service(tmp_path, monkeypatch)
|
||||
entered, release = threading.Event(), threading.Event()
|
||||
|
||||
def prepare(*args, cancel_event):
|
||||
entered.set()
|
||||
release.wait(3)
|
||||
assert cancel_event.is_set()
|
||||
raise InterruptedError("cancelled")
|
||||
|
||||
service.drafts.sources.reference_map = prepare
|
||||
run = service.start("draft", 1)
|
||||
try:
|
||||
until(entered.is_set)
|
||||
service.stop(run["id"])
|
||||
release.set()
|
||||
until(lambda: service.get()["state"] == "cancelled")
|
||||
finally:
|
||||
release.set()
|
||||
service.close()
|
||||
assert source.owner is None and not lock.locked()
|
||||
assert service.get()["query_session_id"] is None
|
||||
@@ -0,0 +1,263 @@
|
||||
"""Regression for 006: coverage, pose freshness and stable accepted-frame display."""
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from test_planning_live import event, fixture_service
|
||||
|
||||
from k1link.missions.live_presentation import LivePresentation, stored_alignment
|
||||
from k1link.missions.reference_map import build_reference_map, reference_intervals
|
||||
|
||||
|
||||
def sample(t=100.0, x=0.0, segment=0):
|
||||
hint = np.eye(4)
|
||||
hint[:2, :2] = [[0, -1], [1, 0]]
|
||||
return dict(
|
||||
points=np.array([[x, 1.0, 0], [x, 2.0, 0]]),
|
||||
path=np.array([[0.0, 0, 0], [x, 0, 0]]),
|
||||
hint=hint,
|
||||
monotonic_ns=int(t * 1e9),
|
||||
sequence=int(t * 10),
|
||||
segment=segment,
|
||||
events=[],
|
||||
)
|
||||
|
||||
|
||||
def fit():
|
||||
matrix = np.eye(4)
|
||||
matrix[:3, 3] = [3, -2, 1]
|
||||
return dict(
|
||||
status="candidate",
|
||||
T_reference_query=matrix.tolist(),
|
||||
matched_query_indices=[0],
|
||||
overlap=0.9,
|
||||
inlier_rmse_m=0.1,
|
||||
reasons=[],
|
||||
)
|
||||
|
||||
|
||||
def test_fresh_display_advances_without_reusing_correspondence_indices():
|
||||
view = LivePresentation()
|
||||
accepted = sample()
|
||||
view.accept(fit(), accepted)
|
||||
assert view.result["matched_query_indices"] == []
|
||||
latest = sample(t=101, x=3)
|
||||
view.advance(latest, accepted, int(101.1e9))
|
||||
assert view.sample is latest
|
||||
assert view.result["T_reference_query"] == fit()["T_reference_query"]
|
||||
for current, qualifier, now in [
|
||||
(sample(t=102, x=8), None, int(102.1e9)),
|
||||
(sample(t=102, x=8, segment=1), accepted, int(102.1e9)),
|
||||
(sample(t=99, x=8), accepted, int(100e9)),
|
||||
(sample(t=102, x=8), accepted, int(105e9)),
|
||||
(sample(t=109, x=8), accepted, int(109.1e9)),
|
||||
]:
|
||||
view.advance(current, qualifier, now)
|
||||
assert view.sample is latest
|
||||
|
||||
|
||||
def test_no_hint_before_fit_and_no_rotation_after_loss_or_completion(tmp_path, monkeypatch):
|
||||
import k1link.missions.live_tests as module
|
||||
|
||||
service, source, _, _ = fixture_service(tmp_path, monkeypatch)
|
||||
service.run = dict(
|
||||
id="test",
|
||||
state="running",
|
||||
query_session_id="B",
|
||||
query_generation=2,
|
||||
tracking_state="tracking",
|
||||
)
|
||||
service.reference = np.zeros((3, 3))
|
||||
service.reference_path = np.zeros((2, 3))
|
||||
service.source = source
|
||||
source.state.update(active=True, session_id="B", session_generation=2)
|
||||
now = [int(100.1e9)]
|
||||
monkeypatch.setattr(module, "time", SimpleNamespace(monotonic_ns=lambda: now[0]))
|
||||
monkeypatch.setattr(service, "persist", lambda: None)
|
||||
monkeypatch.setattr(module, "scene_bytes", lambda *a, **kw: (a[3], a[4], kw.get("evidence")))
|
||||
service.update_sample(sample(), now[0])
|
||||
assert service.scene("test") == (None, None, None)
|
||||
accepted = sample()
|
||||
service.commit_result(
|
||||
fit(),
|
||||
accepted,
|
||||
{"accepted": True},
|
||||
"tracking",
|
||||
phase="tracking",
|
||||
message="tracking",
|
||||
tracking_established=True,
|
||||
)
|
||||
now[0] = int(101.1e9)
|
||||
latest = sample(t=101, x=2)
|
||||
service.update_sample(latest, now[0])
|
||||
current, result, evidence = service.scene("test")
|
||||
assert current is latest and evidence[0] is accepted
|
||||
assert result["matched_query_indices"] == []
|
||||
assert service.get()["presentation_state"] == "live"
|
||||
# Renderer freshness, not green extrapolation, controls the current label.
|
||||
now[0] = int(103.2e9)
|
||||
assert service.get()["presentation_state"] == "historical"
|
||||
assert service.scene("test")[2] is None
|
||||
service.commit_result(
|
||||
{**fit(), "status": "rejected"},
|
||||
sample(t=104),
|
||||
{"accepted": False},
|
||||
"lost",
|
||||
phase="lost",
|
||||
message="lost",
|
||||
tracking_established=True,
|
||||
)
|
||||
service.update_sample(sample(t=105, x=9), int(105.1e9))
|
||||
assert service.scene("test")[0] is latest
|
||||
assert service.scene("test")[1]["T_reference_query"] == fit()["T_reference_query"]
|
||||
service.run["state"] = "completed"
|
||||
source.state["active"] = False
|
||||
assert service.scene("test")[0] is latest and service.scene("test")[2] is None
|
||||
|
||||
|
||||
def test_pose_metadata_retained_independently_of_point_snapshot(tmp_path, monkeypatch):
|
||||
from dataclasses import replace
|
||||
|
||||
service, _, _, _ = fixture_service(tmp_path, monkeypatch)
|
||||
pose = replace(event("pose", t=101, p=(4, 5, 6)), orientation_xyzw=(0, 0, 0.6, 0.8))
|
||||
service.observe_pose(pose)
|
||||
assert service.latest_pose["orientation_xyzw"] == [0, 0, 0.6, 0.8]
|
||||
assert service.latest_pose["position"] == [4, 5, 6]
|
||||
assert service.latest_pose["frame_id"] == "session/B"
|
||||
assert service.sample is None
|
||||
|
||||
|
||||
def test_renderer_never_applies_hint_or_colors_unchecked_latest_points(monkeypatch):
|
||||
import k1link.missions.live_scene as renderer
|
||||
|
||||
logged = {}
|
||||
|
||||
class Recording:
|
||||
def __init__(self, *a, **kw):
|
||||
pass
|
||||
|
||||
def binary_stream(self):
|
||||
return SimpleNamespace(read=lambda: b"RRF2")
|
||||
|
||||
def log(self, name, value, **kw):
|
||||
logged[name] = value
|
||||
|
||||
def flush(self):
|
||||
pass
|
||||
|
||||
def disconnect(self):
|
||||
pass
|
||||
|
||||
monkeypatch.setattr(renderer.rr, "RecordingStream", Recording)
|
||||
monkeypatch.setattr(renderer.rr, "Points3D", lambda p, **kw: (np.asarray(p), kw["colors"]))
|
||||
monkeypatch.setattr(renderer.rr, "LineStrips3D", lambda p, **kw: np.asarray(p))
|
||||
raw = sample(x=5)
|
||||
renderer.scene_bytes("test", raw["points"], raw["path"], raw, None)
|
||||
assert not isinstance(logged["world/query"], tuple)
|
||||
renderer.scene_bytes("test", raw["points"], raw["path"], raw, fit(), evidence=(sample(), fit()))
|
||||
np.testing.assert_allclose(logged["world/query"][0], raw["points"] + [3, -2, 1])
|
||||
assert not (logged["world/query"][1] == [154, 235, 75]).all(axis=1).any()
|
||||
np.testing.assert_allclose(logged["world/validated_query"][0], [[3, -1, 1]])
|
||||
assert (logged["world/validated_query"][1] == [154, 235, 75]).all()
|
||||
renderer.scene_bytes("test", raw["points"], raw["path"], raw, fit())
|
||||
assert not isinstance(logged["world/validated_query"], tuple)
|
||||
|
||||
|
||||
def seal(directory):
|
||||
return {
|
||||
str(p.relative_to(directory)): hashlib.sha256(p.read_bytes()).hexdigest()
|
||||
for p in directory.rglob("*")
|
||||
if p.is_file()
|
||||
}
|
||||
|
||||
|
||||
def test_saved_display_restores_exact_geometry_and_transform(tmp_path):
|
||||
view = LivePresentation()
|
||||
view.accept(fit(), sample())
|
||||
view.advance(sample(t=101, x=8), sample(), int(101.1e9))
|
||||
metadata = view.save(tmp_path)
|
||||
np.save(tmp_path / "reference.npy", np.zeros((3, 3)))
|
||||
doc = dict(presentation=metadata, artifacts=seal(tmp_path))
|
||||
_, restored, result = stored_alignment(tmp_path, doc)
|
||||
np.testing.assert_equal(restored["points"], view.sample["points"])
|
||||
assert result["T_reference_query"] == fit()["T_reference_query"]
|
||||
assert result["matched_query_indices"] == []
|
||||
(tmp_path / "aligned-preview.npz").write_bytes(b"corrupt")
|
||||
with pytest.raises(ValueError, match="целостности"):
|
||||
stored_alignment(tmp_path, doc)
|
||||
|
||||
|
||||
def test_legacy_last_rejected_fit_does_not_replace_accepted_alignment(tmp_path):
|
||||
for n, accepted in [(1, True), (2, False)]:
|
||||
d = tmp_path / f"step-{n:03d}"
|
||||
d.mkdir()
|
||||
r = fit()
|
||||
r["status"] = "candidate" if accepted else "rejected"
|
||||
r["T_reference_query"][0][3] = n * 3
|
||||
(d / "source.json").write_text(
|
||||
json.dumps(dict(sequence=n, query_path=[[0, 0, 0], [1, 0, 0]]))
|
||||
)
|
||||
(d / "decision.json").write_text(json.dumps(dict(temporal=dict(accepted=accepted))))
|
||||
(d / "registration-result.json").write_text(json.dumps(r))
|
||||
np.savez(d / "registration-input.npz", reference=np.zeros((3, 3)), query=np.ones((3, 3)))
|
||||
doc = dict(artifacts=seal(tmp_path), result_source_sequence=2)
|
||||
_, _, result = stored_alignment(tmp_path, doc)
|
||||
assert result["T_reference_query"][0][3] == 3
|
||||
(tmp_path / "step-001/decision.json").write_text(
|
||||
json.dumps(dict(temporal=dict(accepted=False)))
|
||||
)
|
||||
doc["artifacts"] = seal(tmp_path)
|
||||
assert stored_alignment(tmp_path, doc) is None
|
||||
|
||||
|
||||
def poses():
|
||||
return [dict(distance_m=i, position=[i, 0, 0]) for i in range(160)]
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"start,end,expected",
|
||||
[(0, 30, [(0, 40), (40, 50)]), (50, 90, [(30, 70), (70, 110)]), (140, 159, [(120, 159)])],
|
||||
)
|
||||
def test_context_is_independent_of_route_with_bounded_tiles(start, end, expected):
|
||||
assert reference_intervals(poses(), start, end) == expected
|
||||
|
||||
|
||||
def test_reference_context_rejects_bad_distances_but_not_long_route():
|
||||
bad = poses()
|
||||
bad[4]["distance_m"] = -1
|
||||
with pytest.raises(ValueError):
|
||||
reference_intervals(bad, 0, 30)
|
||||
assert reference_intervals(poses(), 0, 101)[-1][1] == 121
|
||||
|
||||
|
||||
def test_context_map_preserves_source_bound_tiles_and_deduplicates():
|
||||
calls = []
|
||||
|
||||
def extract(session, generation, a, b):
|
||||
calls.append((session, generation, a, b))
|
||||
return np.array([[0, 0, 0], [a, 1, 0], [b, 1, 0]], dtype=float), dict(
|
||||
session_id=session, generation=generation, start_index=a, end_index=b
|
||||
)
|
||||
|
||||
sources = SimpleNamespace(bound=lambda s, g: dict(poses=poses()), submap=extract)
|
||||
points, metadata = build_reference_map(sources, "A", "hash", 0, 30)
|
||||
assert calls == [("A", "hash", 0, 40), ("A", "hash", 40, 50)]
|
||||
assert metadata["route_interval"] == [0, 30] and metadata["map_interval"] == [0, 50]
|
||||
assert len(points) == 4 and len(metadata["tiles"]) == 2
|
||||
|
||||
|
||||
def test_context_does_not_hide_source_changes_between_tiles():
|
||||
calls = []
|
||||
|
||||
def extract(*args):
|
||||
calls.append(args)
|
||||
if len(calls) > 1:
|
||||
raise ValueError("source changed")
|
||||
return np.zeros((3, 3)), {}
|
||||
|
||||
sources = SimpleNamespace(bound=lambda *a: dict(poses=poses()), submap=extract)
|
||||
with pytest.raises(ValueError, match="source changed"):
|
||||
build_reference_map(sources, "A", "hash", 0, 30)
|
||||
@@ -0,0 +1,115 @@
|
||||
"""Intentional STOP is not stale localisation; no sockets or device commands."""
|
||||
|
||||
import json
|
||||
import threading
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from test_planning_live import event, fixture_service, until
|
||||
from test_xgrids_acquisition_lifecycle import (
|
||||
_install_real_prepared_stop_dispatch_fixture,
|
||||
service_with_fake_runtime,
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("late_error", [False, True])
|
||||
def test_stop_during_search_finishes_before_late_worker_without_publishing_loss(
|
||||
tmp_path, monkeypatch, late_error,
|
||||
):
|
||||
import k1link.missions.live_tests as module
|
||||
|
||||
service, source, lock, _ = fixture_service(tmp_path, monkeypatch)
|
||||
clock = [100.]
|
||||
entered, release = threading.Event(), threading.Event()
|
||||
monkeypatch.setattr(module, "time", SimpleNamespace(
|
||||
monotonic=lambda: clock[0], monotonic_ns=lambda: int(clock[0] * 1e9)))
|
||||
|
||||
def search(*args, **kwargs):
|
||||
entered.set()
|
||||
assert release.wait(5)
|
||||
if late_error:
|
||||
raise ValueError("late worker failure")
|
||||
return dict(status="candidate", T_reference_query=np.eye(4).tolist())
|
||||
|
||||
monkeypatch.setattr(module, "run_route_relocalization", search)
|
||||
run = service.start("draft", 1)
|
||||
points = np.random.default_rng(11).uniform([-1, -3, -1], [8, 3, 3], (1500, 3))
|
||||
try:
|
||||
until(lambda: service.get()["state"] == "waiting")
|
||||
source.state.update(active=True, session_id="B", session_generation=2)
|
||||
for i in range(20):
|
||||
source.queue.put(event("pose", t=100+i*.5, sequence=2*i+1))
|
||||
source.queue.put(event("points", t=100+i*.5+.001, sequence=2*i+2, points=points))
|
||||
until(source.queue.empty)
|
||||
clock[0] = 110.01
|
||||
until(entered.is_set)
|
||||
until(lambda: service.get()["planning_phase"] == "searching")
|
||||
source.state["spatial_stop_requested"] = True
|
||||
until(lambda: service.get()["planning_phase"] == "ended")
|
||||
assert service.thread.is_alive() # Fit ownership not released prematurely.
|
||||
assert service.accepted_sample is None
|
||||
release.set()
|
||||
until(lambda: not service.thread.is_alive())
|
||||
report = service.get()
|
||||
assert report["state"] == "completed"
|
||||
assert report["termination_reason"] == "spatial-stop-requested"
|
||||
assert report["result"] is None and report.get("initialization_result") is None
|
||||
assert not any(t["phase"] == "lost" for t in report["phase_transitions"])
|
||||
assert source.state["active"] and not lock.locked()
|
||||
decision = json.loads((service.directory(run["id"]) / "step-001/decision.json").read_text())
|
||||
assert not decision["temporal"]["accepted"]
|
||||
assert decision["temporal"]["reason"] == (
|
||||
"calculation-unavailable" if late_error else "spatial-stop-requested")
|
||||
finally:
|
||||
release.set()
|
||||
service.close()
|
||||
|
||||
|
||||
def test_stop_before_first_cloud_keeps_capture_running(tmp_path, monkeypatch):
|
||||
service, source, lock, _ = fixture_service(tmp_path, monkeypatch)
|
||||
service.start("draft", 1)
|
||||
try:
|
||||
until(lambda: service.get()["state"] == "waiting")
|
||||
source.state.update(active=True, session_id="B", session_generation=2,
|
||||
spatial_stop_requested=True)
|
||||
source.queue.put(event("session-start"))
|
||||
until(lambda: not service.thread.is_alive())
|
||||
assert service.get()["termination_reason"] == "spatial-stop-requested"
|
||||
assert source.state["active"] and not lock.locked()
|
||||
finally:
|
||||
service.close()
|
||||
|
||||
|
||||
def test_scene_bounds_are_cached_per_immutable_reference(tmp_path, monkeypatch):
|
||||
service, _, _, _ = fixture_service(tmp_path, monkeypatch)
|
||||
service.reference = np.array([[0., 0., -2.], [1., 1., 4.]])
|
||||
first = service.scene_height_bounds()
|
||||
assert first == (-2., 80.)
|
||||
assert service.scene_height_bounds() is first
|
||||
service.scene_reference = np.array([[0., 0., -4.], [1., 1., np.nan]])
|
||||
assert service.scene_height_bounds() == (-4., 80.)
|
||||
service.scene_reference = np.empty((0, 3))
|
||||
assert service.scene_height_bounds() == (None, None)
|
||||
service.scene_reference = service.reference = None
|
||||
assert service.scene_height_bounds() == (None, None)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("rejected", [False, True])
|
||||
def test_only_admitted_device_stop_fences_planning(tmp_path, monkeypatch, rejected):
|
||||
device, runtime = service_with_fake_runtime(tmp_path)
|
||||
fixture = _install_real_prepared_stop_dispatch_fixture(device, runtime)
|
||||
ingress = device.live_perception_ingress
|
||||
assert ingress.snapshot()["active"]
|
||||
assert not ingress.snapshot()["spatial_stop_requested"]
|
||||
if rejected:
|
||||
def reject(**kwargs):
|
||||
raise ValueError("fixture rejected before dispatch")
|
||||
monkeypatch.setattr(fixture.control, "request_stop", reject)
|
||||
with pytest.raises(ValueError, match="fixture rejected"):
|
||||
device.stop_acquisition(fixture.request)
|
||||
else:
|
||||
device.stop_acquisition(fixture.request)
|
||||
assert ingress.snapshot()["active"] # Recording finalisation is independent.
|
||||
assert ingress.snapshot()["spatial_stop_requested"] is not rejected
|
||||
assert runtime.stop_calls == 0
|
||||
@@ -0,0 +1,122 @@
|
||||
"""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"
|
||||
@@ -0,0 +1,139 @@
|
||||
"""Exact local lookups and one verified preparation snapshot, not a load test."""
|
||||
|
||||
import json
|
||||
import threading
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
from test_session_recording import _command
|
||||
|
||||
from k1link.missions.reference_map import build_reference_map
|
||||
from k1link.missions.reference_window import ReferenceWindowIndex, reference_window
|
||||
from k1link.missions.sources import PlanningSources
|
||||
|
||||
|
||||
@pytest.mark.parametrize("center", [[0, 0, 0], [30, 5, -10], [-25, -20, 5]])
|
||||
def test_index_preserves_exact_crop_order_duplicates_and_boundary(center):
|
||||
rng = np.random.default_rng(601)
|
||||
center = np.array(center, dtype=float)
|
||||
near = rng.normal(center, 4, (1500, 3))
|
||||
boundary = center + np.array([[10, 0, 0], [-10, 0, 0], [0, 0, 10]])
|
||||
reference = np.vstack([near, boundary, near[:30], near + 150])
|
||||
sample = dict(path=np.array([center]), points=np.array([center]))
|
||||
expected, before = reference_window(reference, sample, np.eye(4))
|
||||
index = ReferenceWindowIndex(reference)
|
||||
actual, after = reference_window(reference, sample, np.eye(4), index=index)
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
assert before["target_sha256"] == after["target_sha256"]
|
||||
assert after["examined_points"] < after["map_points"]
|
||||
with pytest.raises(ValueError, match="another map"):
|
||||
reference_window(reference.copy(), sample, np.eye(4), index=index)
|
||||
|
||||
|
||||
def test_empty_index_and_broad_crop_are_exact():
|
||||
index = ReferenceWindowIndex(np.empty((0, 3)))
|
||||
assert index.crop(np.zeros(3), 10)[1] == 0
|
||||
points = np.random.default_rng(4).normal(size=(400, 3))
|
||||
index = ReferenceWindowIndex(points)
|
||||
assert index.crop(np.zeros(3), 1000)[0] is points
|
||||
with pytest.raises(ValueError):
|
||||
index.crop(np.zeros(3), float("nan"))
|
||||
|
||||
|
||||
def source_fixture(tmp_path, extractor):
|
||||
command = _command(tmp_path / "source")
|
||||
detail = SimpleNamespace(
|
||||
plugin_id=command.plugin_id,
|
||||
summary=SimpleNamespace(replayable=True, lab=None),
|
||||
as_dict=lambda: {"display_name": "fixture"},
|
||||
)
|
||||
store = SimpleNamespace(
|
||||
data_dir=tmp_path / "data", get_session=lambda _: detail, prepare_replay=lambda _: command
|
||||
)
|
||||
|
||||
def export(_source, dest):
|
||||
dest.write_text(json.dumps(dict(poses=[dict(distance_m=i) for i in range(101)])))
|
||||
|
||||
sources = PlanningSources(store, {command.plugin_id: export}, {command.plugin_id: extractor})
|
||||
doc = sources.get(command.session_id)
|
||||
return sources, command, doc["generation"]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("mode", ["success", "corrupt-source", "extractor-error", "cancel"])
|
||||
def test_preparation_stages_once_checks_source_at_exit_and_cleans_up(tmp_path, monkeypatch, mode):
|
||||
import k1link.missions.sources as module
|
||||
|
||||
paths = []
|
||||
cancel = threading.Event()
|
||||
|
||||
def extract(path, doc, first, last):
|
||||
paths.append(path)
|
||||
if mode == "corrupt-source" and len(paths) == 2:
|
||||
target = command.primary_artifact.path
|
||||
target.write_bytes(b"X" * target.stat().st_size)
|
||||
if mode == "extractor-error":
|
||||
raise ValueError("fixture extraction failed")
|
||||
if mode == "cancel":
|
||||
cancel.set()
|
||||
return np.array([[first, 0, 0], [last, 0, 0]], dtype=float), {"interval": [first, last]}
|
||||
|
||||
sources, command, generation = source_fixture(tmp_path, extract)
|
||||
stages, validations = [], []
|
||||
stage = module._stage_replay_prefix
|
||||
verify = sources.verify
|
||||
|
||||
def counted_stage(*args, **kwargs):
|
||||
result = stage(*args, **kwargs)
|
||||
stages.append(result[0])
|
||||
return result
|
||||
|
||||
def counted_verify(*args):
|
||||
validations.append(args)
|
||||
return verify(*args)
|
||||
|
||||
monkeypatch.setattr(module, "_stage_replay_prefix", counted_stage)
|
||||
monkeypatch.setattr(sources, "verify", counted_verify)
|
||||
if mode == "success":
|
||||
points, meta = build_reference_map(sources, command.session_id, generation, 0, 100)
|
||||
np.testing.assert_array_equal(points[:, 0], [0, 40, 80, 100])
|
||||
assert len(meta["tiles"]) == 3 and len(validations) == 2
|
||||
else:
|
||||
with pytest.raises((ValueError, InterruptedError)):
|
||||
build_reference_map(
|
||||
sources, command.session_id, generation, 0, 100, cancel_event=cancel
|
||||
)
|
||||
assert len(stages) == 1 and not stages[0].exists()
|
||||
assert len(set(paths)) == 1
|
||||
assert not list(sources.root.glob(".source.*"))
|
||||
|
||||
|
||||
def test_single_submap_retains_verified_api(tmp_path):
|
||||
def extract(_path, _doc, first, last):
|
||||
return np.ones((300, 3)), {"interval": [first, last]}
|
||||
|
||||
sources, command, generation = source_fixture(tmp_path, extract)
|
||||
points, meta = sources.submap(command.session_id, generation, 1, 5)
|
||||
assert len(points) == 300 and meta["interval"] == [1, 5]
|
||||
assert meta["generation"] == generation
|
||||
assert not list(sources.root.glob(".source.*"))
|
||||
|
||||
|
||||
def test_cancellation_during_copy_remains_cancellation_not_a_product_error(tmp_path, monkeypatch):
|
||||
import k1link.missions.sources as module
|
||||
|
||||
def unused(*args):
|
||||
raise AssertionError("Cancelled snapshot must not reach the extractor.")
|
||||
|
||||
sources, command, generation = source_fixture(tmp_path, unused)
|
||||
cancel = threading.Event()
|
||||
stage = module._stage_replay_prefix
|
||||
|
||||
def cancel_during_stage(*args, **kwargs):
|
||||
cancel.set()
|
||||
return stage(*args, **kwargs)
|
||||
|
||||
monkeypatch.setattr(module, "_stage_replay_prefix", cancel_during_stage)
|
||||
with pytest.raises(InterruptedError, match="cancelled"):
|
||||
build_reference_map(sources, command.session_id, generation, 0, 100, cancel_event=cancel)
|
||||
assert not list(sources.root.glob(".source.*"))
|
||||
@@ -0,0 +1,321 @@
|
||||
"""Selected-route retrieval is distinct from the old start-neighbourhood search."""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from k1link.missions.route_relocalization import (
|
||||
ROUTE_RELOCALIZATION_POLICY,
|
||||
ReferenceGrid,
|
||||
choose_route_location,
|
||||
local_submap,
|
||||
rank_route_candidates,
|
||||
relocalize_route,
|
||||
relocalize_start_then_route,
|
||||
)
|
||||
from k1link.missions.stationary_bootstrap import BOOTSTRAP_POLICY, StationaryBootstrap
|
||||
|
||||
|
||||
def scene():
|
||||
"""Three deliberately different local places along one 90-m route."""
|
||||
rng = np.random.default_rng(92)
|
||||
parts = []
|
||||
for center, scale in (
|
||||
(10.0, (2.0, 0.7, 1.0)),
|
||||
(45.0, (0.8, 4.0, 2.0)),
|
||||
(80.0, (4.0, 1.2, 0.4)),
|
||||
):
|
||||
points = rng.normal(size=(900, 3)) * scale
|
||||
points[:, 0] += center
|
||||
parts.append(points)
|
||||
return np.concatenate(parts), np.array([[0.0, 0.0, 0.0], [90.0, 0.0, 0.0]])
|
||||
|
||||
|
||||
def candidate_result(matrix, *, overlap=0.95, rmse=0.1):
|
||||
return dict(
|
||||
status="candidate",
|
||||
T_reference_query=np.asarray(matrix).tolist(),
|
||||
initial_T_reference_query=np.asarray(matrix).tolist(),
|
||||
matched_query_indices=[1],
|
||||
overlap=overlap,
|
||||
inlier_rmse_m=rmse,
|
||||
registration_seconds=0.01,
|
||||
localization_confirmed=False,
|
||||
vehicle_control=False,
|
||||
)
|
||||
|
||||
|
||||
def attempt(index, progress, matrix, *, overlap=0.95, rmse=0.1):
|
||||
return dict(
|
||||
candidate=dict(
|
||||
index=index,
|
||||
position=[progress, 0, 0],
|
||||
progress_m=float(progress),
|
||||
descriptor_distance=0.1,
|
||||
),
|
||||
yaw_deg=0.0,
|
||||
result=candidate_result(matrix, overlap=overlap, rmse=rmse),
|
||||
)
|
||||
|
||||
|
||||
def test_descriptor_retrieval_covers_full_selected_route_not_its_start():
|
||||
reference, path = scene()
|
||||
# The stationary K1 cloud is at the middle place in another local SLAM frame.
|
||||
query = reference[(reference[:, 0] > 37) & (reference[:, 0] < 53)] - [45.0, 3.0, 0.0]
|
||||
ranked, coverage = rank_route_candidates(reference, path, query)
|
||||
|
||||
assert coverage["descriptor_scope"] == "entire-selected-route"
|
||||
assert coverage["route_anchor_count"] > 10
|
||||
assert ranked
|
||||
assert len(ranked) == coverage["descriptor_covered_anchor_count"]
|
||||
assert len(ranked) > ROUTE_RELOCALIZATION_POLICY["candidate_batch_size"]
|
||||
assert abs(ranked[0].progress_m - 45.0) <= ROUTE_RELOCALIZATION_POLICY["anchor_spacing_m"]
|
||||
|
||||
|
||||
def test_full_route_atlas_is_not_rejected_by_one_gicp_target_cap():
|
||||
reference, _ = scene()
|
||||
full_route = np.tile(reference, (40, 1)) # 108,000 map points, not one GICP target.
|
||||
atlas = ReferenceGrid(full_route)
|
||||
target = local_submap(atlas, [45.0, 0.0, 0.0], 28.0, maximum_points=100_000)
|
||||
|
||||
assert len(full_route) > 100_000
|
||||
assert 300 <= len(target) <= 100_000
|
||||
|
||||
|
||||
def test_dense_eighty_metre_submap_keeps_its_footprint_under_the_gicp_budget():
|
||||
x, y = np.meshgrid(np.arange(-40.0, 40.0, 0.2), np.arange(-40.0, 40.0, 0.2))
|
||||
reference = np.column_stack((x.ravel(), y.ravel(), np.zeros(x.size)))
|
||||
target = local_submap(reference, [0.0, 0.0, 0.0], 80.0, maximum_points=1_000)
|
||||
|
||||
assert 300 <= len(target) <= 1_000
|
||||
|
||||
|
||||
def test_middle_of_route_relocalisation_qualifies_with_real_gicp():
|
||||
pytest.importorskip("small_gicp")
|
||||
reference, path = scene()
|
||||
query = reference[(reference[:, 0] > 37) & (reference[:, 0] < 53)] - [45.0, 3.0, 0.0]
|
||||
output = relocalize_route(reference, path, query, [0.0, 0.0, 0.0])
|
||||
|
||||
assert output["status"] == "candidate", output["reasons"]
|
||||
assert output["initialization"]["complete"]
|
||||
assert abs(output["initialization"]["selected_route_progress_m"] - 45.0) <= 5.0
|
||||
assert output["overlap"] > 0.9 and output["inlier_rmse_m"] < 0.05
|
||||
assert not output["localization_confirmed"] and not output["vehicle_control"]
|
||||
|
||||
|
||||
def test_hybrid_accepts_the_dense_start_before_route_retrieval(monkeypatch):
|
||||
"""A normal start must not be degraded by global target voxelisation."""
|
||||
import k1link.missions.route_relocalization as module
|
||||
|
||||
reference, path = scene()
|
||||
query = reference[:700]
|
||||
local = candidate_result(np.eye(4))
|
||||
local["initialization"] = dict(complete=True, reason=None, attempts=[{}] * 108)
|
||||
monkeypatch.setattr(module, "acquire_entry", lambda *args, **kwargs: dict(local))
|
||||
|
||||
def unexpected_route(*args, **kwargs):
|
||||
raise AssertionError("A qualified dense start must not enter route recovery.")
|
||||
|
||||
monkeypatch.setattr(module, "relocalize_route", unexpected_route)
|
||||
output = relocalize_start_then_route(reference, path, query, [0.0, 0.0, 0.0])
|
||||
|
||||
assert output["status"] == "candidate"
|
||||
assert output["initialization"]["strategy"] == "dense-start-first-then-route-recovery/v1"
|
||||
assert output["initialization"]["expected_attempts"] == 108
|
||||
assert output["initialization"]["stages"][0]["name"] == "dense-start"
|
||||
|
||||
|
||||
def test_hybrid_records_a_route_recovery_only_after_a_dense_start_rejection(monkeypatch):
|
||||
import k1link.missions.route_relocalization as module
|
||||
|
||||
reference, path = scene()
|
||||
query = reference[:700]
|
||||
local = candidate_result(np.eye(4))
|
||||
local.update(status="rejected", reasons=["no-admissible-entry"])
|
||||
local["initialization"] = dict(complete=True, reason="no-admissible-entry", attempts=[{}])
|
||||
recovered = candidate_result(np.eye(4))
|
||||
recovered["initialization"] = dict(
|
||||
complete=True,
|
||||
reason=None,
|
||||
attempts=[{}],
|
||||
selected_candidate_index=4,
|
||||
selected_route_progress_m=20.0,
|
||||
)
|
||||
monkeypatch.setattr(module, "acquire_entry", lambda *args, **kwargs: dict(local))
|
||||
monkeypatch.setattr(module, "relocalize_route", lambda *args, **kwargs: dict(recovered))
|
||||
|
||||
output = relocalize_start_then_route(reference, path, query, [0.0, 0.0, 0.0])
|
||||
|
||||
assert output["status"] == "candidate"
|
||||
assert [stage["name"] for stage in output["initialization"]["stages"]] == [
|
||||
"dense-start",
|
||||
"route-recovery",
|
||||
]
|
||||
assert output["initialization"]["expected_attempts"] == 2
|
||||
|
||||
|
||||
def test_equally_good_distinct_route_places_are_rejected_as_ambiguous():
|
||||
first, second = np.eye(4), np.eye(4)
|
||||
second[0, 3] = 50.0
|
||||
output = choose_route_location(
|
||||
[attempt(0, 0.0, first), attempt(10, 50.0, second, overlap=0.94, rmse=0.11)],
|
||||
[0.0, 0.0, 0.0],
|
||||
complete=True,
|
||||
)
|
||||
|
||||
assert output["status"] == "rejected"
|
||||
assert output["reasons"] == ["ambiguous-route-location"]
|
||||
assert output["matched_query_indices"] == []
|
||||
assert not output["localization_confirmed"] and not output["vehicle_control"]
|
||||
|
||||
|
||||
def test_no_qualified_candidate_is_a_route_location_failure_not_a_fake_prior():
|
||||
output = choose_route_location([], [0.0, 0.0, 0.0], complete=True)
|
||||
|
||||
assert output["status"] == "rejected"
|
||||
assert output["reasons"] == ["no-route-location"]
|
||||
assert output["initialization"]["complete"]
|
||||
assert output["initialization"]["expected_attempts"] == 0
|
||||
|
||||
|
||||
def test_global_result_must_account_for_every_generated_hypothesis_before_prior():
|
||||
boot = StationaryBootstrap(
|
||||
np.array([[0.0, 0, 0], [40.0, 0, 0]]),
|
||||
initialization_policy=ROUTE_RELOCALIZATION_POLICY,
|
||||
)
|
||||
# Exercise the policy contract directly; no old 108-seed local-start
|
||||
# assumption is permitted for whole-route retrieval.
|
||||
initialization = dict(
|
||||
policy=ROUTE_RELOCALIZATION_POLICY,
|
||||
scope="selected-route",
|
||||
complete=True,
|
||||
expected_attempts=2,
|
||||
attempts=[{}, {}],
|
||||
)
|
||||
boot.phase = "searching"
|
||||
boot.initialization_sample = dict(monotonic_ns=1, segment=0)
|
||||
boot.search_started_ns = 1
|
||||
accepted = boot.offer_prior(
|
||||
dict(
|
||||
status="candidate",
|
||||
T_reference_query=np.eye(4).tolist(),
|
||||
initialization=initialization,
|
||||
),
|
||||
1_000_000_000,
|
||||
0,
|
||||
)
|
||||
assert accepted["provisional"]
|
||||
|
||||
second = StationaryBootstrap(
|
||||
np.array([[0.0, 0, 0], [40.0, 0, 0]]),
|
||||
initialization_policy=ROUTE_RELOCALIZATION_POLICY,
|
||||
)
|
||||
second.phase = "searching"
|
||||
second.initialization_sample = dict(monotonic_ns=1, segment=0)
|
||||
second.search_started_ns = 1
|
||||
initialization["expected_attempts"] = 3
|
||||
rejected = second.offer_prior(
|
||||
dict(
|
||||
status="candidate",
|
||||
T_reference_query=np.eye(4).tolist(),
|
||||
initialization=initialization,
|
||||
),
|
||||
1_000_000_000,
|
||||
0,
|
||||
)
|
||||
assert not rejected["provisional"] and rejected["reason"] == "initialization-incomplete"
|
||||
|
||||
|
||||
def test_global_ambiguity_has_a_specific_operator_message():
|
||||
from k1link.missions.stationary_live import phase_message
|
||||
|
||||
boot = StationaryBootstrap(
|
||||
np.array([[0.0, 0, 0], [40.0, 0, 0]]),
|
||||
initialization_policy=ROUTE_RELOCALIZATION_POLICY,
|
||||
)
|
||||
boot.phase, boot.reason = "lost", "initialization-ambiguous"
|
||||
assert "несколько похожих участков" in phase_message(boot)
|
||||
|
||||
|
||||
def test_route_search_deadline_cannot_outlive_the_stationary_prefix_freshness_fence():
|
||||
assert (
|
||||
ROUTE_RELOCALIZATION_POLICY["deadline_s"] < BOOTSTRAP_POLICY["maximum_prior_source_age_s"]
|
||||
)
|
||||
assert (
|
||||
ROUTE_RELOCALIZATION_POLICY["maximum_search_wall_s"]
|
||||
<= BOOTSTRAP_POLICY["maximum_prior_source_age_s"]
|
||||
)
|
||||
|
||||
|
||||
def test_precise_search_reaches_the_last_ranked_anchor(monkeypatch):
|
||||
import k1link.missions.route_relocalization as module
|
||||
|
||||
reference, path = scene()
|
||||
query = reference[:700]
|
||||
ranked, _ = rank_route_candidates(reference, path, query)
|
||||
calls = []
|
||||
prepared_targets = []
|
||||
original_prepare = module.PreparedReference.__init__
|
||||
|
||||
def prepare(self, target):
|
||||
prepared_targets.append(target)
|
||||
original_prepare(self, target)
|
||||
|
||||
def register(self, query, initial, *, policy):
|
||||
calls.append(policy)
|
||||
result = candidate_result(np.eye(4))
|
||||
if len(calls) < (len(ranked) - 1) * 3 + 1:
|
||||
result.update(status="rejected", reasons=["fixture mismatch"])
|
||||
return result
|
||||
|
||||
monkeypatch.setattr(module.PreparedReference, "register", register)
|
||||
monkeypatch.setattr(module.PreparedReference, "__init__", prepare)
|
||||
result = relocalize_route(reference, path, query, [0, 0, 0])
|
||||
info = result["initialization"]
|
||||
assert result["status"] == "candidate"
|
||||
assert info["selected_candidate_index"] == ranked[-1].index
|
||||
assert len(calls) == info["expected_attempts"] == len(ranked) * 3
|
||||
assert len(prepared_targets) == len(ranked)
|
||||
assert not info["remaining_candidate_indices"]
|
||||
assert len(info["candidate_batches"]) > 1
|
||||
assert all(policy == ROUTE_RELOCALIZATION_POLICY["registration_policy"] for policy in calls)
|
||||
|
||||
|
||||
def test_budget_exhaustion_is_not_a_completed_negative_or_provisional_fit(monkeypatch):
|
||||
import k1link.missions.route_relocalization as module
|
||||
|
||||
reference, path = scene()
|
||||
elapsed = [0.0]
|
||||
|
||||
def register(self, query, initial, *, policy):
|
||||
elapsed[0] += 12
|
||||
return candidate_result(np.eye(4))
|
||||
|
||||
monkeypatch.setattr(module.PreparedReference, "register", register)
|
||||
result = relocalize_route(reference, path, reference[:700], [0, 0, 0],
|
||||
clock=lambda: elapsed[0])
|
||||
info = result["initialization"]
|
||||
assert result["reasons"] == ["incomplete-route-search"]
|
||||
assert not info["complete"] and not info["candidate_queue_exhausted"]
|
||||
assert info["remaining_candidate_indices"]
|
||||
assert info["expected_attempts"] > len(info["attempts"])
|
||||
assert info["candidate_queue"] == []
|
||||
|
||||
|
||||
def test_ambiguity_uses_fitted_places_even_when_retrieval_anchor_is_same():
|
||||
first, second = np.eye(4), np.eye(4)
|
||||
second[0, 3] = 40
|
||||
result = choose_route_location([attempt(0, 0, first), attempt(0, 0, second)],
|
||||
[0, 0, 0], complete=True)
|
||||
assert result["reasons"] == ["ambiguous-route-location"]
|
||||
|
||||
|
||||
def test_fresh_confirmation_fallback_skips_dense_search(monkeypatch):
|
||||
import k1link.missions.route_relocalization as module
|
||||
|
||||
reference, path = scene()
|
||||
def no_dense(*args, **kwargs):
|
||||
raise AssertionError("Already disproved dense-start prior must not repeat.")
|
||||
monkeypatch.setattr(module, "acquire_entry", no_dense)
|
||||
monkeypatch.setattr(module, "relocalize_route", lambda *a, **k: {"route_only": True})
|
||||
assert relocalize_start_then_route(reference, path, reference[:700], [0, 0, 0],
|
||||
route_only=True) == {"route_only": True}
|
||||
@@ -0,0 +1,124 @@
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
import hashlib
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
|
||||
import pytest
|
||||
from test_stream_summary import _write_capture, _pcl_payload, _pose_payload
|
||||
from test_session_recording import _command
|
||||
from k1link.device_plugins.xgrids_k1.session_overview import export_session_overview
|
||||
from k1link.sessions.overview import SessionOverviewService
|
||||
|
||||
|
||||
def test_overview_reads_geometry_without_fabricating_missing_timing(tmp_path):
|
||||
src = tmp_path / 'mqtt.raw.k1mqtt'
|
||||
_write_capture(src, [('lixel/application/report/lio_pcl', _pcl_payload(scaler=1000, point_count=4)),
|
||||
('lixel/application/report/lio_pose', _pose_payload((0, 0, 0))),
|
||||
('lixel/application/report/lio_pose', _pose_payload((3, 4, 0)))])
|
||||
digest = hashlib.sha256(src.read_bytes()).hexdigest()
|
||||
result = export_session_overview(src, tmp_path / 'scene.rrd')
|
||||
assert result['point_count'] == 4
|
||||
assert result['sample_points'] > 0
|
||||
assert result['path_m'] == 5
|
||||
assert result['chart'] == [] and result['mean_hz'] is None
|
||||
assert result['spatial_available']
|
||||
assert (tmp_path / 'scene.rrd').stat().st_size > 100
|
||||
assert hashlib.sha256(src.read_bytes()).hexdigest() == digest
|
||||
|
||||
|
||||
def test_overview_counts_corrupt_frames_and_cancels(tmp_path):
|
||||
src = tmp_path / 'mqtt.raw.k1mqtt'
|
||||
_write_capture(src, [('lixel/application/report/lio_pcl', b'bad')])
|
||||
assert export_session_overview(src, tmp_path / 'scene.rrd')['decode_errors'] == 1
|
||||
stop = threading.Event(); stop.set()
|
||||
with pytest.raises(RuntimeError, match='cancelled'):
|
||||
export_session_overview(src, tmp_path / 'unused.rrd', cancel_event=stop)
|
||||
|
||||
|
||||
def test_cache_is_single_flight_source_bound_and_reused_after_restart(tmp_path):
|
||||
command = _command(tmp_path / 'source')
|
||||
detail = SimpleNamespace(plugin_id=command.plugin_id, summary=SimpleNamespace(replayable=True, lab=None), as_dict=lambda: {'session_id': command.session_id})
|
||||
store = SimpleNamespace(data_dir=tmp_path / 'data', get_session=lambda _: detail, prepare_replay=lambda _: command)
|
||||
calls = []
|
||||
def exporter(source, destination, **kwargs):
|
||||
calls.append(source)
|
||||
destination.write_bytes(b'bounded-rrd')
|
||||
return {'point_count': 42}
|
||||
service = SessionOverviewService(store, {command.plugin_id: exporter})
|
||||
try:
|
||||
for _ in range(5): service.get(command.session_id)
|
||||
for _ in range(100):
|
||||
result = service.get(command.session_id)
|
||||
if result['state'] == 'ready': break
|
||||
time.sleep(.01)
|
||||
assert result['metrics']['point_count'] == 42
|
||||
assert len(calls) == 1
|
||||
assert service.scene(command.session_id, result['generation']).read_bytes() == b'bounded-rrd'
|
||||
with pytest.raises(ValueError): service.scene(command.session_id, '0'*64)
|
||||
finally: service.close()
|
||||
restored = SessionOverviewService(store, {command.plugin_id: exporter})
|
||||
try:
|
||||
assert restored.get(command.session_id)['state'] == 'ready'
|
||||
assert len(calls) == 1
|
||||
command.primary_artifact.path.write_bytes(b'X' * command.primary_artifact.file_byte_length)
|
||||
assert restored.get(command.session_id)['state'] in {'queued', 'preparing'}
|
||||
finally: restored.close()
|
||||
|
||||
|
||||
def test_lab_overview_does_not_leak_unbounded_parent_geometry(tmp_path):
|
||||
detail = SimpleNamespace(plugin_id='test', summary=SimpleNamespace(replayable=True, lab=object()), as_dict=lambda: {'session_id': 'derived'})
|
||||
store = SimpleNamespace(data_dir=tmp_path, get_session=lambda _: detail, prepare_replay=lambda _: pytest.fail('parent must not be opened'))
|
||||
service = SessionOverviewService(store, {'test': lambda *_: pytest.fail('not called')})
|
||||
try:
|
||||
result = service.get('derived')
|
||||
assert result['metrics'] is None and result['scene_url'] is None
|
||||
finally: service.close()
|
||||
|
||||
|
||||
def test_height_slice_is_reversible_and_only_replaces_display_points(tmp_path):
|
||||
import rerun as rr
|
||||
import numpy as np
|
||||
from rerun.experimental import RrdReader
|
||||
from k1link.sessions.overview_spatial import spatial_metadata, render_spatial_update
|
||||
source = tmp_path / 'overview.rrd'
|
||||
recording = rr.RecordingStream('missioncore_session_overview')
|
||||
recording.save(source)
|
||||
recording.log('world/cloud', rr.Points3D([[0, 0, 0], [1, 0, 3], [2, 0, 8]], colors=[[20, 30, 40]] * 3), static=True)
|
||||
recording.flush(); recording.disconnect()
|
||||
digest = hashlib.sha256(source.read_bytes()).hexdigest()
|
||||
assert spatial_metadata(source) == {'height_min_m': 0, 'height_max_m': 8, 'sample_points': 3}
|
||||
for ceiling, expected in [(3, 2), (-1, 0), (None, 3)]:
|
||||
data, count, eye = render_spatial_update(source, ceiling, 'top')
|
||||
assert count == expected and eye['eyeUp'] == [0, 1, 0]
|
||||
assert eye['position'][2] > eye['lookTarget'][2]
|
||||
out = tmp_path / f'view-{expected}.rrd'; out.write_bytes(data)
|
||||
cloud = next(c for c in RrdReader(out).stream() if c.entity_path == '/world/cloud')
|
||||
points = cloud.to_record_batch().column('Points3D:positions')[0].values.values.to_numpy().reshape(-1, 3)
|
||||
assert len(points) == expected
|
||||
assert ceiling is None or np.all(points[:, 2] <= ceiling)
|
||||
assert RrdReader(out).recordings()[0].recording_id == RrdReader(source).recordings()[0].recording_id
|
||||
assert hashlib.sha256(source.read_bytes()).hexdigest() == digest
|
||||
|
||||
|
||||
def test_camera_presets_fit_an_elongated_survey_to_viewport_width():
|
||||
import numpy as np
|
||||
from k1link.sessions.overview_spatial import _camera_eye
|
||||
points = np.array([[x, y, z] for x in (-10, 10) for y in (-250, 250) for z in (0, 30)])
|
||||
top = _camera_eye(points, 'top', 2.5)
|
||||
assert np.allclose(top['eyeUp'], [-1, 0, 0])
|
||||
assert np.allclose(np.array(top['position'])[:2], top['lookTarget'][:2])
|
||||
assert top['position'][2] < _camera_eye(points, 'top', .7)['position'][2]
|
||||
for mode in ('top', '3d'):
|
||||
eye = _camera_eye(points, mode, 2.5)
|
||||
position, target, up = map(np.asarray, (eye['position'], eye['lookTarget'], eye['eyeUp']))
|
||||
forward = target - position; forward /= np.linalg.norm(forward)
|
||||
right = np.cross(forward, up); right /= np.linalg.norm(right)
|
||||
screen_up = np.cross(right, forward)
|
||||
relative = points - position
|
||||
depth = relative @ forward
|
||||
assert np.all(depth > 0)
|
||||
assert np.all(np.abs(relative @ right) < depth * np.tan(np.pi / 8) * 2.5)
|
||||
assert np.all(np.abs(relative @ screen_up) < depth * np.tan(np.pi / 8))
|
||||
@@ -0,0 +1,304 @@
|
||||
"""Temporal authority, source identity and fresh-data fences for initialization."""
|
||||
|
||||
from dataclasses import replace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from k1link.missions.stationary_bootstrap import StationaryBootstrap
|
||||
from k1link.missions.stationary_entry import STATIONARY_POLICY
|
||||
from k1link.sessions.live_planning import PlanningLiveEvent
|
||||
|
||||
PATH = np.array([[0.0, 0, 0], [40.0, 0, 0]])
|
||||
POINTS = np.random.default_rng(36).uniform([-4, -4, -1], [4, 4, 3], (1800, 3))
|
||||
|
||||
|
||||
def event(t, seq, *, kind="pose", **kw):
|
||||
return PlanningLiveEvent(
|
||||
"B",
|
||||
1,
|
||||
seq,
|
||||
int((t + 1) * 1e9),
|
||||
1,
|
||||
kind,
|
||||
points=POINTS if kind == "points" else None,
|
||||
position=(0.0, 0, 0),
|
||||
**kw,
|
||||
)
|
||||
|
||||
|
||||
def fit():
|
||||
return dict(status="candidate", T_reference_query=np.eye(4).tolist())
|
||||
|
||||
|
||||
def initialized():
|
||||
boot = StationaryBootstrap(PATH)
|
||||
for i in range(20):
|
||||
boot.ingest(event(i * 0.5, i * 2 + 1), 0)
|
||||
boot.ingest(event(i * 0.5 + 0.001, i * 2 + 2, kind="points"), 0)
|
||||
assert boot.start_search(10_999_999_999) is None
|
||||
assert boot.start_search(11_000_000_000) is not None
|
||||
result = dict(
|
||||
**fit(), initialization=dict(complete=True, policy=STATIONARY_POLICY, attempts=[{}] * 108)
|
||||
)
|
||||
return boot, result
|
||||
|
||||
|
||||
def fresh_window(boot, start, seq, segment=1):
|
||||
for i in range(7):
|
||||
boot.ingest(event(start + i * 0.5, seq + i * 2), segment)
|
||||
boot.ingest(event(start + i * 0.5 + 0.001, seq + i * 2 + 1, kind="points"), segment)
|
||||
return boot.validation(int((start + 4.1) * 1e9), 20.0)
|
||||
|
||||
|
||||
def test_slow_prior_never_counts_and_three_disjoint_fresh_windows_are_required():
|
||||
boot, result = initialized()
|
||||
ready = 36_000_000_000
|
||||
temporal = boot.offer_prior(result, ready, 1)
|
||||
assert temporal["provisional"] and not temporal["accepted"]
|
||||
assert temporal["age_s"] > 8 and temporal["source_segment"] == 0
|
||||
assert boot.gate.matrix is None and boot.gate.streak == 0
|
||||
seen = set()
|
||||
for i, start in enumerate((36.0, 41.0, 46.0)):
|
||||
sample, seed = fresh_window(boot, start, 100 + 20 * i)
|
||||
assert np.allclose(seed, np.eye(4))
|
||||
ids = {e["sequence"] for e in sample["events"]}
|
||||
assert not ids & seen
|
||||
seen |= ids
|
||||
assert all(e["monotonic_ns"] > ready for e in sample["events"])
|
||||
stamp = sample["monotonic_ns"]
|
||||
temporal = boot.accept_fresh(fit(), sample, int((start + 4.2) * 1e9), 1)
|
||||
assert temporal["accepted"] and boot.gate.sample_ns == stamp
|
||||
assert boot.gate.streak == i + 1
|
||||
assert boot.phase == ("tracking" if i == 2 else "validating")
|
||||
assert boot.tracking_established
|
||||
boot.tick(boot.gate.sample_ns + 8_000_000_001, 1)
|
||||
assert boot.phase == "lost" and boot.gate.matrix is None and boot.prior is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"change,reason",
|
||||
[
|
||||
("rejected", "initialization-rejected"),
|
||||
("partial", "initialization-incomplete"),
|
||||
("wrong-policy", "initialization-incomplete"),
|
||||
("late", "initialization-expired"),
|
||||
("gaps", "too-many-receipt-gaps"),
|
||||
],
|
||||
)
|
||||
def test_bad_initialization_never_becomes_prior(change, reason):
|
||||
boot, result = initialized()
|
||||
now, segment = 36_000_000_000, 1
|
||||
if change == "rejected":
|
||||
result["status"] = "rejected"
|
||||
elif change == "partial":
|
||||
result["initialization"]["attempts"] = [{}] * 107
|
||||
elif change == "wrong-policy":
|
||||
result["initialization"]["policy"] = {**STATIONARY_POLICY, "version": "other"}
|
||||
elif change == "late":
|
||||
now = 41_000_000_001
|
||||
else:
|
||||
segment = 2
|
||||
temporal = boot.offer_prior(result, now, segment)
|
||||
assert temporal["reason"] == reason and not temporal["provisional"]
|
||||
assert boot.gate.matrix is None and boot.prior is None
|
||||
|
||||
|
||||
def test_receipts_from_before_ready_cannot_validate_and_silence_expires_prior():
|
||||
boot, result = initialized()
|
||||
boot.offer_prior(result, 36_000_000_000, 0)
|
||||
for i in range(5):
|
||||
boot.ingest(event(34 + i * 0.1, 100 + i * 2), 0)
|
||||
boot.ingest(event(34 + i * 0.1 + 0.001, 101 + i * 2, kind="points"), 0)
|
||||
assert boot.validation(40_000_000_000, 0) is None
|
||||
assert boot.fresh.events == [] or not boot.fresh.events
|
||||
boot.tick(46_000_000_001, 0)
|
||||
assert boot.reason == "prior-expired" and boot.gate.matrix is None
|
||||
assert not boot.offer_prior(result, 47_000_000_000, 0)["provisional"]
|
||||
|
||||
|
||||
def test_fresh_rejection_consumes_prior_without_automatic_retry():
|
||||
boot, result = initialized()
|
||||
boot.offer_prior(result, 36_000_000_000, 1)
|
||||
sample, _ = fresh_window(boot, 36.0, 100)
|
||||
assert boot.prior is None
|
||||
temporal = boot.accept_fresh(dict(status="rejected"), sample, 40_200_000_000, 1)
|
||||
assert not temporal["accepted"] and boot.phase == "lost"
|
||||
assert boot.validation(41_000_000_000, 20) is None
|
||||
assert boot.start_search(41_000_000_000) is None
|
||||
|
||||
|
||||
def test_gap_while_fresh_fit_runs_or_input_end_cannot_restore_tracking():
|
||||
for reason in ("receipt-gap", "input-ended"):
|
||||
boot, result = initialized()
|
||||
boot.offer_prior(result, 36_000_000_000, 1)
|
||||
sample, _ = fresh_window(boot, 36.0, 100)
|
||||
if reason == "receipt-gap":
|
||||
boot.ingest(event(40.0, 120), 2)
|
||||
else:
|
||||
boot.stop(reason)
|
||||
temporal = boot.accept_fresh(fit(), sample, 41_100_000_000, boot.segment)
|
||||
assert not temporal["accepted"] and temporal["reason"] == reason
|
||||
assert boot.gate.matrix is None
|
||||
|
||||
|
||||
def test_identity_and_clock_regression_stop_the_chain():
|
||||
boot, _ = initialized()
|
||||
changed = replace(event(11.0, 100), generation=2)
|
||||
with pytest.raises(ValueError, match="generation"):
|
||||
boot.ingest(changed, 0)
|
||||
assert boot.reason == "identity-changed"
|
||||
boot, _ = initialized()
|
||||
with pytest.raises(ValueError, match="regressed"):
|
||||
boot.ingest(event(8.0, 100), 0)
|
||||
assert boot.reason == "source-order-changed"
|
||||
|
||||
|
||||
def test_fresh_data_fence_is_checked_again_at_completion():
|
||||
boot, result = initialized()
|
||||
boot.offer_prior(result, 36_000_000_000, 1)
|
||||
sample, _ = fresh_window(boot, 36.0, 100)
|
||||
sample["events"][0]["monotonic_ns"] = sample["fresh_floor_ns"]
|
||||
assert not boot.accept_fresh(fit(), sample, 40_200_000_000, 1)["accepted"]
|
||||
assert boot.reason == "pre-validation-data" and boot.gate.matrix is None
|
||||
|
||||
|
||||
def test_initial_failure_and_later_loss_have_distinct_operator_messages():
|
||||
from k1link.missions.stationary_live import phase_message
|
||||
boot, result = initialized()
|
||||
result.update(status="rejected", initialization={"reason": "incomplete-search"})
|
||||
rejected = boot.offer_prior(result, 36_000_000_000, 0)
|
||||
assert rejected["reason"] == "initialization-incomplete"
|
||||
assert not boot.tracking_established
|
||||
assert "Синхронизация маршрута не завершилась" in phase_message(boot)
|
||||
assert "Остановите устройство и запись" in phase_message(boot)
|
||||
assert "потеряна" not in phase_message(boot)
|
||||
boot.tracking_established = True
|
||||
boot.stop("stale")
|
||||
assert "Привязка потеряна" in phase_message(boot)
|
||||
|
||||
|
||||
def test_initial_geometric_rejection_tells_operator_that_route_did_not_sync():
|
||||
from k1link.missions.stationary_live import phase_message
|
||||
|
||||
boot, result = initialized()
|
||||
result.update(status="rejected", initialization={"reason": "no-admissible-entry"})
|
||||
rejected = boot.offer_prior(result, 36_000_000_000, 0)
|
||||
|
||||
assert rejected["reason"] == "initialization-rejected"
|
||||
message = phase_message(boot)
|
||||
assert "Синхронизация маршрута не выполнена" in message
|
||||
assert "Переинициализировать" in message
|
||||
assert "исследованного участка" in message
|
||||
|
||||
|
||||
def test_gap_straddling_ready_can_finish_before_first_fresh_cloud_only():
|
||||
boot, result = initialized()
|
||||
ready = 30_000_000_000
|
||||
assert boot.offer_prior(result, ready, 0)["provisional"]
|
||||
# First new receipt arrives after the pre-existing gap. Readiness has never
|
||||
# been granted, no query points have entered validation, and the one-shot
|
||||
# prior lifetime/fresh fence are not extended.
|
||||
boot.ingest(event(30, 100), 1)
|
||||
assert boot.phase == "refreshing" and boot.ready_ns == ready
|
||||
assert boot.floor_ns == ready and boot.gate.matrix is None
|
||||
sample, _ = fresh_window(boot, 31, 101, segment=1)
|
||||
assert sample["segment"] == 1
|
||||
assert all(e["monotonic_ns"] > ready for e in sample["events"])
|
||||
assert boot.accept_fresh(fit(), sample, 35_200_000_000, 1)["accepted"]
|
||||
assert boot.gate.streak == 1 and not boot.tracking_established
|
||||
boot.ingest(event(37, 150), 2)
|
||||
assert boot.phase == "lost" and boot.prior is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize("case", ["second-gap", "fresh-cloud", "expired"])
|
||||
def test_pre_validation_gap_never_extends_or_reuses_a_prior(case):
|
||||
boot, result = initialized()
|
||||
boot.offer_prior(result, 30_000_000_000, 0)
|
||||
first_segment = 0 if case == "fresh-cloud" else 1
|
||||
boot.ingest(event(30, 100), first_segment)
|
||||
if case == "expired":
|
||||
boot.tick(40_000_000_001, 1)
|
||||
assert boot.reason == "prior-expired"
|
||||
else:
|
||||
if case == "fresh-cloud":
|
||||
boot.ingest(event(30.001, 101, kind="points"), first_segment)
|
||||
boot.ingest(event(33, 102), first_segment + 1)
|
||||
assert boot.reason == "receipt-gap"
|
||||
assert boot.prior is None and boot.gate.matrix is None
|
||||
|
||||
|
||||
def queued_prior():
|
||||
boot, result = initialized()
|
||||
matrix = np.eye(4)
|
||||
matrix[0, 3] = 10
|
||||
result["initialization"]["candidate_queue"] = [
|
||||
dict(T_reference_query=np.eye(4).tolist(), candidate_index=0, ambiguous=False),
|
||||
dict(T_reference_query=matrix.tolist(), candidate_index=2, ambiguous=False),
|
||||
]
|
||||
return boot, result, matrix
|
||||
|
||||
|
||||
def test_next_distinct_candidate_requires_three_new_disjoint_windows():
|
||||
boot, result, matrix = queued_prior()
|
||||
boot.offer_prior(result, 25_000_000_000, 1)
|
||||
sample, _ = fresh_window(boot, 25.0, 100)
|
||||
rejected_ids = {e["sequence"] for e in sample["events"]}
|
||||
rejected = boot.accept_fresh(dict(status="rejected"), sample, 29_200_000_000, 1)
|
||||
assert not rejected["accepted"] and rejected["next_candidate_trial"] == 2
|
||||
assert boot.phase == "refreshing" and boot.gate.matrix is None
|
||||
assert boot.floor_ns == 29_200_000_000 and not boot.fresh.events
|
||||
assert boot.candidate_trial == 2 and not boot.tracking_established
|
||||
for i, start in enumerate((30.0, 35.0, 40.0)):
|
||||
fresh, seed = fresh_window(boot, start, 200 + i * 20)
|
||||
assert np.allclose(seed, matrix)
|
||||
assert not rejected_ids.intersection(e["sequence"] for e in fresh["events"])
|
||||
assert all(e["monotonic_ns"] > 29_200_000_000 for e in fresh["events"])
|
||||
boot.accept_fresh(dict(status="candidate", T_reference_query=matrix.tolist()),
|
||||
fresh, int((start + 4.2) * 1e9), 1)
|
||||
assert boot.gate.streak == i + 1
|
||||
assert boot.phase == "tracking" and not boot.candidate_queue
|
||||
|
||||
|
||||
@pytest.mark.parametrize("failure", ["expired", "gap", "ended", "ambiguous", "stale-result"])
|
||||
def test_candidate_queue_never_bypasses_freshness_identity_or_ambiguity(failure):
|
||||
boot, result, _ = queued_prior()
|
||||
ready = 45_000_000_000 if failure == "expired" else 25_000_000_000
|
||||
if failure == "ambiguous":
|
||||
result["initialization"]["candidate_queue"][1]["ambiguous"] = True
|
||||
# Use the route policy's existing 35 s search fence for the late prefix case.
|
||||
if failure == "expired":
|
||||
boot.search_started_ns += 5_000_000_000
|
||||
assert boot.offer_prior(result, ready, 1)["provisional"]
|
||||
start = ready / 1e9
|
||||
sample, _ = fresh_window(boot, start, 100)
|
||||
now = int((start + 4.2) * 1e9)
|
||||
if failure == "expired":
|
||||
now = 51_000_000_000
|
||||
elif failure == "gap":
|
||||
boot.ingest(event(start + 4, 120), 2)
|
||||
elif failure == "ended":
|
||||
boot.stop("input-ended")
|
||||
elif failure == "stale-result":
|
||||
now = sample["monotonic_ns"] + 9_000_000_000
|
||||
rejected = boot.accept_fresh(dict(status="rejected"), sample, now, boot.segment)
|
||||
assert not rejected["accepted"] and boot.phase == "lost"
|
||||
assert boot.prior is None and boot.gate.matrix is None and not boot.candidate_queue
|
||||
|
||||
|
||||
def test_dense_start_fresh_rejection_requests_new_prefix_then_route_search():
|
||||
boot, result = initialized()
|
||||
result["initialization"]["stages"] = [dict(name="dense-start")]
|
||||
boot.offer_prior(result, 25_000_000_000, 1)
|
||||
sample, _ = fresh_window(boot, 25.0, 100)
|
||||
boot.accept_fresh(dict(status="rejected"), sample, 29_200_000_000, 1)
|
||||
assert boot.phase == "lost" and boot.retry_route_search
|
||||
boot.stop("input-ended")
|
||||
assert not boot.retry_route_search
|
||||
|
||||
|
||||
def test_ambiguous_first_hypothesis_never_becomes_a_provisional_prior():
|
||||
boot, result, _ = queued_prior()
|
||||
result["initialization"]["candidate_queue"][0]["ambiguous"] = True
|
||||
assert not boot.offer_prior(result, 25_000_000_000, 1)["provisional"]
|
||||
assert boot.reason == "initialization-ambiguous" and boot.prior is None
|
||||
Reference in New Issue
Block a user