From 101707d54c4afbb38210a0d3f661584c59c9194c Mon Sep 17 00:00:00 2001 From: DCCONSTRUCTIONS Date: Fri, 24 Jul 2026 21:47:40 +0300 Subject: [PATCH] feat(simulation): drive stock rover through PX4 offboard --- .../src/core/polygon/liveWorker.ts | 134 +++++++ .../control-station/src/styles/workspaces.css | 38 ++ .../src/workspaces/PolygonLivePanel.tsx | 131 ++++++- .../test/polygonRunArchive.test.mjs | 50 ++- simulation/s1/worker-agent.sh | 14 +- src/k1link/simulation/px4_rover_control.py | 368 ++++++++++++++++++ src/k1link/simulation/worker_agent.py | 179 ++++++++- src/k1link/simulation/worker_gateway.py | 86 +++- src/k1link/web/polygon_api.py | 43 +- tests/test_polygon_api.py | 59 ++- tests/test_simulation_worker_agent_command.py | 195 ++++++++++ tests/test_simulation_worker_gateway.py | 47 +++ 12 files changed, 1329 insertions(+), 15 deletions(-) create mode 100644 src/k1link/simulation/px4_rover_control.py create mode 100644 tests/test_simulation_worker_agent_command.py diff --git a/apps/control-station/src/core/polygon/liveWorker.ts b/apps/control-station/src/core/polygon/liveWorker.ts index 122685b..d0c76ad 100644 --- a/apps/control-station/src/core/polygon/liveWorker.ts +++ b/apps/control-station/src/core/polygon/liveWorker.ts @@ -24,6 +24,19 @@ export interface PolygonVehicleState { orientation: { x: number; y: number; z: number; w: number }; } +export interface PolygonCommandAcceptance { + runId: string; + commandId: string; + sequence: number; + issuedAtSimNs: number; + validUntilSimNs: number; + speedMps: number; + steeringNormalized: number; + armed: boolean; + offboard: boolean; + ttlExpiredCount: number; +} + export class PolygonWorkerContractError extends Error {} export class PolygonWorkerApiError extends Error { @@ -89,6 +102,25 @@ const SAFETY_KEYS = new Set([ "actuator_authority", "navigation_or_safety_accepted", ]); +const COMMAND_KEYS = new Set([ + "schema_version", + "run_id", + "command_id", + "sequence", + "issued_at_sim_ns", + "valid_until_sim_ns", + "speed_mps", + "steering_normalized", + "authority_scope", + "delivery", +]); +const COMMAND_DELIVERY_KEYS = new Set([ + "provider", + "mode", + "armed", + "offboard", + "ttl_expired_count", +]); function isRecord(value: unknown): value is Record { return typeof value === "object" && value !== null && !Array.isArray(value); @@ -266,6 +298,59 @@ export function decodePolygonVehicleState(payload: unknown): PolygonVehicleState }; } +export function decodePolygonCommandAcceptance(payload: unknown): PolygonCommandAcceptance { + const value = record(payload, "Подтверждение команды"); + exactKeys(value, COMMAND_KEYS, "Подтверждение команды"); + if ( + value.schema_version !== "missioncore.command-acceptance/v1" || + value.authority_scope !== "virtual-only" + ) { + throw new PolygonWorkerContractError("Команда вышла за virtual-only контракт."); + } + const delivery = record(value.delivery, "delivery"); + exactKeys(delivery, COMMAND_DELIVERY_KEYS, "delivery"); + if ( + delivery.provider !== "px4-ros2-offboard" || + delivery.mode !== "speed-steering" + ) { + throw new PolygonWorkerContractError("Команда подтверждена неизвестным PX4 adapter."); + } + const sequence = integerValue(value.sequence, "sequence"); + const issuedAtSimNs = integerValue(value.issued_at_sim_ns, "issued_at_sim_ns"); + const validUntilSimNs = integerValue(value.valid_until_sim_ns, "valid_until_sim_ns"); + const speedMps = finiteValue(value.speed_mps, "speed_mps"); + const steeringNormalized = finiteValue( + value.steering_normalized, + "steering_normalized", + ); + if ( + sequence < 1 || + validUntilSimNs <= issuedAtSimNs || + validUntilSimNs - issuedAtSimNs > 250_000_000 || + speedMps < -1.5 || + speedMps > 1.5 || + steeringNormalized < -1 || + steeringNormalized > 1 + ) { + throw new PolygonWorkerContractError("Команда нарушает допустимый S1 envelope."); + } + return { + runId: safeId(value.run_id, "run_id"), + commandId: safeId(value.command_id, "command_id"), + sequence, + issuedAtSimNs, + validUntilSimNs, + speedMps, + steeringNormalized, + armed: booleanValue(delivery.armed, "delivery.armed"), + offboard: booleanValue(delivery.offboard, "delivery.offboard"), + ttlExpiredCount: integerValue( + delivery.ttl_expired_count, + "delivery.ttl_expired_count", + ), + }; +} + async function responseBody(response: Response): Promise { const contentType = response.headers.get("content-type") ?? ""; if (!contentType.toLowerCase().startsWith("application/json")) { @@ -391,3 +476,52 @@ export async function stopPolygonWorker( fetcher, )); } + +export async function sendPolygonRoverCommand( + runId: string, + { + speedMps, + steeringNormalized, + idempotencyKey, + signal, + fetcher = globalThis.fetch, + }: { + speedMps: number; + steeringNormalized: number; + idempotencyKey: string; + signal?: AbortSignal; + fetcher?: PolygonWorkerFetch; + }, +): Promise { + if (!SAFE_ID.test(runId)) { + throw new PolygonWorkerContractError("Некорректный идентификатор активного прогона."); + } + if ( + !Number.isFinite(speedMps) || + !Number.isFinite(steeringNormalized) || + speedMps < -1.5 || + speedMps > 1.5 || + steeringNormalized < -1 || + steeringNormalized > 1 + ) { + throw new PolygonWorkerContractError("Команда вышла за допустимый S1 envelope."); + } + return decodePolygonCommandAcceptance(await requestJson( + `/api/v1/polygon/worker/runs/${encodeURIComponent(runId)}/commands`, + { + method: "POST", + headers: { + Accept: "application/json", + "Content-Type": "application/json", + "Idempotency-Key": idempotencyKey, + }, + body: JSON.stringify({ + speed_mps: speedMps, + steering_normalized: steeringNormalized, + }), + signal, + }, + "Simulation Worker не подтвердил команду ровера.", + fetcher, + )); +} diff --git a/apps/control-station/src/styles/workspaces.css b/apps/control-station/src/styles/workspaces.css index 50439b9..4ed9b94 100644 --- a/apps/control-station/src/styles/workspaces.css +++ b/apps/control-station/src/styles/workspaces.css @@ -300,6 +300,44 @@ gap: 0.42rem; } +.polygon-live-telemetry > .polygon-rover-controls { + gap: 0.55rem; + border: 1px solid rgb(var(--nodedc-accent-rgb) / 0.24); + background: + radial-gradient(circle at 0 0, rgb(var(--nodedc-accent-rgb) / 0.1), transparent 58%), + rgb(255 255 255 / 0.03); +} + +.polygon-rover-controls__heading { + display: flex; + align-items: center; + justify-content: space-between; + gap: 0.5rem; +} + +.polygon-rover-controls__heading > span { + color: var(--nodedc-text-primary); + font-size: 0.62rem; + font-weight: 680; +} + +.polygon-rover-controls__grid { + display: grid; + grid-template-columns: repeat(2, minmax(0, 1fr)); + gap: 0.38rem; +} + +.polygon-rover-controls__grid > button:last-child { + grid-column: 1 / -1; +} + +.polygon-rover-controls small { + color: var(--nodedc-text-muted); + font-family: "SFMono-Regular", Consolas, "Liberation Mono", monospace; + font-size: 0.53rem; + line-height: 1.45; +} + .polygon-live-telemetry > div:not(.polygon-live-boundary) { display: grid; min-width: 0; diff --git a/apps/control-station/src/workspaces/PolygonLivePanel.tsx b/apps/control-station/src/workspaces/PolygonLivePanel.tsx index 1063276..de56a80 100644 --- a/apps/control-station/src/workspaces/PolygonLivePanel.tsx +++ b/apps/control-station/src/workspaces/PolygonLivePanel.tsx @@ -1,4 +1,4 @@ -import { lazy, Suspense, useEffect, useState } from "react"; +import { lazy, Suspense, useEffect, useRef, useState } from "react"; import { Button, GlassSurface, @@ -8,12 +8,28 @@ import { import { fetchPolygonVehicleState, fetchPolygonWorkerStatus, + sendPolygonRoverCommand, startPolygonWorker, stopPolygonWorker, + type PolygonCommandAcceptance, type PolygonVehicleState, type PolygonWorkerStatus, } from "../core/polygon/liveWorker"; +interface ControlIntent { + id: "forward" | "reverse" | "left" | "right"; + label: string; + speedMps: number; + steeringNormalized: number; +} + +const CONTROL_INTENTS: ControlIntent[] = [ + { id: "forward", label: "Вперёд", speedMps: 1, steeringNormalized: 0 }, + { id: "left", label: "Влево", speedMps: 0.8, steeringNormalized: -0.55 }, + { id: "right", label: "Вправо", speedMps: 0.8, steeringNormalized: 0.55 }, + { id: "reverse", label: "Назад", speedMps: -0.65, steeringNormalized: 0 }, +]; + const PolygonRoverScene = lazy(async () => { const module = await import("./PolygonRoverScene"); return { default: module.PolygonRoverScene }; @@ -30,7 +46,11 @@ export function PolygonLivePanel() { const [trajectory, setTrajectory] = useState([]); const [error, setError] = useState(null); const [actionBusy, setActionBusy] = useState(false); + const [controlIntent, setControlIntent] = useState(null); + const [commandAcceptance, setCommandAcceptance] = + useState(null); const [generation, setGeneration] = useState(0); + const commandInFlight = useRef(false); useEffect(() => { if (actionBusy) return; @@ -73,12 +93,75 @@ export function PolygonLivePanel() { const runActive = Boolean(worker?.activeRunId); + useEffect(() => { + const runId = worker?.activeRunId; + if (!runId || worker.runState !== "running" || !controlIntent) return; + const controller = new AbortController(); + let timer: number | null = null; + const publish = async () => { + if (commandInFlight.current || controller.signal.aborted) return; + commandInFlight.current = true; + try { + const accepted = await sendPolygonRoverCommand(runId, { + speedMps: controlIntent.speedMps, + steeringNormalized: controlIntent.steeringNormalized, + idempotencyKey: crypto.randomUUID(), + signal: controller.signal, + }); + if (controller.signal.aborted) return; + setCommandAcceptance(accepted); + setError(null); + } catch (commandError) { + if (controller.signal.aborted) return; + setControlIntent(null); + setError(message(commandError)); + } finally { + commandInFlight.current = false; + if (!controller.signal.aborted) { + timer = window.setTimeout(() => void publish(), 100); + } + } + }; + void publish(); + return () => { + controller.abort(); + if (timer !== null) window.clearTimeout(timer); + }; + }, [controlIntent, worker?.activeRunId, worker?.runState]); + + useEffect(() => { + if (!worker?.activeRunId) { + setControlIntent(null); + setCommandAcceptance(null); + } + }, [worker?.activeRunId]); + + const stopMotion = async () => { + const runId = worker?.activeRunId; + setControlIntent(null); + if (!runId || worker?.runState !== "running") return; + try { + const accepted = await sendPolygonRoverCommand(runId, { + speedMps: 0, + steeringNormalized: 0, + idempotencyKey: crypto.randomUUID(), + }); + setCommandAcceptance(accepted); + setError(null); + } catch (commandError) { + setError(message(commandError)); + } + }; + const runAction = async () => { if (!worker?.controlAvailable || actionBusy) return; setActionBusy(true); setError(null); try { const idempotencyKey = crypto.randomUUID(); + if (worker.activeRunId) { + await stopMotion(); + } const next = worker.activeRunId ? await stopPolygonWorker(worker.activeRunId, { idempotencyKey }) : await startPolygonWorker({ idempotencyKey }); @@ -136,6 +219,40 @@ export function PolygonLivePanel() {
+
+
+ Управление PX4 + + {commandAcceptance?.offboard ? "Armed · Offboard" : "Ожидает прогона"} + +
+
+ {CONTROL_INTENTS.map((intent) => ( + + ))} + +
+ + {controlIntent + ? `${controlIntent.label}: ${controlIntent.speedMps.toFixed(2)} m/s · steering ${controlIntent.steeringNormalized.toFixed(2)}` + : "Команда 250 мс; потеря браузера автоматически обнуляет скорость."} + +
Прогон {worker?.activeRunId ?? "—"} @@ -156,11 +273,19 @@ export function PolygonLivePanel() { Провайдеры {worker?.providerIds.join(" · ") || "—"}
+
+ Последняя команда + + {commandAcceptance + ? `#${commandAcceptance.sequence} · ${commandAcceptance.speedMps.toFixed(2)} m/s · ${commandAcceptance.steeringNormalized.toFixed(2)}` + : "—"} + +
Virtual only

- Нет actuator authority. Live-поза диагностическая и пока не доказывает - приёмку PX4/ROS 2 telemetry, navigation или safety. + Команды проходят PX4 ROS 2 Offboard только в SITL. Физического actuator + authority нет; live-поза остаётся диагностической.

{error &&

{error}

} diff --git a/apps/control-station/test/polygonRunArchive.test.mjs b/apps/control-station/test/polygonRunArchive.test.mjs index 3c1534f..b22d460 100644 --- a/apps/control-station/test/polygonRunArchive.test.mjs +++ b/apps/control-station/test/polygonRunArchive.test.mjs @@ -12,8 +12,10 @@ let resolvePolygonRunRoute; let PolygonRunContractError; let decodePolygonWorkerStatus; let decodePolygonVehicleState; +let decodePolygonCommandAcceptance; let fetchPolygonWorkerStatus; let fetchPolygonVehicleState; +let sendPolygonRoverCommand; let startPolygonWorker; let stopPolygonWorker; let PolygonWorkerContractError; @@ -38,8 +40,10 @@ before(async () => { ({ decodePolygonWorkerStatus, decodePolygonVehicleState, + decodePolygonCommandAcceptance, fetchPolygonWorkerStatus, fetchPolygonVehicleState, + sendPolygonRoverCommand, startPolygonWorker, stopPolygonWorker, PolygonWorkerContractError, @@ -223,6 +227,28 @@ function vehicleState(overrides = {}) { }; } +function commandAcceptance(overrides = {}) { + return { + schema_version: "missioncore.command-acceptance/v1", + run_id: "s1c-6cb1495-20260724t180000z-aabbcc", + command_id: "cmd-aabbcc", + sequence: 1, + issued_at_sim_ns: 7_000_000_000, + valid_until_sim_ns: 7_250_000_000, + speed_mps: 1, + steering_normalized: -0.55, + authority_scope: "virtual-only", + delivery: { + provider: "px4-ros2-offboard", + mode: "speed-steering", + armed: true, + offboard: true, + ttl_expired_count: 0, + }, + ...overrides, + }; +} + test("Polygon is a worker-gated product root and keeps a compatible direct route", () => { assert.deepEqual(resolvePolygonRunRoute("?workspace=polygon-run"), { active: true, @@ -321,11 +347,14 @@ test("Polygon fetchers use bounded same-origin GET endpoints", async () => { test("Polygon Live decodes only D-only virtual diagnostic state", () => { const status = decodePolygonWorkerStatus(workerStatus()); const live = decodePolygonVehicleState(vehicleState()); + const command = decodePolygonCommandAcceptance(commandAcceptance()); assert.equal(status.available, true); assert.equal(status.activeRunId, live.runId); assert.equal(status.isolation.artifactPolicy, "d-only"); assert.deepEqual(live.position, { x: 1.25, y: -0.5, z: 0.18 }); + assert.equal(command.offboard, true); + assert.equal(command.steeringNormalized, -0.55); assert.throws( () => decodePolygonWorkerStatus(workerStatus({ authority: { @@ -344,6 +373,12 @@ test("Polygon Live decodes only D-only virtual diagnostic state", () => { })), PolygonWorkerContractError, ); + assert.throws( + () => decodePolygonCommandAcceptance(commandAcceptance({ + authority_scope: "physical", + })), + PolygonWorkerContractError, + ); }); test("Polygon Live uses same-origin gateway and idempotent lifecycle requests", async () => { @@ -356,6 +391,7 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests", const fetcher = async (url, init) => { calls.push({ url: String(url), init }); if (String(url).endsWith("/live")) return jsonResponse(vehicleState()); + if (String(url).endsWith("/commands")) return jsonResponse(commandAcceptance()); if (String(url).endsWith("/stop")) return jsonResponse(stopped); return jsonResponse(workerStatus()); }; @@ -363,6 +399,12 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests", await fetchPolygonWorkerStatus({ fetcher }); await fetchPolygonVehicleState({ fetcher }); await startPolygonWorker({ idempotencyKey: "start-001", fetcher }); + await sendPolygonRoverCommand("s1c-6cb1495-20260724t180000z-aabbcc", { + speedMps: 1, + steeringNormalized: -0.55, + idempotencyKey: "command-001", + fetcher, + }); await stopPolygonWorker("s1c-6cb1495-20260724t180000z-aabbcc", { idempotencyKey: "stop-001", fetcher, @@ -372,9 +414,15 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests", "/api/v1/polygon/worker", "/api/v1/polygon/worker/live", "/api/v1/polygon/worker/runs", + "/api/v1/polygon/worker/runs/s1c-6cb1495-20260724t180000z-aabbcc/commands", "/api/v1/polygon/worker/runs/s1c-6cb1495-20260724t180000z-aabbcc/stop", ]); assert.equal(calls[2].init.headers["Idempotency-Key"], "start-001"); - assert.equal(calls[3].init.headers["Idempotency-Key"], "stop-001"); + assert.equal(calls[3].init.headers["Idempotency-Key"], "command-001"); + assert.equal(calls[4].init.headers["Idempotency-Key"], "stop-001"); assert.equal(calls[2].init.body, JSON.stringify({ scenario_id: "stock-rover-ackermann" })); + assert.equal(calls[3].init.body, JSON.stringify({ + speed_mps: 1, + steering_normalized: -0.55, + })); }); diff --git a/simulation/s1/worker-agent.sh b/simulation/s1/worker-agent.sh index 350124f..adb8e60 100755 --- a/simulation/s1/worker-agent.sh +++ b/simulation/s1/worker-agent.sh @@ -12,6 +12,8 @@ readonly SOURCE_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/../.." && pwd - readonly MISSION_CORE_COMMIT="${1:-}" readonly SOCKET_ROOT="/run/missioncore-sim" readonly SOCKET_PATH="${SOCKET_ROOT}/worker.sock" +readonly ROS_SETUP="/opt/ros/jazzy/setup.bash" +readonly PX4_MSGS_SETUP="${D_ROOT}/build/ros2/px4_msgs/install/setup.bash" if [[ -z "${MISSION_CORE_COMMIT}" ]]; then echo "usage: $0 " >&2 @@ -51,9 +53,19 @@ if [[ "${EUID}" -eq 0 || "${WSL_DISTRO_NAME:-}" != "${EXPECTED_DISTRO}" ]]; then echo "error: worker agent escaped the reviewed worker identity" >&2 exit 2 fi +if [[ ! -f "${ROS_SETUP}" || ! -f "${PX4_MSGS_SETUP}" ]]; then + echo "error: reviewed ROS 2 Jazzy/px4_msgs runtime is incomplete" >&2 + exit 2 +fi + +# shellcheck disable=SC1091 +source "${ROS_SETUP}" +# shellcheck disable=SC1091 +source "${PX4_MSGS_SETUP}" exec env \ - PYTHONPATH="${SOURCE_ROOT}/src" \ + ROS_DOMAIN_ID=0 \ + PYTHONPATH="${SOURCE_ROOT}/src:${PYTHONPATH:-}" \ python3 -m k1link.simulation.worker_agent \ --socket "${SOCKET_PATH}" \ --mission-core-commit "${MISSION_CORE_COMMIT}" \ diff --git a/src/k1link/simulation/px4_rover_control.py b/src/k1link/simulation/px4_rover_control.py new file mode 100644 index 0000000..c03ccae --- /dev/null +++ b/src/k1link/simulation/px4_rover_control.py @@ -0,0 +1,368 @@ +from __future__ import annotations + +import importlib +import math +import threading +import time +from contextlib import suppress +from dataclasses import dataclass +from typing import Any, Final + +from k1link.simulation.contracts import AckermannControlSetpoint + +OFFBOARD_HEARTBEAT_SECONDS: Final = 0.1 +OFFBOARD_WARMUP_HEARTBEATS: Final = 12 +OFFBOARD_ADMISSION_TIMEOUT_SECONDS: Final = 12.0 +VEHICLE_COMMAND_DO_SET_MODE: Final = 176 +VEHICLE_COMMAND_COMPONENT_ARM_DISARM: Final = 400 + + +class Px4RoverControlError(RuntimeError): + """The virtual-only PX4 rover command boundary failed closed.""" + + +@dataclass(frozen=True, slots=True) +class Px4RoverControlSnapshot: + run_id: str + armed: bool + offboard: bool + ttl_expired_count: int + + +class Ros2Px4AckermannControl: + """Publish canonical speed+steering through the PX4 ROS 2 offboard boundary.""" + + def __init__( + self, + *, + heartbeat_seconds: float = OFFBOARD_HEARTBEAT_SECONDS, + admission_timeout_seconds: float = OFFBOARD_ADMISSION_TIMEOUT_SECONDS, + ) -> None: + if not 0.05 <= heartbeat_seconds <= 0.25: + raise ValueError("PX4 heartbeat period must remain within 4..20 Hz") + if admission_timeout_seconds <= 0: + raise ValueError("PX4 admission timeout must be positive") + self.heartbeat_seconds = heartbeat_seconds + self.admission_timeout_seconds = admission_timeout_seconds + self._lock = threading.RLock() + self._started = threading.Event() + self._control_ready = threading.Event() + self._disarmed = threading.Event() + self._stop = threading.Event() + self._thread: threading.Thread | None = None + self._run_id: str | None = None + self._latest_speed_mps = 0.0 + self._latest_steering_normalized = 0.0 + self._valid_until_monotonic_ns = 0 + self._ttl_expired_count = 0 + self._ttl_expired_for_latest = False + self._armed = False + self._offboard = False + self._disarm_requested = False + self._failure: BaseException | None = None + + def start(self, run_id: str) -> Px4RoverControlSnapshot: + with self._lock: + if self._thread is not None: + raise Px4RoverControlError("PX4 command controller already owns a run") + self._run_id = run_id + self._latest_speed_mps = 0.0 + self._latest_steering_normalized = 0.0 + self._valid_until_monotonic_ns = 0 + self._ttl_expired_for_latest = False + self._failure = None + self._stop.clear() + self._started.clear() + self._control_ready.clear() + self._disarmed.clear() + self._disarm_requested = False + self._thread = threading.Thread( + target=self._run, + name=f"px4-rover-control:{run_id}", + daemon=True, + ) + self._thread.start() + if not self._started.wait(timeout=3): + self._abort_thread() + raise Px4RoverControlError("PX4 ROS 2 command publisher did not start") + self._raise_failure() + if not self._control_ready.wait(timeout=self.admission_timeout_seconds): + self._abort_thread() + self._raise_failure() + raise Px4RoverControlError("PX4 did not confirm armed Offboard rover control") + self._raise_failure() + return self.snapshot(run_id) + + def submit(self, command: AckermannControlSetpoint) -> Px4RoverControlSnapshot: + with self._lock: + if self._run_id != command.run_id or self._thread is None: + raise Px4RoverControlError("PX4 command belongs to another or inactive run") + self._raise_failure_locked() + ttl_ns = command.valid_until_sim_ns - command.issued_at_sim_ns + self._latest_speed_mps = command.speed_mps + self._latest_steering_normalized = command.steering_normalized + self._valid_until_monotonic_ns = time.monotonic_ns() + ttl_ns + self._ttl_expired_for_latest = False + return self.snapshot(command.run_id) + + def stop(self, run_id: str) -> Px4RoverControlSnapshot: + with self._lock: + if self._run_id != run_id or self._thread is None: + raise Px4RoverControlError("PX4 stop belongs to another or inactive run") + self._latest_speed_mps = 0.0 + self._latest_steering_normalized = 0.0 + self._valid_until_monotonic_ns = 0 + self._disarm_requested = True + self._disarmed.clear() + self._disarmed.wait(timeout=2) + snapshot = self.snapshot(run_id) + self._stop.set() + thread = self._thread + thread.join(timeout=3) + if thread.is_alive(): + raise Px4RoverControlError("PX4 command publisher did not stop") + self._raise_failure() + with self._lock: + self._thread = None + self._run_id = None + return snapshot + + def snapshot(self, run_id: str) -> Px4RoverControlSnapshot: + with self._lock: + if self._run_id != run_id: + raise Px4RoverControlError("PX4 snapshot belongs to another run") + return Px4RoverControlSnapshot( + run_id=run_id, + armed=self._armed, + offboard=self._offboard, + ttl_expired_count=self._ttl_expired_count, + ) + + def _abort_thread(self) -> None: + self._stop.set() + thread = self._thread + if thread is not None: + thread.join(timeout=3) + with self._lock: + self._thread = None + self._run_id = None + + def _raise_failure(self) -> None: + with self._lock: + self._raise_failure_locked() + + def _raise_failure_locked(self) -> None: + if self._failure is not None: + raise Px4RoverControlError( + f"PX4 ROS 2 command publisher failed: {type(self._failure).__name__}" + ) from self._failure + + def _run(self) -> None: + rclpy: Any | None = None + node: Any | None = None + try: + rclpy = importlib.import_module("rclpy") + qos_module = importlib.import_module("rclpy.qos") + messages = importlib.import_module("px4_msgs.msg") + rclpy.init(args=None) + node = rclpy.create_node("missioncore_px4_ackermann_control") + qos = qos_module.QoSProfile( + reliability=qos_module.ReliabilityPolicy.BEST_EFFORT, + durability=qos_module.DurabilityPolicy.TRANSIENT_LOCAL, + history=qos_module.HistoryPolicy.KEEP_LAST, + depth=1, + ) + offboard_publisher = node.create_publisher( + messages.OffboardControlMode, + "/fmu/in/offboard_control_mode", + qos, + ) + speed_publisher = node.create_publisher( + messages.RoverSpeedSetpoint, + "/fmu/in/rover_speed_setpoint", + qos, + ) + steering_publisher = node.create_publisher( + messages.RoverSteeringSetpoint, + "/fmu/in/rover_steering_setpoint", + qos, + ) + attitude_publisher = node.create_publisher( + messages.RoverAttitudeSetpoint, + "/fmu/in/rover_attitude_setpoint", + qos, + ) + rate_publisher = node.create_publisher( + messages.RoverRateSetpoint, + "/fmu/in/rover_rate_setpoint", + qos, + ) + command_publisher = node.create_publisher( + messages.VehicleCommand, + "/fmu/in/vehicle_command", + qos, + ) + + def receive_status(status: Any) -> None: + with self._lock: + self._armed = status.arming_state == messages.VehicleStatus.ARMING_STATE_ARMED + self._offboard = ( + status.nav_state == messages.VehicleStatus.NAVIGATION_STATE_OFFBOARD + ) + if self._armed and self._offboard: + self._control_ready.set() + + node.create_subscription( + messages.VehicleStatus, + "/fmu/out/vehicle_status_v1", + receive_status, + qos, + ) + self._started.set() + heartbeat_count = 0 + last_mode_request_monotonic = 0.0 + while not self._stop.is_set(): + now_monotonic = time.monotonic() + now_monotonic_ns = time.monotonic_ns() + rclpy.spin_once(node, timeout_sec=0) + with self._lock: + expired = ( + self._valid_until_monotonic_ns > 0 + and now_monotonic_ns >= self._valid_until_monotonic_ns + ) + if expired: + self._latest_speed_mps = 0.0 + self._latest_steering_normalized = 0.0 + self._valid_until_monotonic_ns = 0 + if not self._ttl_expired_for_latest: + self._ttl_expired_count += 1 + self._ttl_expired_for_latest = True + speed_mps = self._latest_speed_mps + steering_normalized = self._latest_steering_normalized + disarm_requested = self._disarm_requested + timestamp_us = int(node.get_clock().now().nanoseconds // 1_000) + self._publish_setpoint( + messages, + offboard_publisher, + speed_publisher, + steering_publisher, + attitude_publisher, + rate_publisher, + timestamp_us, + speed_mps, + steering_normalized, + ) + heartbeat_count += 1 + if ( + heartbeat_count >= OFFBOARD_WARMUP_HEARTBEATS + and not self._control_ready.is_set() + and now_monotonic - last_mode_request_monotonic >= 1.0 + ): + self._publish_vehicle_command( + messages, + command_publisher, + timestamp_us, + VEHICLE_COMMAND_DO_SET_MODE, + param1=1.0, + param2=6.0, + ) + self._publish_vehicle_command( + messages, + command_publisher, + timestamp_us, + VEHICLE_COMMAND_COMPONENT_ARM_DISARM, + param1=1.0, + ) + last_mode_request_monotonic = now_monotonic + if disarm_requested: + self._publish_vehicle_command( + messages, + command_publisher, + timestamp_us, + VEHICLE_COMMAND_COMPONENT_ARM_DISARM, + param1=0.0, + ) + with self._lock: + self._armed = False + self._offboard = False + self._disarm_requested = False + self._disarmed.set() + self._stop.wait(self.heartbeat_seconds) + except BaseException as exc: + with self._lock: + self._failure = exc + self._started.set() + self._control_ready.set() + self._disarmed.set() + finally: + if node is not None: + node.destroy_node() + if rclpy is not None: + with suppress(Exception): + rclpy.shutdown() + + @staticmethod + def _publish_setpoint( + messages: Any, + offboard_publisher: Any, + speed_publisher: Any, + steering_publisher: Any, + attitude_publisher: Any, + rate_publisher: Any, + timestamp_us: int, + speed_mps: float, + steering_normalized: float, + ) -> None: + offboard = messages.OffboardControlMode() + offboard.timestamp = timestamp_us + offboard.position = False + offboard.velocity = True + offboard.acceleration = False + offboard.attitude = False + offboard.body_rate = False + offboard.thrust_and_torque = False + offboard.direct_actuator = False + offboard_publisher.publish(offboard) + + speed = messages.RoverSpeedSetpoint() + speed.timestamp = timestamp_us + speed.speed_body_x = float(speed_mps) + speed.speed_body_y = math.nan + speed_publisher.publish(speed) + + steering = messages.RoverSteeringSetpoint() + steering.timestamp = timestamp_us + steering.normalized_steering_setpoint = float(steering_normalized) + steering_publisher.publish(steering) + + attitude = messages.RoverAttitudeSetpoint() + attitude.timestamp = timestamp_us + attitude.yaw_setpoint = math.nan + attitude_publisher.publish(attitude) + + rate = messages.RoverRateSetpoint() + rate.timestamp = timestamp_us + rate.yaw_rate_setpoint = math.nan + rate_publisher.publish(rate) + + @staticmethod + def _publish_vehicle_command( + messages: Any, + publisher: Any, + timestamp_us: int, + command: int, + *, + param1: float, + param2: float = 0.0, + ) -> None: + message = messages.VehicleCommand() + message.timestamp = timestamp_us + message.param1 = param1 + message.param2 = param2 + message.command = command + message.target_system = 1 + message.target_component = 1 + message.source_system = 1 + message.source_component = 1 + message.from_external = True + publisher.publish(message) diff --git a/src/k1link/simulation/worker_agent.py b/src/k1link/simulation/worker_agent.py index e79a0cc..c699bf4 100644 --- a/src/k1link/simulation/worker_agent.py +++ b/src/k1link/simulation/worker_agent.py @@ -11,12 +11,14 @@ import subprocess import sys import threading import time +from contextlib import suppress from datetime import UTC, datetime from pathlib import Path from types import FrameType from typing import Any, Final from k1link.simulation.contracts import ( + AckermannControlSetpoint, AuthorityProfile, ProviderPin, QualificationRun, @@ -26,6 +28,10 @@ from k1link.simulation.contracts import ( ) from k1link.simulation.orchestrator import SimulationApplicationService from k1link.simulation.process_supervisor import PosixProcessSupervisor +from k1link.simulation.px4_rover_control import ( + Px4RoverControlSnapshot, + Ros2Px4AckermannControl, +) from k1link.simulation.run_store import ( ACTIVE_RECOVERY_STATES, QualificationRunNotFoundError, @@ -44,6 +50,7 @@ from k1link.simulation.worker import ( SimulationWorldControl, ) from k1link.simulation.worker_gateway import ( + COMMAND_ACCEPTANCE_SCHEMA, MAX_MESSAGE_BYTES, OPERATIONS, REQUEST_SCHEMA, @@ -213,6 +220,8 @@ class SimulationWorkerAgent: ) self.service = SimulationApplicationService(self.store, self.worker) self.collector = GazeboPoseCollector(stock_rover_process_environment(self.paths)) + self.command_controller = Ros2Px4AckermannControl() + self._command_lock = threading.RLock() self._shutdown = threading.Event() self._server_socket: socket.socket | None = None self._connection_threads: set[threading.Thread] = set() @@ -258,6 +267,8 @@ class SimulationWorkerAgent: self._shutdown.set() active = self._active_run() if active is not None: + with suppress(Exception): + self.command_controller.stop(active.run_id) try: self.service.stop( active.run_id, @@ -309,6 +320,23 @@ class SimulationWorkerAgent: mission_core_commit=_string(payload["mission_core_commit"], "commit", 40), idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160), ) + if operation == "command": + _exact_keys( + payload, + {"run_id", "speed_mps", "steering_normalized", "idempotency_key"}, + "command payload", + ) + return self.command( + run_id=_safe_id(payload["run_id"], "run id"), + speed_mps=_bounded_number(payload["speed_mps"], "speed", -1.5, 1.5), + steering_normalized=_bounded_number( + payload["steering_normalized"], + "steering", + -1.0, + 1.0, + ), + idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160), + ) _exact_keys(payload, {"run_id", "idempotency_key"}, "stop payload") return self.stop( run_id=_safe_id(payload["run_id"], "run id"), @@ -367,9 +395,22 @@ class SimulationWorkerAgent: host_monotonic_ns=time.monotonic_ns(), ) if running.state is not RunState.RUNNING: - raise WorkerAgentError( - f"provider start ended in terminal state {running.state.value}" - ) + raise WorkerAgentError(f"provider start ended in terminal state {running.state.value}") + try: + self.command_controller.start(run_id) + except Exception as exc: + try: + self.service.stop( + run_id, + idempotency_key=f"{run_id}:command-controller-admission-failed", + observed_at_utc=_utc_now(), + host_monotonic_ns=time.monotonic_ns(), + sim_time_ns=None, + terminal_state=RunState.ABORTED, + reason="px4-command-controller-admission-failed", + ) + finally: + raise WorkerAgentError("PX4 command controller admission failed") from exc return self.status() def stop(self, *, run_id: str, idempotency_key: str) -> dict[str, Any]: @@ -381,19 +422,109 @@ class SimulationWorkerAgent: sim_time_ns = int(self.collector.collect(run_id)["sim_time_ns"]) except Exception: sim_time_ns = None + command_stop_error: Exception | None = None + try: + self.command_controller.stop(run_id) + except Exception as exc: + command_stop_error = exc terminal = self.service.stop( run_id, idempotency_key=idempotency_key, observed_at_utc=_utc_now(), host_monotonic_ns=time.monotonic_ns(), sim_time_ns=sim_time_ns, + terminal_state=( + RunState.ABORTED if command_stop_error is not None else RunState.COMPLETED + ), + reason=( + "px4-command-controller-stop-failed" + if command_stop_error is not None + else "operator-stop-clean" + ), ) - if terminal.state is not RunState.COMPLETED: + if command_stop_error is not None: raise WorkerAgentError( - f"provider stop ended in terminal state {terminal.state.value}" - ) + "PX4 command controller did not stop cleanly" + ) from command_stop_error + if terminal.state is not RunState.COMPLETED: + raise WorkerAgentError(f"provider stop ended in terminal state {terminal.state.value}") return self.status() + def command( + self, + *, + run_id: str, + speed_mps: float, + steering_normalized: float, + idempotency_key: str, + ) -> dict[str, Any]: + with self._command_lock: + return self._command_locked( + run_id=run_id, + speed_mps=speed_mps, + steering_normalized=steering_normalized, + idempotency_key=idempotency_key, + ) + + def _command_locked( + self, + *, + run_id: str, + speed_mps: float, + steering_normalized: float, + idempotency_key: str, + ) -> dict[str, Any]: + active = self._active_run() + if active is None or active.run_id != run_id or active.state is not RunState.RUNNING: + raise WorkerAgentError("requested run does not own running command authority") + command_id = f"cmd-{hashlib.sha256(idempotency_key.encode()).hexdigest()[:24]}" + existing = next( + (item for item in self.store.list_commands(run_id) if item.command_id == command_id), + None, + ) + if existing is not None: + if not isinstance(existing, AckermannControlSetpoint) or ( + existing.speed_mps != speed_mps + or existing.steering_normalized != steering_normalized + ): + raise WorkerAgentError("command idempotency key is bound to another setpoint") + snapshot = self.command_controller.snapshot(run_id) + return _command_acceptance(existing, snapshot) + sample = self.collector.collect(run_id) + issued_at_sim_ns = int(sample["sim_time_ns"]) + ttl_ns = min(active.authority.command_ttl_max_ns, 250_000_000) + command = AckermannControlSetpoint( + run_id=run_id, + command_id=command_id, + sequence=len(self.store.list_commands(run_id)) + 1, + issued_at_sim_ns=issued_at_sim_ns, + valid_until_sim_ns=issued_at_sim_ns + ttl_ns, + authority_generation=active.authority.generation, + speed_mps=speed_mps, + steering_normalized=steering_normalized, + source="mission-core-control-station", + ) + self.store.submit_command(command) + try: + snapshot = self.command_controller.submit(command) + except Exception as exc: + current = self.store.load(run_id) + self.store.append_event( + run_id, + event_type="command.delivery-failed", + observed_at_utc=_utc_now(), + host_monotonic_ns=time.monotonic_ns(), + sim_time_ns=issued_at_sim_ns, + payload={ + "command_id": command.command_id, + "provider": "px4-ros2-offboard", + "detail": type(exc).__name__, + }, + expected_revision=current.revision, + ) + raise WorkerAgentError("PX4 rejected the canonical rover setpoint") from exc + return _command_acceptance(command, snapshot) + def _handle_connection(self, connection: socket.socket) -> None: request_id: str | None = None try: @@ -561,6 +692,18 @@ def _number(value: object, label: str) -> float: return result +def _bounded_number( + value: object, + label: str, + minimum: float, + maximum: float, +) -> float: + result = _number(value, label) + if not minimum <= result <= maximum: + raise WorkerAgentError(f"{label} is outside the admitted range") + return result + + def _integer(value: object, label: str) -> int: if isinstance(value, bool) or not isinstance(value, int | str): raise WorkerAgentError(f"{label} must be an integer") @@ -592,6 +735,30 @@ def _safe_error(exc: Exception) -> str: return f"{type(exc).__name__}: worker operation failed" +def _command_acceptance( + command: AckermannControlSetpoint, + snapshot: Px4RoverControlSnapshot, +) -> dict[str, Any]: + return { + "schema_version": COMMAND_ACCEPTANCE_SCHEMA, + "run_id": command.run_id, + "command_id": command.command_id, + "sequence": command.sequence, + "issued_at_sim_ns": command.issued_at_sim_ns, + "valid_until_sim_ns": command.valid_until_sim_ns, + "speed_mps": command.speed_mps, + "steering_normalized": command.steering_normalized, + "authority_scope": command.authority_scope.value, + "delivery": { + "provider": "px4-ros2-offboard", + "mode": "speed-steering", + "armed": snapshot.armed, + "offboard": snapshot.offboard, + "ttl_expired_count": snapshot.ttl_expired_count, + }, + } + + def main() -> int: parser = argparse.ArgumentParser(description="Serve the Mission Core Simulation Worker agent.") parser.add_argument("--socket", type=Path, required=True) diff --git a/src/k1link/simulation/worker_gateway.py b/src/k1link/simulation/worker_gateway.py index f6ee515..41192ec 100644 --- a/src/k1link/simulation/worker_gateway.py +++ b/src/k1link/simulation/worker_gateway.py @@ -1,6 +1,7 @@ from __future__ import annotations import json +import math import socket from collections.abc import Mapping from pathlib import Path @@ -11,8 +12,9 @@ REQUEST_SCHEMA: Final = "missioncore.simulation-worker-request/v1" RESPONSE_SCHEMA: Final = "missioncore.simulation-worker-response/v1" STATUS_SCHEMA: Final = "missioncore.simulation-worker-status/v1" VEHICLE_STATE_SCHEMA: Final = "missioncore.vehicle-state/v1" +COMMAND_ACCEPTANCE_SCHEMA: Final = "missioncore.command-acceptance/v1" MAX_MESSAGE_BYTES: Final = 64 * 1024 -OPERATIONS: Final = frozenset({"status", "live", "start", "stop"}) +OPERATIONS: Final = frozenset({"status", "live", "start", "command", "stop"}) class SimulationWorkerGatewayError(RuntimeError): @@ -47,6 +49,15 @@ class PolygonWorkerGateway(Protocol): idempotency_key: str, ) -> dict[str, Any]: ... + def command( + self, + *, + run_id: str, + speed_mps: float, + steering_normalized: float, + idempotency_key: str, + ) -> dict[str, Any]: ... + class UnixSocketWorkerGateway: """Bounded request/response transport between Mission Core and one local worker.""" @@ -113,6 +124,27 @@ class UnixSocketWorkerGateway: _validate_status(result) return result + def command( + self, + *, + run_id: str, + speed_mps: float, + steering_normalized: float, + idempotency_key: str, + ) -> dict[str, Any]: + result = self._request( + "command", + { + "run_id": run_id, + "speed_mps": speed_mps, + "steering_normalized": steering_normalized, + "idempotency_key": idempotency_key, + }, + timeout_seconds=self.request_timeout_seconds, + ) + _validate_command_acceptance(result) + return result + def _request( self, operation: str, @@ -258,3 +290,55 @@ def _validate_vehicle_state(value: Mapping[str, Any]) -> None: or not isinstance(value.get("safety"), dict) ): raise SimulationWorkerGatewayError("vehicle state contains invalid values") + + +def _validate_command_acceptance(value: Mapping[str, Any]) -> None: + expected = { + "schema_version", + "run_id", + "command_id", + "sequence", + "issued_at_sim_ns", + "valid_until_sim_ns", + "speed_mps", + "steering_normalized", + "authority_scope", + "delivery", + } + if set(value) != expected or value.get("schema_version") != COMMAND_ACCEPTANCE_SCHEMA: + raise SimulationWorkerGatewayError("command acceptance does not match v1") + delivery = value.get("delivery") + numeric_fields = ( + "sequence", + "issued_at_sim_ns", + "valid_until_sim_ns", + "speed_mps", + "steering_normalized", + ) + if ( + not isinstance(value.get("run_id"), str) + or not isinstance(value.get("command_id"), str) + or any( + isinstance(value.get(field), bool) + or not isinstance(value.get(field), int | float) + or not math.isfinite(value[field]) + for field in numeric_fields + ) + or value.get("authority_scope") != "virtual-only" + or not isinstance(delivery, dict) + or set(delivery) + != { + "provider", + "mode", + "armed", + "offboard", + "ttl_expired_count", + } + or delivery.get("provider") != "px4-ros2-offboard" + or delivery.get("mode") != "speed-steering" + or not isinstance(delivery.get("armed"), bool) + or not isinstance(delivery.get("offboard"), bool) + or isinstance(delivery.get("ttl_expired_count"), bool) + or not isinstance(delivery.get("ttl_expired_count"), int) + ): + raise SimulationWorkerGatewayError("command acceptance contains invalid values") diff --git a/src/k1link/web/polygon_api.py b/src/k1link/web/polygon_api.py index 7a86f89..c9c400c 100644 --- a/src/k1link/web/polygon_api.py +++ b/src/k1link/web/polygon_api.py @@ -10,7 +10,7 @@ from typing import Annotated, Any, Final from fastapi import APIRouter, Header, HTTPException, Query from fastapi import Path as PathParameter -from pydantic import BaseModel, ConfigDict +from pydantic import BaseModel, ConfigDict, Field from k1link.simulation import ( QualificationRun, @@ -54,6 +54,13 @@ class StartStockRoverRequest(BaseModel): scenario_id: str +class AckermannCommandRequest(BaseModel): + model_config = ConfigDict(extra="forbid", frozen=True) + + speed_mps: float = Field(ge=-1.5, le=1.5, allow_inf_nan=False) + steering_normalized: float = Field(ge=-1.0, le=1.0, allow_inf_nan=False) + + def configured_polygon_runs_root() -> Path | None: raw = os.environ.get(POLYGON_RUNS_ROOT_ENV) if raw is None or not raw.strip(): @@ -250,6 +257,40 @@ def build_polygon_router( detail="Simulation Worker не подтвердил остановку.", ) from exc + @router.post("/worker/runs/{run_id}/commands") + def command_polygon_worker_run( + run_id: Annotated[ + str, + PathParameter(pattern=r"^[a-z0-9][a-z0-9-]{0,63}$"), + ], + request: AckermannCommandRequest, + idempotency_key: Annotated[ + str, + Header(alias="Idempotency-Key", min_length=1, max_length=160), + ], + ) -> dict[str, Any]: + gateway, _ = _control_context( + worker_provider, + control_provider, + commit_provider, + ) + try: + return gateway.command( + run_id=run_id, + speed_mps=request.speed_mps, + steering_normalized=request.steering_normalized, + idempotency_key=idempotency_key, + ) + except SimulationWorkerUnavailableError as exc: + raise HTTPException(status_code=503, detail="Simulation Worker недоступен.") from exc + except SimulationWorkerRejectedError as exc: + raise HTTPException(status_code=409, detail=str(exc)) from exc + except SimulationWorkerGatewayError as exc: + raise HTTPException( + status_code=502, + detail="Simulation Worker не подтвердил команду ровера.", + ) from exc + return router diff --git a/tests/test_polygon_api.py b/tests/test_polygon_api.py index 07fbfe3..a4bb626 100644 --- a/tests/test_polygon_api.py +++ b/tests/test_polygon_api.py @@ -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: diff --git a/tests/test_simulation_worker_agent_command.py b/tests/test_simulation_worker_agent_command.py new file mode 100644 index 0000000..08e845f --- /dev/null +++ b/tests/test_simulation_worker_agent_command.py @@ -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) diff --git a/tests/test_simulation_worker_gateway.py b/tests/test_simulation_worker_gateway.py index 142c22e..6470178 100644 --- a/tests/test_simulation_worker_gateway.py +++ b/tests/test_simulation_worker_gateway.py @@ -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)