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

Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
This commit is contained in:
DCCONSTRUCTIONS
2026-09-21 08:47:19 +03:00
parent be58d589e2
commit e515ab1b8c
189 changed files with 19074 additions and 758 deletions
+160
View File
@@ -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"))
+158
View File
@@ -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)
+4
View File
@@ -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)
+27 -6
View File
@@ -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")
+24
View File
@@ -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:
+110
View File
@@ -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)
+114
View File
@@ -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()
+64
View File
@@ -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
)
+98
View File
@@ -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
+247
View File
@@ -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)
+312
View File
@@ -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
+441
View File
@@ -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
+196
View File
@@ -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
+136
View File
@@ -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()
+187
View File
@@ -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
+263
View File
@@ -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)
+115
View File
@@ -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
+122
View File
@@ -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"
+139
View File
@@ -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.*"))
+321
View File
@@ -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}
+124
View File
@@ -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))
+304
View File
@@ -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