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