feat(simulation): drive stock rover through PX4 offboard

This commit is contained in:
DCCONSTRUCTIONS 2026-07-24 21:47:40 +03:00
parent 53fd1e3bfc
commit 101707d54c
12 changed files with 1329 additions and 15 deletions

View File

@ -24,6 +24,19 @@ export interface PolygonVehicleState {
orientation: { x: number; y: number; z: number; w: number }; 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 PolygonWorkerContractError extends Error {}
export class PolygonWorkerApiError extends Error { export class PolygonWorkerApiError extends Error {
@ -89,6 +102,25 @@ const SAFETY_KEYS = new Set([
"actuator_authority", "actuator_authority",
"navigation_or_safety_accepted", "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<string, unknown> { function isRecord(value: unknown): value is Record<string, unknown> {
return typeof value === "object" && value !== null && !Array.isArray(value); 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<unknown> { async function responseBody(response: Response): Promise<unknown> {
const contentType = response.headers.get("content-type") ?? ""; const contentType = response.headers.get("content-type") ?? "";
if (!contentType.toLowerCase().startsWith("application/json")) { if (!contentType.toLowerCase().startsWith("application/json")) {
@ -391,3 +476,52 @@ export async function stopPolygonWorker(
fetcher, 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<PolygonCommandAcceptance> {
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,
));
}

View File

@ -300,6 +300,44 @@
gap: 0.42rem; 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) { .polygon-live-telemetry > div:not(.polygon-live-boundary) {
display: grid; display: grid;
min-width: 0; min-width: 0;

View File

@ -1,4 +1,4 @@
import { lazy, Suspense, useEffect, useState } from "react"; import { lazy, Suspense, useEffect, useRef, useState } from "react";
import { import {
Button, Button,
GlassSurface, GlassSurface,
@ -8,12 +8,28 @@ import {
import { import {
fetchPolygonVehicleState, fetchPolygonVehicleState,
fetchPolygonWorkerStatus, fetchPolygonWorkerStatus,
sendPolygonRoverCommand,
startPolygonWorker, startPolygonWorker,
stopPolygonWorker, stopPolygonWorker,
type PolygonCommandAcceptance,
type PolygonVehicleState, type PolygonVehicleState,
type PolygonWorkerStatus, type PolygonWorkerStatus,
} from "../core/polygon/liveWorker"; } 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 PolygonRoverScene = lazy(async () => {
const module = await import("./PolygonRoverScene"); const module = await import("./PolygonRoverScene");
return { default: module.PolygonRoverScene }; return { default: module.PolygonRoverScene };
@ -30,7 +46,11 @@ export function PolygonLivePanel() {
const [trajectory, setTrajectory] = useState<PolygonVehicleState[]>([]); const [trajectory, setTrajectory] = useState<PolygonVehicleState[]>([]);
const [error, setError] = useState<string | null>(null); const [error, setError] = useState<string | null>(null);
const [actionBusy, setActionBusy] = useState(false); const [actionBusy, setActionBusy] = useState(false);
const [controlIntent, setControlIntent] = useState<ControlIntent | null>(null);
const [commandAcceptance, setCommandAcceptance] =
useState<PolygonCommandAcceptance | null>(null);
const [generation, setGeneration] = useState(0); const [generation, setGeneration] = useState(0);
const commandInFlight = useRef(false);
useEffect(() => { useEffect(() => {
if (actionBusy) return; if (actionBusy) return;
@ -73,12 +93,75 @@ export function PolygonLivePanel() {
const runActive = Boolean(worker?.activeRunId); 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 () => { const runAction = async () => {
if (!worker?.controlAvailable || actionBusy) return; if (!worker?.controlAvailable || actionBusy) return;
setActionBusy(true); setActionBusy(true);
setError(null); setError(null);
try { try {
const idempotencyKey = crypto.randomUUID(); const idempotencyKey = crypto.randomUUID();
if (worker.activeRunId) {
await stopMotion();
}
const next = worker.activeRunId const next = worker.activeRunId
? await stopPolygonWorker(worker.activeRunId, { idempotencyKey }) ? await stopPolygonWorker(worker.activeRunId, { idempotencyKey })
: await startPolygonWorker({ idempotencyKey }); : await startPolygonWorker({ idempotencyKey });
@ -136,6 +219,40 @@ export function PolygonLivePanel() {
</Suspense> </Suspense>
<div className="polygon-live-telemetry"> <div className="polygon-live-telemetry">
<div className="polygon-rover-controls">
<div className="polygon-rover-controls__heading">
<span>Управление PX4</span>
<StatusBadge tone={commandAcceptance?.offboard ? "success" : "neutral"}>
{commandAcceptance?.offboard ? "Armed · Offboard" : "Ожидает прогона"}
</StatusBadge>
</div>
<div className="polygon-rover-controls__grid">
{CONTROL_INTENTS.map((intent) => (
<Button
key={intent.id}
size="compact"
variant={controlIntent?.id === intent.id ? "primary" : "secondary"}
disabled={!runActive || worker?.runState !== "running" || actionBusy}
onClick={() => setControlIntent(intent)}
>
{intent.label}
</Button>
))}
<Button
size="compact"
variant="secondary"
disabled={!runActive || worker?.runState !== "running" || actionBusy}
onClick={() => void stopMotion()}
>
Стоп
</Button>
</div>
<small>
{controlIntent
? `${controlIntent.label}: ${controlIntent.speedMps.toFixed(2)} m/s · steering ${controlIntent.steeringNormalized.toFixed(2)}`
: "Команда 250 мс; потеря браузера автоматически обнуляет скорость."}
</small>
</div>
<div> <div>
<span>Прогон</span> <span>Прогон</span>
<strong>{worker?.activeRunId ?? "—"}</strong> <strong>{worker?.activeRunId ?? "—"}</strong>
@ -156,11 +273,19 @@ export function PolygonLivePanel() {
<span>Провайдеры</span> <span>Провайдеры</span>
<strong>{worker?.providerIds.join(" · ") || "—"}</strong> <strong>{worker?.providerIds.join(" · ") || "—"}</strong>
</div> </div>
<div>
<span>Последняя команда</span>
<strong>
{commandAcceptance
? `#${commandAcceptance.sequence} · ${commandAcceptance.speedMps.toFixed(2)} m/s · ${commandAcceptance.steeringNormalized.toFixed(2)}`
: "—"}
</strong>
</div>
<div className="polygon-live-boundary"> <div className="polygon-live-boundary">
<StatusBadge tone="success">Virtual only</StatusBadge> <StatusBadge tone="success">Virtual only</StatusBadge>
<p> <p>
Нет actuator authority. Live-поза диагностическая и пока не доказывает Команды проходят PX4 ROS 2 Offboard только в SITL. Физического actuator
приёмку PX4/ROS 2 telemetry, navigation или safety. authority нет; live-поза остаётся диагностической.
</p> </p>
</div> </div>
{error && <p className="polygon-live-error">{error}</p>} {error && <p className="polygon-live-error">{error}</p>}

View File

@ -12,8 +12,10 @@ let resolvePolygonRunRoute;
let PolygonRunContractError; let PolygonRunContractError;
let decodePolygonWorkerStatus; let decodePolygonWorkerStatus;
let decodePolygonVehicleState; let decodePolygonVehicleState;
let decodePolygonCommandAcceptance;
let fetchPolygonWorkerStatus; let fetchPolygonWorkerStatus;
let fetchPolygonVehicleState; let fetchPolygonVehicleState;
let sendPolygonRoverCommand;
let startPolygonWorker; let startPolygonWorker;
let stopPolygonWorker; let stopPolygonWorker;
let PolygonWorkerContractError; let PolygonWorkerContractError;
@ -38,8 +40,10 @@ before(async () => {
({ ({
decodePolygonWorkerStatus, decodePolygonWorkerStatus,
decodePolygonVehicleState, decodePolygonVehicleState,
decodePolygonCommandAcceptance,
fetchPolygonWorkerStatus, fetchPolygonWorkerStatus,
fetchPolygonVehicleState, fetchPolygonVehicleState,
sendPolygonRoverCommand,
startPolygonWorker, startPolygonWorker,
stopPolygonWorker, stopPolygonWorker,
PolygonWorkerContractError, 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", () => { test("Polygon is a worker-gated product root and keeps a compatible direct route", () => {
assert.deepEqual(resolvePolygonRunRoute("?workspace=polygon-run"), { assert.deepEqual(resolvePolygonRunRoute("?workspace=polygon-run"), {
active: true, 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", () => { test("Polygon Live decodes only D-only virtual diagnostic state", () => {
const status = decodePolygonWorkerStatus(workerStatus()); const status = decodePolygonWorkerStatus(workerStatus());
const live = decodePolygonVehicleState(vehicleState()); const live = decodePolygonVehicleState(vehicleState());
const command = decodePolygonCommandAcceptance(commandAcceptance());
assert.equal(status.available, true); assert.equal(status.available, true);
assert.equal(status.activeRunId, live.runId); assert.equal(status.activeRunId, live.runId);
assert.equal(status.isolation.artifactPolicy, "d-only"); assert.equal(status.isolation.artifactPolicy, "d-only");
assert.deepEqual(live.position, { x: 1.25, y: -0.5, z: 0.18 }); 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( assert.throws(
() => decodePolygonWorkerStatus(workerStatus({ () => decodePolygonWorkerStatus(workerStatus({
authority: { authority: {
@ -344,6 +373,12 @@ test("Polygon Live decodes only D-only virtual diagnostic state", () => {
})), })),
PolygonWorkerContractError, PolygonWorkerContractError,
); );
assert.throws(
() => decodePolygonCommandAcceptance(commandAcceptance({
authority_scope: "physical",
})),
PolygonWorkerContractError,
);
}); });
test("Polygon Live uses same-origin gateway and idempotent lifecycle requests", async () => { 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) => { const fetcher = async (url, init) => {
calls.push({ url: String(url), init }); calls.push({ url: String(url), init });
if (String(url).endsWith("/live")) return jsonResponse(vehicleState()); if (String(url).endsWith("/live")) return jsonResponse(vehicleState());
if (String(url).endsWith("/commands")) return jsonResponse(commandAcceptance());
if (String(url).endsWith("/stop")) return jsonResponse(stopped); if (String(url).endsWith("/stop")) return jsonResponse(stopped);
return jsonResponse(workerStatus()); return jsonResponse(workerStatus());
}; };
@ -363,6 +399,12 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests",
await fetchPolygonWorkerStatus({ fetcher }); await fetchPolygonWorkerStatus({ fetcher });
await fetchPolygonVehicleState({ fetcher }); await fetchPolygonVehicleState({ fetcher });
await startPolygonWorker({ idempotencyKey: "start-001", 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", { await stopPolygonWorker("s1c-6cb1495-20260724t180000z-aabbcc", {
idempotencyKey: "stop-001", idempotencyKey: "stop-001",
fetcher, fetcher,
@ -372,9 +414,15 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests",
"/api/v1/polygon/worker", "/api/v1/polygon/worker",
"/api/v1/polygon/worker/live", "/api/v1/polygon/worker/live",
"/api/v1/polygon/worker/runs", "/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", "/api/v1/polygon/worker/runs/s1c-6cb1495-20260724t180000z-aabbcc/stop",
]); ]);
assert.equal(calls[2].init.headers["Idempotency-Key"], "start-001"); 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[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,
}));
}); });

View File

@ -12,6 +12,8 @@ readonly SOURCE_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/../.." && pwd -
readonly MISSION_CORE_COMMIT="${1:-}" readonly MISSION_CORE_COMMIT="${1:-}"
readonly SOCKET_ROOT="/run/missioncore-sim" readonly SOCKET_ROOT="/run/missioncore-sim"
readonly SOCKET_PATH="${SOCKET_ROOT}/worker.sock" 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 if [[ -z "${MISSION_CORE_COMMIT}" ]]; then
echo "usage: $0 <exact-40-character-mission-core-commit>" >&2 echo "usage: $0 <exact-40-character-mission-core-commit>" >&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 echo "error: worker agent escaped the reviewed worker identity" >&2
exit 2 exit 2
fi 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 \ exec env \
PYTHONPATH="${SOURCE_ROOT}/src" \ ROS_DOMAIN_ID=0 \
PYTHONPATH="${SOURCE_ROOT}/src:${PYTHONPATH:-}" \
python3 -m k1link.simulation.worker_agent \ python3 -m k1link.simulation.worker_agent \
--socket "${SOCKET_PATH}" \ --socket "${SOCKET_PATH}" \
--mission-core-commit "${MISSION_CORE_COMMIT}" \ --mission-core-commit "${MISSION_CORE_COMMIT}" \

View File

@ -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)

View File

@ -11,12 +11,14 @@ import subprocess
import sys import sys
import threading import threading
import time import time
from contextlib import suppress
from datetime import UTC, datetime from datetime import UTC, datetime
from pathlib import Path from pathlib import Path
from types import FrameType from types import FrameType
from typing import Any, Final from typing import Any, Final
from k1link.simulation.contracts import ( from k1link.simulation.contracts import (
AckermannControlSetpoint,
AuthorityProfile, AuthorityProfile,
ProviderPin, ProviderPin,
QualificationRun, QualificationRun,
@ -26,6 +28,10 @@ from k1link.simulation.contracts import (
) )
from k1link.simulation.orchestrator import SimulationApplicationService from k1link.simulation.orchestrator import SimulationApplicationService
from k1link.simulation.process_supervisor import PosixProcessSupervisor from k1link.simulation.process_supervisor import PosixProcessSupervisor
from k1link.simulation.px4_rover_control import (
Px4RoverControlSnapshot,
Ros2Px4AckermannControl,
)
from k1link.simulation.run_store import ( from k1link.simulation.run_store import (
ACTIVE_RECOVERY_STATES, ACTIVE_RECOVERY_STATES,
QualificationRunNotFoundError, QualificationRunNotFoundError,
@ -44,6 +50,7 @@ from k1link.simulation.worker import (
SimulationWorldControl, SimulationWorldControl,
) )
from k1link.simulation.worker_gateway import ( from k1link.simulation.worker_gateway import (
COMMAND_ACCEPTANCE_SCHEMA,
MAX_MESSAGE_BYTES, MAX_MESSAGE_BYTES,
OPERATIONS, OPERATIONS,
REQUEST_SCHEMA, REQUEST_SCHEMA,
@ -213,6 +220,8 @@ class SimulationWorkerAgent:
) )
self.service = SimulationApplicationService(self.store, self.worker) self.service = SimulationApplicationService(self.store, self.worker)
self.collector = GazeboPoseCollector(stock_rover_process_environment(self.paths)) self.collector = GazeboPoseCollector(stock_rover_process_environment(self.paths))
self.command_controller = Ros2Px4AckermannControl()
self._command_lock = threading.RLock()
self._shutdown = threading.Event() self._shutdown = threading.Event()
self._server_socket: socket.socket | None = None self._server_socket: socket.socket | None = None
self._connection_threads: set[threading.Thread] = set() self._connection_threads: set[threading.Thread] = set()
@ -258,6 +267,8 @@ class SimulationWorkerAgent:
self._shutdown.set() self._shutdown.set()
active = self._active_run() active = self._active_run()
if active is not None: if active is not None:
with suppress(Exception):
self.command_controller.stop(active.run_id)
try: try:
self.service.stop( self.service.stop(
active.run_id, active.run_id,
@ -309,6 +320,23 @@ class SimulationWorkerAgent:
mission_core_commit=_string(payload["mission_core_commit"], "commit", 40), mission_core_commit=_string(payload["mission_core_commit"], "commit", 40),
idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160), 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") _exact_keys(payload, {"run_id", "idempotency_key"}, "stop payload")
return self.stop( return self.stop(
run_id=_safe_id(payload["run_id"], "run id"), run_id=_safe_id(payload["run_id"], "run id"),
@ -367,9 +395,22 @@ class SimulationWorkerAgent:
host_monotonic_ns=time.monotonic_ns(), host_monotonic_ns=time.monotonic_ns(),
) )
if running.state is not RunState.RUNNING: if running.state is not RunState.RUNNING:
raise WorkerAgentError( raise WorkerAgentError(f"provider start ended in terminal state {running.state.value}")
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() return self.status()
def stop(self, *, run_id: str, idempotency_key: str) -> dict[str, Any]: 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"]) sim_time_ns = int(self.collector.collect(run_id)["sim_time_ns"])
except Exception: except Exception:
sim_time_ns = None 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( terminal = self.service.stop(
run_id, run_id,
idempotency_key=idempotency_key, idempotency_key=idempotency_key,
observed_at_utc=_utc_now(), observed_at_utc=_utc_now(),
host_monotonic_ns=time.monotonic_ns(), host_monotonic_ns=time.monotonic_ns(),
sim_time_ns=sim_time_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( 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() 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: def _handle_connection(self, connection: socket.socket) -> None:
request_id: str | None = None request_id: str | None = None
try: try:
@ -561,6 +692,18 @@ def _number(value: object, label: str) -> float:
return result 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: def _integer(value: object, label: str) -> int:
if isinstance(value, bool) or not isinstance(value, int | str): if isinstance(value, bool) or not isinstance(value, int | str):
raise WorkerAgentError(f"{label} must be an integer") 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" 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: def main() -> int:
parser = argparse.ArgumentParser(description="Serve the Mission Core Simulation Worker agent.") parser = argparse.ArgumentParser(description="Serve the Mission Core Simulation Worker agent.")
parser.add_argument("--socket", type=Path, required=True) parser.add_argument("--socket", type=Path, required=True)

View File

@ -1,6 +1,7 @@
from __future__ import annotations from __future__ import annotations
import json import json
import math
import socket import socket
from collections.abc import Mapping from collections.abc import Mapping
from pathlib import Path from pathlib import Path
@ -11,8 +12,9 @@ REQUEST_SCHEMA: Final = "missioncore.simulation-worker-request/v1"
RESPONSE_SCHEMA: Final = "missioncore.simulation-worker-response/v1" RESPONSE_SCHEMA: Final = "missioncore.simulation-worker-response/v1"
STATUS_SCHEMA: Final = "missioncore.simulation-worker-status/v1" STATUS_SCHEMA: Final = "missioncore.simulation-worker-status/v1"
VEHICLE_STATE_SCHEMA: Final = "missioncore.vehicle-state/v1" VEHICLE_STATE_SCHEMA: Final = "missioncore.vehicle-state/v1"
COMMAND_ACCEPTANCE_SCHEMA: Final = "missioncore.command-acceptance/v1"
MAX_MESSAGE_BYTES: Final = 64 * 1024 MAX_MESSAGE_BYTES: Final = 64 * 1024
OPERATIONS: Final = frozenset({"status", "live", "start", "stop"}) OPERATIONS: Final = frozenset({"status", "live", "start", "command", "stop"})
class SimulationWorkerGatewayError(RuntimeError): class SimulationWorkerGatewayError(RuntimeError):
@ -47,6 +49,15 @@ class PolygonWorkerGateway(Protocol):
idempotency_key: str, idempotency_key: str,
) -> dict[str, Any]: ... ) -> dict[str, Any]: ...
def command(
self,
*,
run_id: str,
speed_mps: float,
steering_normalized: float,
idempotency_key: str,
) -> dict[str, Any]: ...
class UnixSocketWorkerGateway: class UnixSocketWorkerGateway:
"""Bounded request/response transport between Mission Core and one local worker.""" """Bounded request/response transport between Mission Core and one local worker."""
@ -113,6 +124,27 @@ class UnixSocketWorkerGateway:
_validate_status(result) _validate_status(result)
return 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( def _request(
self, self,
operation: str, operation: str,
@ -258,3 +290,55 @@ def _validate_vehicle_state(value: Mapping[str, Any]) -> None:
or not isinstance(value.get("safety"), dict) or not isinstance(value.get("safety"), dict)
): ):
raise SimulationWorkerGatewayError("vehicle state contains invalid values") 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")

View File

@ -10,7 +10,7 @@ from typing import Annotated, Any, Final
from fastapi import APIRouter, Header, HTTPException, Query from fastapi import APIRouter, Header, HTTPException, Query
from fastapi import Path as PathParameter from fastapi import Path as PathParameter
from pydantic import BaseModel, ConfigDict from pydantic import BaseModel, ConfigDict, Field
from k1link.simulation import ( from k1link.simulation import (
QualificationRun, QualificationRun,
@ -54,6 +54,13 @@ class StartStockRoverRequest(BaseModel):
scenario_id: str 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: def configured_polygon_runs_root() -> Path | None:
raw = os.environ.get(POLYGON_RUNS_ROOT_ENV) raw = os.environ.get(POLYGON_RUNS_ROOT_ENV)
if raw is None or not raw.strip(): if raw is None or not raw.strip():
@ -250,6 +257,40 @@ def build_polygon_router(
detail="Simulation Worker не подтвердил остановку.", detail="Simulation Worker не подтвердил остановку.",
) from exc ) 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 return router

View File

@ -18,7 +18,11 @@ from k1link.simulation import (
RunKind, RunKind,
RunState, 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_A = "a" * 64
SHA_B = "b" * 64 SHA_B = "b" * 64
@ -241,6 +245,35 @@ class _FakeWorkerGateway:
self.active_run_id = None self.active_run_id = None
return self._status() 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]: def _status(self) -> dict[str, Any]:
return { return {
"schema_version": "missioncore.simulation-worker-status/v1", "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")() live = _endpoint(enabled, "/api/v1/polygon/worker/live", "GET")()
assert live["run_id"] == running["active_run_id"] assert live["run_id"] == running["active_run_id"]
assert live["source"]["quality"] == "diagnostic" 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")( stopped = _endpoint(enabled, "/api/v1/polygon/worker/runs/{run_id}/stop", "POST")(
run_id=running["active_run_id"], run_id=running["active_run_id"],
idempotency_key="stop-001", idempotency_key="stop-001",
) )
assert stopped["active_run_id"] is None 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: def test_polygon_worker_status_is_explicitly_unavailable_when_not_registered() -> None:

View File

@ -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)

View File

@ -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( def _serve_once(
socket_path: Path, socket_path: Path,
result: dict[str, Any], 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: with pytest.raises(SimulationWorkerUnavailableError) as failure:
gateway.status() gateway.status()
assert str(tmp_path) not in str(failure.value) 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)