feat(simulation): drive stock rover through PX4 offboard

2026-07-24 21:47:40 +03:00
parent 53fd1e3bfc
commit 101707d54c
12 changed files with 1329 additions and 15 deletions
@@ -18,7 +18,11 @@ from k1link.simulation import (
    RunKind,
    RunState,
 )
-from k1link.web.polygon_api import StartStockRoverRequest, build_polygon_router
+from k1link.web.polygon_api import (
+    AckermannCommandRequest,
+    StartStockRoverRequest,
+    build_polygon_router,
+)

 SHA_A = "a" * 64
 SHA_B = "b" * 64
@@ -241,6 +245,35 @@ class _FakeWorkerGateway:
        self.active_run_id = None
        return self._status()

+    def command(
+        self,
+        *,
+        run_id: str,
+        speed_mps: float,
+        steering_normalized: float,
+        idempotency_key: str,
+    ) -> dict[str, Any]:
+        self.calls.append(("command", idempotency_key))
+        assert run_id == self.active_run_id
+        return {
+            "schema_version": "missioncore.command-acceptance/v1",
+            "run_id": run_id,
+            "command_id": "cmd-test",
+            "sequence": 1,
+            "issued_at_sim_ns": 1_000_000,
+            "valid_until_sim_ns": 251_000_000,
+            "speed_mps": speed_mps,
+            "steering_normalized": steering_normalized,
+            "authority_scope": "virtual-only",
+            "delivery": {
+                "provider": "px4-ros2-offboard",
+                "mode": "speed-steering",
+                "armed": True,
+                "offboard": True,
+                "ttl_expired_count": 0,
+            },
+        }
+
    def _status(self) -> dict[str, Any]:
        return {
            "schema_version": "missioncore.simulation-worker-status/v1",
@@ -297,12 +330,34 @@ def test_polygon_worker_api_fails_closed_then_proxies_virtual_only_lifecycle() -
    live = _endpoint(enabled, "/api/v1/polygon/worker/live", "GET")()
    assert live["run_id"] == running["active_run_id"]
    assert live["source"]["quality"] == "diagnostic"
+    command = _endpoint(
+        enabled,
+        "/api/v1/polygon/worker/runs/{run_id}/commands",
+        "POST",
+    )(
+        run_id=running["active_run_id"],
+        request=AckermannCommandRequest(speed_mps=1.0, steering_normalized=-0.25),
+        idempotency_key="command-001",
+    )
+    assert command["authority_scope"] == "virtual-only"
+    assert command["delivery"]["offboard"] is True
    stopped = _endpoint(enabled, "/api/v1/polygon/worker/runs/{run_id}/stop", "POST")(
        run_id=running["active_run_id"],
        idempotency_key="stop-001",
    )
    assert stopped["active_run_id"] is None
-    assert worker.calls == [("start", "start-001"), ("stop", "stop-001")]
+    assert worker.calls == [
+        ("start", "start-001"),
+        ("command", "command-001"),
+        ("stop", "stop-001"),
+    ]
+
+
+def test_polygon_worker_command_rejects_values_outside_lab_envelope() -> None:
+    with pytest.raises(ValueError):
+        AckermannCommandRequest(speed_mps=1.51, steering_normalized=0)
+    with pytest.raises(ValueError):
+        AckermannCommandRequest(speed_mps=0, steering_normalized=-1.01)


 def test_polygon_worker_status_is_explicitly_unavailable_when_not_registered() -> None:
@@ -0,0 +1,195 @@
+from __future__ import annotations
+
+import threading
+from math import isnan
+from pathlib import Path
+
+import pytest
+
+from k1link.simulation.contracts import (
+    AckermannControlSetpoint,
+    AuthorityProfile,
+    ProviderPin,
+    QualificationRun,
+    ReproducibilityTier,
+    RunKind,
+    RunState,
+)
+from k1link.simulation.px4_rover_control import (
+    Px4RoverControlSnapshot,
+    Ros2Px4AckermannControl,
+)
+from k1link.simulation.run_store import QualificationRunStore
+from k1link.simulation.worker_agent import SimulationWorkerAgent, WorkerAgentError
+
+
+class _Collector:
+    def collect(self, run_id: str) -> dict[str, object]:
+        return {"run_id": run_id, "sim_time_ns": 1_000_000_000}
+
+
+class _Controller:
+    def __init__(self) -> None:
+        self.commands: list[object] = []
+
+    def submit(self, command: AckermannControlSetpoint) -> Px4RoverControlSnapshot:
+        self.commands.append(command)
+        return Px4RoverControlSnapshot(
+            run_id=command.run_id,
+            armed=True,
+            offboard=True,
+            ttl_expired_count=0,
+        )
+
+    def snapshot(self, run_id: str) -> Px4RoverControlSnapshot:
+        return Px4RoverControlSnapshot(
+            run_id=run_id,
+            armed=True,
+            offboard=True,
+            ttl_expired_count=0,
+        )
+
+
+class _Message:
+    pass
+
+
+class _Messages:
+    OffboardControlMode = _Message
+    RoverSpeedSetpoint = _Message
+    RoverSteeringSetpoint = _Message
+    RoverAttitudeSetpoint = _Message
+    RoverRateSetpoint = _Message
+
+
+class _Publisher:
+    def __init__(self) -> None:
+        self.messages: list[_Message] = []
+
+    def publish(self, message: _Message) -> None:
+        self.messages.append(message)
+
+
+def _running_store(root: Path) -> QualificationRunStore:
+    store = QualificationRunStore(root)
+    admitted = store.create(
+        QualificationRun(
+            run_id="s1c-command-test",
+            episode_id="episode-s1c-command-test",
+            kind=RunKind.SIMULATION_CLOSED_LOOP,
+            state=RunState.ADMITTED,
+            scenario_generation="stock-rover",
+            scenario_sha256="a" * 64,
+            profile_generation="stock-rover-lifecycle-v1",
+            profile_sha256="b" * 64,
+            mission_core_commit="c" * 40,
+            providers=(
+                ProviderPin("px4-autopilot", "v1.17.0", "v1.17.0"),
+                ProviderPin("gazebo", "harmonic", "8.14.0"),
+            ),
+            host_profile_id="mission-gpu-s0",
+            host_profile_sha256="d" * 64,
+            seed=42,
+            reproducibility_tier=ReproducibilityTier.R1,
+            authority=AuthorityProfile(
+                generation=1,
+                command_ttl_max_ns=250_000_000,
+                heartbeat_timeout_monotonic_ns=500_000_000,
+            ),
+            clock_domain="gazebo:/clock",
+            created_at_utc="2026-07-24T18:00:00Z",
+        )
+    )
+    starting = store.transition(
+        admitted.run_id,
+        RunState.STARTING,
+        expected_revision=0,
+        observed_at_utc="2026-07-24T18:00:01Z",
+        host_monotonic_ns=1,
+    )
+    store.transition(
+        admitted.run_id,
+        RunState.RUNNING,
+        expected_revision=starting.revision,
+        observed_at_utc="2026-07-24T18:00:02Z",
+        host_monotonic_ns=2,
+        sim_time_ns=0,
+    )
+    return store
+
+
+def test_worker_agent_journals_and_idempotently_delivers_virtual_command(
+    tmp_path: Path,
+) -> None:
+    agent = object.__new__(SimulationWorkerAgent)
+    agent.store = _running_store(tmp_path)
+    agent.collector = _Collector()
+    controller = _Controller()
+    agent.command_controller = controller
+    agent._command_lock = threading.RLock()
+
+    accepted = agent.command(
+        run_id="s1c-command-test",
+        speed_mps=1.0,
+        steering_normalized=-0.25,
+        idempotency_key="browser-intent-001",
+    )
+    repeated = agent.command(
+        run_id="s1c-command-test",
+        speed_mps=1.0,
+        steering_normalized=-0.25,
+        idempotency_key="browser-intent-001",
+    )
+
+    assert accepted == repeated
+    assert accepted["authority_scope"] == "virtual-only"
+    assert accepted["delivery"]["provider"] == "px4-ros2-offboard"
+    assert accepted["delivery"]["armed"] is True
+    commands = agent.store.list_commands("s1c-command-test")
+    assert len(commands) == 1
+    assert commands[0].valid_until_sim_ns - commands[0].issued_at_sim_ns == 250_000_000
+    assert len(controller.commands) == 1
+
+
+def test_worker_agent_command_dispatch_fails_closed_on_unsafe_envelope() -> None:
+    agent = object.__new__(SimulationWorkerAgent)
+
+    with pytest.raises(WorkerAgentError, match="outside"):
+        agent.dispatch(
+            {
+                "schema_version": "missioncore.simulation-worker-request/v1",
+                "request_id": "request-001",
+                "operation": "command",
+                "payload": {
+                    "run_id": "s1c-command-test",
+                    "speed_mps": 1.51,
+                    "steering_normalized": 0.0,
+                    "idempotency_key": "browser-intent-002",
+                },
+            }
+        )
+
+
+def test_px4_adapter_publishes_speed_steering_without_direct_actuators() -> None:
+    publishers = [_Publisher() for _ in range(5)]
+
+    Ros2Px4AckermannControl._publish_setpoint(
+        _Messages,
+        *publishers,
+        123,
+        1.0,
+        -0.55,
+    )
+
+    offboard = publishers[0].messages[0]
+    speed = publishers[1].messages[0]
+    steering = publishers[2].messages[0]
+    attitude = publishers[3].messages[0]
+    rate = publishers[4].messages[0]
+    assert offboard.velocity is True
+    assert offboard.direct_actuator is False
+    assert speed.speed_body_x == 1.0
+    assert isnan(speed.speed_body_y)
+    assert steering.normalized_steering_setpoint == -0.55
+    assert isnan(attitude.yaw_setpoint)
+    assert isnan(rate.yaw_rate_setpoint)
@@ -42,6 +42,27 @@ def _status() -> dict[str, Any]:
    }


