DCCONSTRUCTIONS
e515ab1b8c
Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
188 lines
7.2 KiB
Python
188 lines
7.2 KiB
Python
"""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
|