feat(simulation): add Worker AI polygon runtime and terrain navigation

2026-09-25 16:40:45 +03:00
parent a7c64e009d
commit f01bd39037
88 changed files with 9918 additions and 108 deletions
@@ -0,0 +1,303 @@
+"""Lifecycle and clock-isolation checks; GPU/stream performance is qualified on Worker."""
+
+import importlib.util
+import threading
+import time
+from pathlib import Path
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+from pydantic import ValidationError
+from test_ai_polygon import make_world, sample
+from test_observatory_recorded_jobs import _definitions
+
+from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue
+from k1link.simulation.ai_polygon.contracts import (
+    RealtimeSnapshot,
+    RunCreate,
+    StreamEndpoint,
+    WorkerHello,
+)
+from k1link.simulation.ai_polygon.runs import RunStore
+from k1link.simulation.ai_polygon.worlds import WorldStore
+
+
+@pytest.fixture
+def realtime(tmp_path):
+    queue = ObservatoryRecordedJobQueue(tmp_path, definitions=_definitions())
+    store = RunStore(WorldStore(tmp_path), queue)
+    hello = WorkerHello(
+        worker_id="worker-006",
+        instance_id="a" * 32,
+        runtime="isaac-sim-6.1",
+        model_ids=["reference"],
+        profile_sha256="b" * 64,
+        runtime_sources={key: "c" * 64 for key in ("worker", "scene", "models", "robot")},
+        execution_modes=["realtime"],
+        stream=StreamEndpoint(server="100.80.1.2"),
+    )
+    store.register(hello)
+    world = make_world(store.worlds)
+    row = store.start(
+        RunCreate(world_id=world["world_id"], clock="realtime", start_paused=True),
+        "test-realtime-01",
+    )
+    return store, hello, row
+
+
+def snapshot(**changes):
+    fields = dict(
+        sequence=0,
+        control_sequence=0,
+        state="ready",
+        phase="running",
+        simulation_time_ns=0,
+        wall_elapsed_seconds=1,
+        physics_steps=0,
+        render_frames=30,
+        sensor_frames=0,
+        inference_count=0,
+        dropped_frames=0,
+        rtf=0,
+        render_fps=30,
+        sensor_fps=0,
+        ai_hz=0,
+        pose_xy=(0, 0),
+        pose_yaw=0,
+        speed_mps=0,
+        applied_speed_mps=0,
+        applied_yaw_rate_rps=0,
+        stop_reason="paused",
+        ai_ready=False,
+        stream_ready=True,
+        camera="follow",
+    )
+    return RealtimeSnapshot(**{**fields, **changes})
+
+
+def test_worker_owns_acknowledgement_and_no_frame_archive_on_core(realtime):
+    store, hello, row = realtime
+    key = row["run_id"]
+    assert not (store.directory(key) / "frames").exists()
+    assert store.poll(hello.instance_id, key)["run"]["state"] == "starting"
+    store.snapshot(key, hello.instance_id, snapshot())
+    command = store.control(key, "play")
+    assert command["state"] == "ready"
+    assert store.control(key, "play")["control_sequence"] == command["control_sequence"]
+    assert store.poll(hello.instance_id, key)["run"]["state"] == "ready"
+    store.snapshot(
+        key,
+        hello.instance_id,
+        snapshot(sequence=1, control_sequence=1, state="running", simulation_time_ns=66666667),
+    )
+    assert store.get(key)["state"] == "running"
+    with pytest.raises(RuntimeError):
+        store.sample(key, hello.instance_id, sample())
+    with pytest.raises(RuntimeError):
+        store.control(key, "step")
+
+
+def test_disconnect_restart_reconcile_never_releases_gpu_or_replays(realtime):
+    store, hello, row = realtime
+    key = row["run_id"]
+    store.snapshot(key, hello.instance_id, snapshot())
+    store.seen -= 21
+    assert store.status()["active_run"]["state"] == "disconnected"
+    with pytest.raises(RuntimeError):
+        store.queue.reserve_simulation("airun-" + "d" * 32)
+    recovered = RunStore(store.worlds, store.queue)
+    with pytest.raises(RuntimeError):
+        recovered.register(hello.model_copy(update={"instance_id": "d" * 32}))
+    recovered.register(hello)
+    assert recovered.poll(hello.instance_id, key)["action"] == "pause"
+    recovered.snapshot(key, hello.instance_id, snapshot(sequence=3))
+    assert recovered.get(key)["state"] == "ready"
+    recovered.control(key, "stop")
+    recovered.snapshot(key, hello.instance_id, snapshot(sequence=4))
+    assert recovered.get(key)["state"] == "stopping"
+    recovered.finish(key, hello.instance_id, "stopped", "")
+    recovered.queue.reserve_simulation("airun-" + "d" * 32)
+
+
+def test_snapshot_is_idempotent_and_rejects_unissued_ack(realtime):
+    store, hello, row = realtime
+    key = row["run_id"]
+    first = snapshot(sequence=5, simulation_time_ns=1000)
+    assert store.snapshot(key, hello.instance_id, first) == store.snapshot(
+        key, hello.instance_id, first
+    )
+    with pytest.raises(ValueError):
+        store.snapshot(key, hello.instance_id, snapshot(sequence=6, control_sequence=2))
+    with pytest.raises(ValueError):
+        store.snapshot(key, hello.instance_id, snapshot(sequence=6, simulation_time_ns=999))
+    with pytest.raises(ValidationError):
+        StreamEndpoint(server="8.8.8.8")
+
+
+def test_slow_ai_is_bounded_and_pause_fences_inflight_result(tmp_path):
+    path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py"
+    spec = importlib.util.spec_from_file_location("polygon_realtime_ai", path)
+    module = importlib.util.module_from_spec(spec)
+    spec.loader.exec_module(module)
+    entered, release = threading.Event(), threading.Event()
+    seen = []
+
+    class Model:
+        def ready(self):
+            pass
+
+        def close(self):
+            pass
+
+        def infer(self, rgb):
+            seen.append(int(rgb[0, 0, 0]))
+            entered.set()
+            assert release.wait(2)
+            return None, []
+
+    decision = {
+        "speed_mps": 0.3,
+        "yaw_rate_rps": 0,
+        "reason": "road",
+        "road_fraction": 0.7,
+        "obstacle_count": 0,
+    }
+    ai = module.LatestInference(
+        Model,
+        SimpleNamespace(
+            reset=lambda: None, decide=lambda *_: SimpleNamespace(model_dump=lambda: decision)
+        ),
+        tmp_path,
+    )
+    ai.start()
+    try:
+        ai.enable(True)
+        ai.submit(np.zeros((2, 2, 3), dtype=np.uint8), 0, time.monotonic(), 0)
+        assert entered.wait(2)
+        for frame_id in range(1, 11):
+            ai.submit(
+                np.full((2, 2, 3), frame_id, dtype=np.uint8),
+                frame_id,
+                time.monotonic(),
+                frame_id * 1000,
+            )
+        assert ai.dropped == 9
+        assert ai.command(time.monotonic())[0] == 0
+        ai.enable(False)
+        release.set()
+        ai.close()
+        assert seen == [0]
+        assert ai.result is None
+    finally:
+        release.set()
+        ai.close()
+
+
+def test_expired_source_stops_even_if_inference_just_completed(tmp_path):
+    path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py"
+    spec = importlib.util.spec_from_file_location("polygon_command_deadline", path)
+    module = importlib.util.module_from_spec(spec)
+    spec.loader.exec_module(module)
+    ai = module.LatestInference(None, None, tmp_path)
+    ai.enabled = True
+    ai.result = {
+        "captured_at": 0.0,
+        "completed_at": 1.0,
+        "decision": {"speed_mps": 0.3, "yaw_rate_rps": 0},
+    }
+    assert ai.command(1.1)[:3] == (0.0, 0.0, "stale-camera")
+    ai.result["captured_at"] = 1.0
+    assert ai.command(1.1)[:3] == (0.3, 0, "none")
+    ai.result["decision"]["speed_mps"] = -0.1
+    assert ai.command(1.1)[:3] == (-0.1, 0, "none")
+    assert ai.command(1.6)[:2] == (0, 0)
+    # The worker's 0.8 s camera budget still rejects old images, even when
+    # a result has just arrived, and never extends the command watchdog.
+    ai.result.update(captured_at=1.0, completed_at=1.7)
+    assert ai.command(1.79, frame_deadline=0.8)[:3] == (-0.1, 0, "none")
+    assert ai.command(1.81, frame_deadline=0.8)[:3] == (0.0, 0.0, "stale-camera")
+    ai.result.update(captured_at=1.7, completed_at=1.0)
+    assert ai.command(1.79, frame_deadline=0.8)[:2] == (0.0, 0.0)
+
+
+def test_reverse_telemetry_keeps_the_run_speed_limit(realtime):
+    store, hello, row = realtime
+    store.snapshot(row["run_id"], hello.instance_id, snapshot(applied_speed_mps=-0.1))
+    assert store.get(row["run_id"])["telemetry"]["applied_speed_mps"] == -0.1
+    with pytest.raises(ValueError, match="скорость"):
+        store.snapshot(
+            row["run_id"], hello.instance_id, snapshot(sequence=1, applied_speed_mps=-0.4)
+        )
+
+
+def test_resume_requires_fresh_clear_observations(tmp_path):
+    from k1link.simulation.ai_polygon.policy import RoadPolicy
+
+    path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py"
+    spec = importlib.util.spec_from_file_location("polygon_resume", path)
+    module = importlib.util.module_from_spec(spec)
+    spec.loader.exec_module(module)
+
+    class Model:
+        def ready(self):
+            pass
+
+        def close(self):
+            pass
+
+        def infer(self, rgb):
+            return np.ones((512, 512), dtype=bool), []
+
+    ai = module.LatestInference(Model, RoadPolicy(0.3), tmp_path)
+    ai.start()
+
+    def frame(number):
+        ai.submit(np.zeros((2, 2, 3), dtype=np.uint8), number, time.monotonic(), number)
+        deadline = time.monotonic() + 2
+        while ai.count < number and time.monotonic() < deadline:
+            time.sleep(0.001)
+        assert ai.count == number
+        return ai.command(time.monotonic())[0]
+
+    try:
+        ai.enable(True)
+        assert frame(1) == frame(2) == 0
+        assert frame(3) > 0
+        ai.enable(False)
+        ai.enable(True)
+        assert frame(4) == frame(5) == 0
+        assert frame(6) > 0
+    finally:
+        ai.close()
+
+
+def test_windows_snapshot_sharing_retry_is_bounded(tmp_path, monkeypatch):
+    path = Path(__file__).parents[1] / "simulation/ai-polygon/local_state.py"
+    spec = importlib.util.spec_from_file_location("polygon_ipc", path)
+    module = importlib.util.module_from_spec(spec)
+    spec.loader.exec_module(module)
+    original = module.os.replace
+    attempts = []
+
+    def transient(source, target):
+        attempts.append(1)
+        if len(attempts) <= 2:
+            raise PermissionError("Windows sharing violation")
+        original(source, target)
+
+    monkeypatch.setattr(module.os, "replace", transient)
+    monkeypatch.setattr(module.time, "sleep", lambda _: None)
+    target = tmp_path / "snapshot.json"
+    module.write_json(target, {"sequence": 3})
+    assert module.read_json(target) == {"sequence": 3}
+    assert len(attempts) == 3
+    attempts.clear()
+
+    def permanent():
+        attempts.append(1)
+        raise PermissionError("Not a transient lock")
+
+    with pytest.raises(PermissionError):
+        module.sharing_retry(permanent)
+    assert len(attempts) == 8