+def _command_acceptance() -> dict[str, Any]:
+    return {
+        "schema_version": "missioncore.command-acceptance/v1",
+        "run_id": "s1c-command-test",
+        "command_id": "cmd-001",
+        "sequence": 1,
+        "issued_at_sim_ns": 1_000_000,
+        "valid_until_sim_ns": 251_000_000,
+        "speed_mps": 1.0,
+        "steering_normalized": -0.25,
+        "authority_scope": "virtual-only",
+        "delivery": {
+            "provider": "px4-ros2-offboard",
+            "mode": "speed-steering",
+            "armed": True,
+            "offboard": True,
+            "ttl_expired_count": 0,
+        },
+    }
+
+
 def _serve_once(
    socket_path: Path,
    result: dict[str, Any],
@@ -121,3 +142,29 @@ def test_unix_worker_gateway_reports_missing_worker_without_path_disclosure(
    with pytest.raises(SimulationWorkerUnavailableError) as failure:
        gateway.status()
    assert str(tmp_path) not in str(failure.value)
+
+
+def test_unix_worker_gateway_sends_bounded_virtual_rover_command() -> None:
+    socket_path = Path(f"/tmp/mc-{uuid4().hex}.sock")
+    thread, requests = _serve_once(socket_path, _command_acceptance())
+    gateway = UnixSocketWorkerGateway(socket_path)
+
+    try:
+        accepted = gateway.command(
+            run_id="s1c-command-test",
+            speed_mps=1.0,
+            steering_normalized=-0.25,
+            idempotency_key="command-001",
+        )
+        thread.join(timeout=2)
+
+        assert accepted["delivery"]["offboard"] is True
+        assert requests[0]["operation"] == "command"
+        assert requests[0]["payload"] == {
+            "run_id": "s1c-command-test",
+            "speed_mps": 1.0,
+            "steering_normalized": -0.25,
+            "idempotency_key": "command-001",
+        }
+    finally:
+        socket_path.unlink(missing_ok=True)