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.
2026-09-21 08:47:19 +03:00
parent be58d589e2
commit e515ab1b8c
189 changed files with 19074 additions and 758 deletions
@@ -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