feat(simulation): drive stock rover through PX4 offboard
This commit is contained in:
DCCONSTRUCTIONS
parent
53fd1e3bfc
commit
101707d54c
|
|
@ -24,6 +24,19 @@ export interface PolygonVehicleState {
|
|||
orientation: { x: number; y: number; z: number; w: number };
|
||||
}
|
||||
|
||||
export interface PolygonCommandAcceptance {
|
||||
runId: string;
|
||||
commandId: string;
|
||||
sequence: number;
|
||||
issuedAtSimNs: number;
|
||||
validUntilSimNs: number;
|
||||
speedMps: number;
|
||||
steeringNormalized: number;
|
||||
armed: boolean;
|
||||
offboard: boolean;
|
||||
ttlExpiredCount: number;
|
||||
}
|
||||
|
||||
export class PolygonWorkerContractError extends Error {}
|
||||
|
||||
export class PolygonWorkerApiError extends Error {
|
||||
|
|
@ -89,6 +102,25 @@ const SAFETY_KEYS = new Set([
|
|||
"actuator_authority",
|
||||
"navigation_or_safety_accepted",
|
||||
]);
|
||||
const COMMAND_KEYS = new Set([
|
||||
"schema_version",
|
||||
"run_id",
|
||||
"command_id",
|
||||
"sequence",
|
||||
"issued_at_sim_ns",
|
||||
"valid_until_sim_ns",
|
||||
"speed_mps",
|
||||
"steering_normalized",
|
||||
"authority_scope",
|
||||
"delivery",
|
||||
]);
|
||||
const COMMAND_DELIVERY_KEYS = new Set([
|
||||
"provider",
|
||||
"mode",
|
||||
"armed",
|
||||
"offboard",
|
||||
"ttl_expired_count",
|
||||
]);
|
||||
|
||||
function isRecord(value: unknown): value is Record<string, unknown> {
|
||||
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> {
|
||||
const contentType = response.headers.get("content-type") ?? "";
|
||||
if (!contentType.toLowerCase().startsWith("application/json")) {
|
||||
|
|
@ -391,3 +476,52 @@ export async function stopPolygonWorker(
|
|||
fetcher,
|
||||
));
|
||||
}
|
||||
|
||||
export async function sendPolygonRoverCommand(
|
||||
runId: string,
|
||||
{
|
||||
speedMps,
|
||||
steeringNormalized,
|
||||
idempotencyKey,
|
||||
signal,
|
||||
fetcher = globalThis.fetch,
|
||||
}: {
|
||||
speedMps: number;
|
||||
steeringNormalized: number;
|
||||
idempotencyKey: string;
|
||||
signal?: AbortSignal;
|
||||
fetcher?: PolygonWorkerFetch;
|
||||
},
|
||||
): Promise<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;
|
||||
}
|
||||
|
||||
.polygon-live-telemetry > .polygon-rover-controls {
|
||||
gap: 0.55rem;
|
||||
border: 1px solid rgb(var(--nodedc-accent-rgb) / 0.24);
|
||||
background:
|
||||
radial-gradient(circle at 0 0, rgb(var(--nodedc-accent-rgb) / 0.1), transparent 58%),
|
||||
rgb(255 255 255 / 0.03);
|
||||
}
|
||||
|
||||
.polygon-rover-controls__heading {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 0.5rem;
|
||||
}
|
||||
|
||||
.polygon-rover-controls__heading > span {
|
||||
color: var(--nodedc-text-primary);
|
||||
font-size: 0.62rem;
|
||||
font-weight: 680;
|
||||
}
|
||||
|
||||
.polygon-rover-controls__grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(2, minmax(0, 1fr));
|
||||
gap: 0.38rem;
|
||||
}
|
||||
|
||||
.polygon-rover-controls__grid > button:last-child {
|
||||
grid-column: 1 / -1;
|
||||
}
|
||||
|
||||
.polygon-rover-controls small {
|
||||
color: var(--nodedc-text-muted);
|
||||
font-family: "SFMono-Regular", Consolas, "Liberation Mono", monospace;
|
||||
font-size: 0.53rem;
|
||||
line-height: 1.45;
|
||||
}
|
||||
|
||||
.polygon-live-telemetry > div:not(.polygon-live-boundary) {
|
||||
display: grid;
|
||||
min-width: 0;
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
import { lazy, Suspense, useEffect, useState } from "react";
|
||||
import { lazy, Suspense, useEffect, useRef, useState } from "react";
|
||||
import {
|
||||
Button,
|
||||
GlassSurface,
|
||||
|
|
@ -8,12 +8,28 @@ import {
|
|||
import {
|
||||
fetchPolygonVehicleState,
|
||||
fetchPolygonWorkerStatus,
|
||||
sendPolygonRoverCommand,
|
||||
startPolygonWorker,
|
||||
stopPolygonWorker,
|
||||
type PolygonCommandAcceptance,
|
||||
type PolygonVehicleState,
|
||||
type PolygonWorkerStatus,
|
||||
} from "../core/polygon/liveWorker";
|
||||
|
||||
interface ControlIntent {
|
||||
id: "forward" | "reverse" | "left" | "right";
|
||||
label: string;
|
||||
speedMps: number;
|
||||
steeringNormalized: number;
|
||||
}
|
||||
|
||||
const CONTROL_INTENTS: ControlIntent[] = [
|
||||
{ id: "forward", label: "Вперёд", speedMps: 1, steeringNormalized: 0 },
|
||||
{ id: "left", label: "Влево", speedMps: 0.8, steeringNormalized: -0.55 },
|
||||
{ id: "right", label: "Вправо", speedMps: 0.8, steeringNormalized: 0.55 },
|
||||
{ id: "reverse", label: "Назад", speedMps: -0.65, steeringNormalized: 0 },
|
||||
];
|
||||
|
||||
const PolygonRoverScene = lazy(async () => {
|
||||
const module = await import("./PolygonRoverScene");
|
||||
return { default: module.PolygonRoverScene };
|
||||
|
|
@ -30,7 +46,11 @@ export function PolygonLivePanel() {
|
|||
const [trajectory, setTrajectory] = useState<PolygonVehicleState[]>([]);
|
||||
const [error, setError] = useState<string | null>(null);
|
||||
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 commandInFlight = useRef(false);
|
||||
|
||||
useEffect(() => {
|
||||
if (actionBusy) return;
|
||||
|
|
@ -73,12 +93,75 @@ export function PolygonLivePanel() {
|
|||
|
||||
const runActive = Boolean(worker?.activeRunId);
|
||||
|
||||
useEffect(() => {
|
||||
const runId = worker?.activeRunId;
|
||||
if (!runId || worker.runState !== "running" || !controlIntent) return;
|
||||
const controller = new AbortController();
|
||||
let timer: number | null = null;
|
||||
const publish = async () => {
|
||||
if (commandInFlight.current || controller.signal.aborted) return;
|
||||
commandInFlight.current = true;
|
||||
try {
|
||||
const accepted = await sendPolygonRoverCommand(runId, {
|
||||
speedMps: controlIntent.speedMps,
|
||||
steeringNormalized: controlIntent.steeringNormalized,
|
||||
idempotencyKey: crypto.randomUUID(),
|
||||
signal: controller.signal,
|
||||
});
|
||||
if (controller.signal.aborted) return;
|
||||
setCommandAcceptance(accepted);
|
||||
setError(null);
|
||||
} catch (commandError) {
|
||||
if (controller.signal.aborted) return;
|
||||
setControlIntent(null);
|
||||
setError(message(commandError));
|
||||
} finally {
|
||||
commandInFlight.current = false;
|
||||
if (!controller.signal.aborted) {
|
||||
timer = window.setTimeout(() => void publish(), 100);
|
||||
}
|
||||
}
|
||||
};
|
||||
void publish();
|
||||
return () => {
|
||||
controller.abort();
|
||||
if (timer !== null) window.clearTimeout(timer);
|
||||
};
|
||||
}, [controlIntent, worker?.activeRunId, worker?.runState]);
|
||||
|
||||
useEffect(() => {
|
||||
if (!worker?.activeRunId) {
|
||||
setControlIntent(null);
|
||||
setCommandAcceptance(null);
|
||||
}
|
||||
}, [worker?.activeRunId]);
|
||||
|
||||
const stopMotion = async () => {
|
||||
const runId = worker?.activeRunId;
|
||||
setControlIntent(null);
|
||||
if (!runId || worker?.runState !== "running") return;
|
||||
try {
|
||||
const accepted = await sendPolygonRoverCommand(runId, {
|
||||
speedMps: 0,
|
||||
steeringNormalized: 0,
|
||||
idempotencyKey: crypto.randomUUID(),
|
||||
});
|
||||
setCommandAcceptance(accepted);
|
||||
setError(null);
|
||||
} catch (commandError) {
|
||||
setError(message(commandError));
|
||||
}
|
||||
};
|
||||
|
||||
const runAction = async () => {
|
||||
if (!worker?.controlAvailable || actionBusy) return;
|
||||
setActionBusy(true);
|
||||
setError(null);
|
||||
try {
|
||||
const idempotencyKey = crypto.randomUUID();
|
||||
if (worker.activeRunId) {
|
||||
await stopMotion();
|
||||
}
|
||||
const next = worker.activeRunId
|
||||
? await stopPolygonWorker(worker.activeRunId, { idempotencyKey })
|
||||
: await startPolygonWorker({ idempotencyKey });
|
||||
|
|
@ -136,6 +219,40 @@ export function PolygonLivePanel() {
|
|||
</Suspense>
|
||||
|
||||
<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>
|
||||
<span>Прогон</span>
|
||||
<strong>{worker?.activeRunId ?? "—"}</strong>
|
||||
|
|
@ -156,11 +273,19 @@ export function PolygonLivePanel() {
|
|||
<span>Провайдеры</span>
|
||||
<strong>{worker?.providerIds.join(" · ") || "—"}</strong>
|
||||
</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">
|
||||
<StatusBadge tone="success">Virtual only</StatusBadge>
|
||||
<p>
|
||||
Нет actuator authority. Live-поза диагностическая и пока не доказывает
|
||||
приёмку PX4/ROS 2 telemetry, navigation или safety.
|
||||
Команды проходят PX4 ROS 2 Offboard только в SITL. Физического actuator
|
||||
authority нет; live-поза остаётся диагностической.
|
||||
</p>
|
||||
</div>
|
||||
{error && <p className="polygon-live-error">{error}</p>}
|
||||
|
|
|
|||
|
|
@ -12,8 +12,10 @@ let resolvePolygonRunRoute;
|
|||
let PolygonRunContractError;
|
||||
let decodePolygonWorkerStatus;
|
||||
let decodePolygonVehicleState;
|
||||
let decodePolygonCommandAcceptance;
|
||||
let fetchPolygonWorkerStatus;
|
||||
let fetchPolygonVehicleState;
|
||||
let sendPolygonRoverCommand;
|
||||
let startPolygonWorker;
|
||||
let stopPolygonWorker;
|
||||
let PolygonWorkerContractError;
|
||||
|
|
@ -38,8 +40,10 @@ before(async () => {
|
|||
({
|
||||
decodePolygonWorkerStatus,
|
||||
decodePolygonVehicleState,
|
||||
decodePolygonCommandAcceptance,
|
||||
fetchPolygonWorkerStatus,
|
||||
fetchPolygonVehicleState,
|
||||
sendPolygonRoverCommand,
|
||||
startPolygonWorker,
|
||||
stopPolygonWorker,
|
||||
PolygonWorkerContractError,
|
||||
|
|
@ -223,6 +227,28 @@ function vehicleState(overrides = {}) {
|
|||
};
|
||||
}
|
||||
|
||||
function commandAcceptance(overrides = {}) {
|
||||
return {
|
||||
schema_version: "missioncore.command-acceptance/v1",
|
||||
run_id: "s1c-6cb1495-20260724t180000z-aabbcc",
|
||||
command_id: "cmd-aabbcc",
|
||||
sequence: 1,
|
||||
issued_at_sim_ns: 7_000_000_000,
|
||||
valid_until_sim_ns: 7_250_000_000,
|
||||
speed_mps: 1,
|
||||
steering_normalized: -0.55,
|
||||
authority_scope: "virtual-only",
|
||||
delivery: {
|
||||
provider: "px4-ros2-offboard",
|
||||
mode: "speed-steering",
|
||||
armed: true,
|
||||
offboard: true,
|
||||
ttl_expired_count: 0,
|
||||
},
|
||||
...overrides,
|
||||
};
|
||||
}
|
||||
|
||||
test("Polygon is a worker-gated product root and keeps a compatible direct route", () => {
|
||||
assert.deepEqual(resolvePolygonRunRoute("?workspace=polygon-run"), {
|
||||
active: true,
|
||||
|
|
@ -321,11 +347,14 @@ test("Polygon fetchers use bounded same-origin GET endpoints", async () => {
|
|||
test("Polygon Live decodes only D-only virtual diagnostic state", () => {
|
||||
const status = decodePolygonWorkerStatus(workerStatus());
|
||||
const live = decodePolygonVehicleState(vehicleState());
|
||||
const command = decodePolygonCommandAcceptance(commandAcceptance());
|
||||
|
||||
assert.equal(status.available, true);
|
||||
assert.equal(status.activeRunId, live.runId);
|
||||
assert.equal(status.isolation.artifactPolicy, "d-only");
|
||||
assert.deepEqual(live.position, { x: 1.25, y: -0.5, z: 0.18 });
|
||||
assert.equal(command.offboard, true);
|
||||
assert.equal(command.steeringNormalized, -0.55);
|
||||
assert.throws(
|
||||
() => decodePolygonWorkerStatus(workerStatus({
|
||||
authority: {
|
||||
|
|
@ -344,6 +373,12 @@ test("Polygon Live decodes only D-only virtual diagnostic state", () => {
|
|||
})),
|
||||
PolygonWorkerContractError,
|
||||
);
|
||||
assert.throws(
|
||||
() => decodePolygonCommandAcceptance(commandAcceptance({
|
||||
authority_scope: "physical",
|
||||
})),
|
||||
PolygonWorkerContractError,
|
||||
);
|
||||
});
|
||||
|
||||
test("Polygon Live uses same-origin gateway and idempotent lifecycle requests", async () => {
|
||||
|
|
@ -356,6 +391,7 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests",
|
|||
const fetcher = async (url, init) => {
|
||||
calls.push({ url: String(url), init });
|
||||
if (String(url).endsWith("/live")) return jsonResponse(vehicleState());
|
||||
if (String(url).endsWith("/commands")) return jsonResponse(commandAcceptance());
|
||||
if (String(url).endsWith("/stop")) return jsonResponse(stopped);
|
||||
return jsonResponse(workerStatus());
|
||||
};
|
||||
|
|
@ -363,6 +399,12 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests",
|
|||
await fetchPolygonWorkerStatus({ fetcher });
|
||||
await fetchPolygonVehicleState({ fetcher });
|
||||
await startPolygonWorker({ idempotencyKey: "start-001", fetcher });
|
||||
await sendPolygonRoverCommand("s1c-6cb1495-20260724t180000z-aabbcc", {
|
||||
speedMps: 1,
|
||||
steeringNormalized: -0.55,
|
||||
idempotencyKey: "command-001",
|
||||
fetcher,
|
||||
});
|
||||
await stopPolygonWorker("s1c-6cb1495-20260724t180000z-aabbcc", {
|
||||
idempotencyKey: "stop-001",
|
||||
fetcher,
|
||||
|
|
@ -372,9 +414,15 @@ test("Polygon Live uses same-origin gateway and idempotent lifecycle requests",
|
|||
"/api/v1/polygon/worker",
|
||||
"/api/v1/polygon/worker/live",
|
||||
"/api/v1/polygon/worker/runs",
|
||||
"/api/v1/polygon/worker/runs/s1c-6cb1495-20260724t180000z-aabbcc/commands",
|
||||
"/api/v1/polygon/worker/runs/s1c-6cb1495-20260724t180000z-aabbcc/stop",
|
||||
]);
|
||||
assert.equal(calls[2].init.headers["Idempotency-Key"], "start-001");
|
||||
assert.equal(calls[3].init.headers["Idempotency-Key"], "stop-001");
|
||||
assert.equal(calls[3].init.headers["Idempotency-Key"], "command-001");
|
||||
assert.equal(calls[4].init.headers["Idempotency-Key"], "stop-001");
|
||||
assert.equal(calls[2].init.body, JSON.stringify({ scenario_id: "stock-rover-ackermann" }));
|
||||
assert.equal(calls[3].init.body, JSON.stringify({
|
||||
speed_mps: 1,
|
||||
steering_normalized: -0.55,
|
||||
}));
|
||||
});
|
||||
|
|
|
|||
|
|
@ -12,6 +12,8 @@ readonly SOURCE_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/../.." && pwd -
|
|||
readonly MISSION_CORE_COMMIT="${1:-}"
|
||||
readonly SOCKET_ROOT="/run/missioncore-sim"
|
||||
readonly SOCKET_PATH="${SOCKET_ROOT}/worker.sock"
|
||||
readonly ROS_SETUP="/opt/ros/jazzy/setup.bash"
|
||||
readonly PX4_MSGS_SETUP="${D_ROOT}/build/ros2/px4_msgs/install/setup.bash"
|
||||
|
||||
if [[ -z "${MISSION_CORE_COMMIT}" ]]; then
|
||||
echo "usage: $0 <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
|
||||
exit 2
|
||||
fi
|
||||
if [[ ! -f "${ROS_SETUP}" || ! -f "${PX4_MSGS_SETUP}" ]]; then
|
||||
echo "error: reviewed ROS 2 Jazzy/px4_msgs runtime is incomplete" >&2
|
||||
exit 2
|
||||
fi
|
||||
|
||||
# shellcheck disable=SC1091
|
||||
source "${ROS_SETUP}"
|
||||
# shellcheck disable=SC1091
|
||||
source "${PX4_MSGS_SETUP}"
|
||||
|
||||
exec env \
|
||||
PYTHONPATH="${SOURCE_ROOT}/src" \
|
||||
ROS_DOMAIN_ID=0 \
|
||||
PYTHONPATH="${SOURCE_ROOT}/src:${PYTHONPATH:-}" \
|
||||
python3 -m k1link.simulation.worker_agent \
|
||||
--socket "${SOCKET_PATH}" \
|
||||
--mission-core-commit "${MISSION_CORE_COMMIT}" \
|
||||
|
|
|
|||
|
|
@ -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 threading
|
||||
import time
|
||||
from contextlib import suppress
|
||||
from datetime import UTC, datetime
|
||||
from pathlib import Path
|
||||
from types import FrameType
|
||||
from typing import Any, Final
|
||||
|
||||
from k1link.simulation.contracts import (
|
||||
AckermannControlSetpoint,
|
||||
AuthorityProfile,
|
||||
ProviderPin,
|
||||
QualificationRun,
|
||||
|
|
@ -26,6 +28,10 @@ from k1link.simulation.contracts import (
|
|||
)
|
||||
from k1link.simulation.orchestrator import SimulationApplicationService
|
||||
from k1link.simulation.process_supervisor import PosixProcessSupervisor
|
||||
from k1link.simulation.px4_rover_control import (
|
||||
Px4RoverControlSnapshot,
|
||||
Ros2Px4AckermannControl,
|
||||
)
|
||||
from k1link.simulation.run_store import (
|
||||
ACTIVE_RECOVERY_STATES,
|
||||
QualificationRunNotFoundError,
|
||||
|
|
@ -44,6 +50,7 @@ from k1link.simulation.worker import (
|
|||
SimulationWorldControl,
|
||||
)
|
||||
from k1link.simulation.worker_gateway import (
|
||||
COMMAND_ACCEPTANCE_SCHEMA,
|
||||
MAX_MESSAGE_BYTES,
|
||||
OPERATIONS,
|
||||
REQUEST_SCHEMA,
|
||||
|
|
@ -213,6 +220,8 @@ class SimulationWorkerAgent:
|
|||
)
|
||||
self.service = SimulationApplicationService(self.store, self.worker)
|
||||
self.collector = GazeboPoseCollector(stock_rover_process_environment(self.paths))
|
||||
self.command_controller = Ros2Px4AckermannControl()
|
||||
self._command_lock = threading.RLock()
|
||||
self._shutdown = threading.Event()
|
||||
self._server_socket: socket.socket | None = None
|
||||
self._connection_threads: set[threading.Thread] = set()
|
||||
|
|
@ -258,6 +267,8 @@ class SimulationWorkerAgent:
|
|||
self._shutdown.set()
|
||||
active = self._active_run()
|
||||
if active is not None:
|
||||
with suppress(Exception):
|
||||
self.command_controller.stop(active.run_id)
|
||||
try:
|
||||
self.service.stop(
|
||||
active.run_id,
|
||||
|
|
@ -309,6 +320,23 @@ class SimulationWorkerAgent:
|
|||
mission_core_commit=_string(payload["mission_core_commit"], "commit", 40),
|
||||
idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160),
|
||||
)
|
||||
if operation == "command":
|
||||
_exact_keys(
|
||||
payload,
|
||||
{"run_id", "speed_mps", "steering_normalized", "idempotency_key"},
|
||||
"command payload",
|
||||
)
|
||||
return self.command(
|
||||
run_id=_safe_id(payload["run_id"], "run id"),
|
||||
speed_mps=_bounded_number(payload["speed_mps"], "speed", -1.5, 1.5),
|
||||
steering_normalized=_bounded_number(
|
||||
payload["steering_normalized"],
|
||||
"steering",
|
||||
-1.0,
|
||||
1.0,
|
||||
),
|
||||
idempotency_key=_string(payload["idempotency_key"], "idempotency key", 160),
|
||||
)
|
||||
_exact_keys(payload, {"run_id", "idempotency_key"}, "stop payload")
|
||||
return self.stop(
|
||||
run_id=_safe_id(payload["run_id"], "run id"),
|
||||
|
|
@ -367,9 +395,22 @@ class SimulationWorkerAgent:
|
|||
host_monotonic_ns=time.monotonic_ns(),
|
||||
)
|
||||
if running.state is not RunState.RUNNING:
|
||||
raise WorkerAgentError(
|
||||
f"provider start ended in terminal state {running.state.value}"
|
||||
)
|
||||
raise WorkerAgentError(f"provider start ended in terminal state {running.state.value}")
|
||||
try:
|
||||
self.command_controller.start(run_id)
|
||||
except Exception as exc:
|
||||
try:
|
||||
self.service.stop(
|
||||
run_id,
|
||||
idempotency_key=f"{run_id}:command-controller-admission-failed",
|
||||
observed_at_utc=_utc_now(),
|
||||
host_monotonic_ns=time.monotonic_ns(),
|
||||
sim_time_ns=None,
|
||||
terminal_state=RunState.ABORTED,
|
||||
reason="px4-command-controller-admission-failed",
|
||||
)
|
||||
finally:
|
||||
raise WorkerAgentError("PX4 command controller admission failed") from exc
|
||||
return self.status()
|
||||
|
||||
def stop(self, *, run_id: str, idempotency_key: str) -> dict[str, Any]:
|
||||
|
|
@ -381,19 +422,109 @@ class SimulationWorkerAgent:
|
|||
sim_time_ns = int(self.collector.collect(run_id)["sim_time_ns"])
|
||||
except Exception:
|
||||
sim_time_ns = None
|
||||
command_stop_error: Exception | None = None
|
||||
try:
|
||||
self.command_controller.stop(run_id)
|
||||
except Exception as exc:
|
||||
command_stop_error = exc
|
||||
terminal = self.service.stop(
|
||||
run_id,
|
||||
idempotency_key=idempotency_key,
|
||||
observed_at_utc=_utc_now(),
|
||||
host_monotonic_ns=time.monotonic_ns(),
|
||||
sim_time_ns=sim_time_ns,
|
||||
terminal_state=(
|
||||
RunState.ABORTED if command_stop_error is not None else RunState.COMPLETED
|
||||
),
|
||||
reason=(
|
||||
"px4-command-controller-stop-failed"
|
||||
if command_stop_error is not None
|
||||
else "operator-stop-clean"
|
||||
),
|
||||
)
|
||||
if terminal.state is not RunState.COMPLETED:
|
||||
if command_stop_error is not None:
|
||||
raise WorkerAgentError(
|
||||
f"provider stop ended in terminal state {terminal.state.value}"
|
||||
)
|
||||
"PX4 command controller did not stop cleanly"
|
||||
) from command_stop_error
|
||||
if terminal.state is not RunState.COMPLETED:
|
||||
raise WorkerAgentError(f"provider stop ended in terminal state {terminal.state.value}")
|
||||
return self.status()
|
||||
|
||||
def command(
|
||||
self,
|
||||
*,
|
||||
run_id: str,
|
||||
speed_mps: float,
|
||||
steering_normalized: float,
|
||||
idempotency_key: str,
|
||||
) -> dict[str, Any]:
|
||||
with self._command_lock:
|
||||
return self._command_locked(
|
||||
run_id=run_id,
|
||||
speed_mps=speed_mps,
|
||||
steering_normalized=steering_normalized,
|
||||
idempotency_key=idempotency_key,
|
||||
)
|
||||
|
||||
def _command_locked(
|
||||
self,
|
||||
*,
|
||||
run_id: str,
|
||||
speed_mps: float,
|
||||
steering_normalized: float,
|
||||
idempotency_key: str,
|
||||
) -> dict[str, Any]:
|
||||
active = self._active_run()
|
||||
if active is None or active.run_id != run_id or active.state is not RunState.RUNNING:
|
||||
raise WorkerAgentError("requested run does not own running command authority")
|
||||
command_id = f"cmd-{hashlib.sha256(idempotency_key.encode()).hexdigest()[:24]}"
|
||||
existing = next(
|
||||
(item for item in self.store.list_commands(run_id) if item.command_id == command_id),
|
||||
None,
|
||||
)
|
||||
if existing is not None:
|
||||
if not isinstance(existing, AckermannControlSetpoint) or (
|
||||
existing.speed_mps != speed_mps
|
||||
or existing.steering_normalized != steering_normalized
|
||||
):
|
||||
raise WorkerAgentError("command idempotency key is bound to another setpoint")
|
||||
snapshot = self.command_controller.snapshot(run_id)
|
||||
return _command_acceptance(existing, snapshot)
|
||||
sample = self.collector.collect(run_id)
|
||||
issued_at_sim_ns = int(sample["sim_time_ns"])
|
||||
ttl_ns = min(active.authority.command_ttl_max_ns, 250_000_000)
|
||||
command = AckermannControlSetpoint(
|
||||
run_id=run_id,
|
||||
command_id=command_id,
|
||||
sequence=len(self.store.list_commands(run_id)) + 1,
|
||||
issued_at_sim_ns=issued_at_sim_ns,
|
||||
valid_until_sim_ns=issued_at_sim_ns + ttl_ns,
|
||||
authority_generation=active.authority.generation,
|
||||
speed_mps=speed_mps,
|
||||
steering_normalized=steering_normalized,
|
||||
source="mission-core-control-station",
|
||||
)
|
||||
self.store.submit_command(command)
|
||||
try:
|
||||
snapshot = self.command_controller.submit(command)
|
||||
except Exception as exc:
|
||||
current = self.store.load(run_id)
|
||||
self.store.append_event(
|
||||
run_id,
|
||||
event_type="command.delivery-failed",
|
||||
observed_at_utc=_utc_now(),
|
||||
host_monotonic_ns=time.monotonic_ns(),
|
||||
sim_time_ns=issued_at_sim_ns,
|
||||
payload={
|
||||
"command_id": command.command_id,
|
||||
"provider": "px4-ros2-offboard",
|
||||
"detail": type(exc).__name__,
|
||||
},
|
||||
expected_revision=current.revision,
|
||||
)
|
||||
raise WorkerAgentError("PX4 rejected the canonical rover setpoint") from exc
|
||||
return _command_acceptance(command, snapshot)
|
||||
|
||||
def _handle_connection(self, connection: socket.socket) -> None:
|
||||
request_id: str | None = None
|
||||
try:
|
||||
|
|
@ -561,6 +692,18 @@ def _number(value: object, label: str) -> float:
|
|||
return result
|
||||
|
||||
|
||||
def _bounded_number(
|
||||
value: object,
|
||||
label: str,
|
||||
minimum: float,
|
||||
maximum: float,
|
||||
) -> float:
|
||||
result = _number(value, label)
|
||||
if not minimum <= result <= maximum:
|
||||
raise WorkerAgentError(f"{label} is outside the admitted range")
|
||||
return result
|
||||
|
||||
|
||||
def _integer(value: object, label: str) -> int:
|
||||
if isinstance(value, bool) or not isinstance(value, int | str):
|
||||
raise WorkerAgentError(f"{label} must be an integer")
|
||||
|
|
@ -592,6 +735,30 @@ def _safe_error(exc: Exception) -> str:
|
|||
return f"{type(exc).__name__}: worker operation failed"
|
||||
|
||||
|
||||
def _command_acceptance(
|
||||
command: AckermannControlSetpoint,
|
||||
snapshot: Px4RoverControlSnapshot,
|
||||
) -> dict[str, Any]:
|
||||
return {
|
||||
"schema_version": COMMAND_ACCEPTANCE_SCHEMA,
|
||||
"run_id": command.run_id,
|
||||
"command_id": command.command_id,
|
||||
"sequence": command.sequence,
|
||||
"issued_at_sim_ns": command.issued_at_sim_ns,
|
||||
"valid_until_sim_ns": command.valid_until_sim_ns,
|
||||
"speed_mps": command.speed_mps,
|
||||
"steering_normalized": command.steering_normalized,
|
||||
"authority_scope": command.authority_scope.value,
|
||||
"delivery": {
|
||||
"provider": "px4-ros2-offboard",
|
||||
"mode": "speed-steering",
|
||||
"armed": snapshot.armed,
|
||||
"offboard": snapshot.offboard,
|
||||
"ttl_expired_count": snapshot.ttl_expired_count,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="Serve the Mission Core Simulation Worker agent.")
|
||||
parser.add_argument("--socket", type=Path, required=True)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
import socket
|
||||
from collections.abc import Mapping
|
||||
from pathlib import Path
|
||||
|
|
@ -11,8 +12,9 @@ REQUEST_SCHEMA: Final = "missioncore.simulation-worker-request/v1"
|
|||
RESPONSE_SCHEMA: Final = "missioncore.simulation-worker-response/v1"
|
||||
STATUS_SCHEMA: Final = "missioncore.simulation-worker-status/v1"
|
||||
VEHICLE_STATE_SCHEMA: Final = "missioncore.vehicle-state/v1"
|
||||
COMMAND_ACCEPTANCE_SCHEMA: Final = "missioncore.command-acceptance/v1"
|
||||
MAX_MESSAGE_BYTES: Final = 64 * 1024
|
||||
OPERATIONS: Final = frozenset({"status", "live", "start", "stop"})
|
||||
OPERATIONS: Final = frozenset({"status", "live", "start", "command", "stop"})
|
||||
|
||||
|
||||
class SimulationWorkerGatewayError(RuntimeError):
|
||||
|
|
@ -47,6 +49,15 @@ class PolygonWorkerGateway(Protocol):
|
|||
idempotency_key: str,
|
||||
) -> dict[str, Any]: ...
|
||||
|
||||
def command(
|
||||
self,
|
||||
*,
|
||||
run_id: str,
|
||||
speed_mps: float,
|
||||
steering_normalized: float,
|
||||
idempotency_key: str,
|
||||
) -> dict[str, Any]: ...
|
||||
|
||||
|
||||
class UnixSocketWorkerGateway:
|
||||
"""Bounded request/response transport between Mission Core and one local worker."""
|
||||
|
|
@ -113,6 +124,27 @@ class UnixSocketWorkerGateway:
|
|||
_validate_status(result)
|
||||
return result
|
||||
|
||||
def command(
|
||||
self,
|
||||
*,
|
||||
run_id: str,
|
||||
speed_mps: float,
|
||||
steering_normalized: float,
|
||||
idempotency_key: str,
|
||||
) -> dict[str, Any]:
|
||||
result = self._request(
|
||||
"command",
|
||||
{
|
||||
"run_id": run_id,
|
||||
"speed_mps": speed_mps,
|
||||
"steering_normalized": steering_normalized,
|
||||
"idempotency_key": idempotency_key,
|
||||
},
|
||||
timeout_seconds=self.request_timeout_seconds,
|
||||
)
|
||||
_validate_command_acceptance(result)
|
||||
return result
|
||||
|
||||
def _request(
|
||||
self,
|
||||
operation: str,
|
||||
|
|
@ -258,3 +290,55 @@ def _validate_vehicle_state(value: Mapping[str, Any]) -> None:
|
|||
or not isinstance(value.get("safety"), dict)
|
||||
):
|
||||
raise SimulationWorkerGatewayError("vehicle state contains invalid values")
|
||||
|
||||
|
||||
def _validate_command_acceptance(value: Mapping[str, Any]) -> None:
|
||||
expected = {
|
||||
"schema_version",
|
||||
"run_id",
|
||||
"command_id",
|
||||
"sequence",
|
||||
"issued_at_sim_ns",
|
||||
"valid_until_sim_ns",
|
||||
"speed_mps",
|
||||
"steering_normalized",
|
||||
"authority_scope",
|
||||
"delivery",
|
||||
}
|
||||
if set(value) != expected or value.get("schema_version") != COMMAND_ACCEPTANCE_SCHEMA:
|
||||
raise SimulationWorkerGatewayError("command acceptance does not match v1")
|
||||
delivery = value.get("delivery")
|
||||
numeric_fields = (
|
||||
"sequence",
|
||||
"issued_at_sim_ns",
|
||||
"valid_until_sim_ns",
|
||||
"speed_mps",
|
||||
"steering_normalized",
|
||||
)
|
||||
if (
|
||||
not isinstance(value.get("run_id"), str)
|
||||
or not isinstance(value.get("command_id"), str)
|
||||
or any(
|
||||
isinstance(value.get(field), bool)
|
||||
or not isinstance(value.get(field), int | float)
|
||||
or not math.isfinite(value[field])
|
||||
for field in numeric_fields
|
||||
)
|
||||
or value.get("authority_scope") != "virtual-only"
|
||||
or not isinstance(delivery, dict)
|
||||
or set(delivery)
|
||||
!= {
|
||||
"provider",
|
||||
"mode",
|
||||
"armed",
|
||||
"offboard",
|
||||
"ttl_expired_count",
|
||||
}
|
||||
or delivery.get("provider") != "px4-ros2-offboard"
|
||||
or delivery.get("mode") != "speed-steering"
|
||||
or not isinstance(delivery.get("armed"), bool)
|
||||
or not isinstance(delivery.get("offboard"), bool)
|
||||
or isinstance(delivery.get("ttl_expired_count"), bool)
|
||||
or not isinstance(delivery.get("ttl_expired_count"), int)
|
||||
):
|
||||
raise SimulationWorkerGatewayError("command acceptance contains invalid values")
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ from typing import Annotated, Any, Final
|
|||
|
||||
from fastapi import APIRouter, Header, HTTPException, Query
|
||||
from fastapi import Path as PathParameter
|
||||
from pydantic import BaseModel, ConfigDict
|
||||
from pydantic import BaseModel, ConfigDict, Field
|
||||
|
||||
from k1link.simulation import (
|
||||
QualificationRun,
|
||||
|
|
@ -54,6 +54,13 @@ class StartStockRoverRequest(BaseModel):
|
|||
scenario_id: str
|
||||
|
||||
|
||||
class AckermannCommandRequest(BaseModel):
|
||||
model_config = ConfigDict(extra="forbid", frozen=True)
|
||||
|
||||
speed_mps: float = Field(ge=-1.5, le=1.5, allow_inf_nan=False)
|
||||
steering_normalized: float = Field(ge=-1.0, le=1.0, allow_inf_nan=False)
|
||||
|
||||
|
||||
def configured_polygon_runs_root() -> Path | None:
|
||||
raw = os.environ.get(POLYGON_RUNS_ROOT_ENV)
|
||||
if raw is None or not raw.strip():
|
||||
|
|
@ -250,6 +257,40 @@ def build_polygon_router(
|
|||
detail="Simulation Worker не подтвердил остановку.",
|
||||
) from exc
|
||||
|
||||
@router.post("/worker/runs/{run_id}/commands")
|
||||
def command_polygon_worker_run(
|
||||
run_id: Annotated[
|
||||
str,
|
||||
PathParameter(pattern=r"^[a-z0-9][a-z0-9-]{0,63}$"),
|
||||
],
|
||||
request: AckermannCommandRequest,
|
||||
idempotency_key: Annotated[
|
||||
str,
|
||||
Header(alias="Idempotency-Key", min_length=1, max_length=160),
|
||||
],
|
||||
) -> dict[str, Any]:
|
||||
gateway, _ = _control_context(
|
||||
worker_provider,
|
||||
control_provider,
|
||||
commit_provider,
|
||||
)
|
||||
try:
|
||||
return gateway.command(
|
||||
run_id=run_id,
|
||||
speed_mps=request.speed_mps,
|
||||
steering_normalized=request.steering_normalized,
|
||||
idempotency_key=idempotency_key,
|
||||
)
|
||||
except SimulationWorkerUnavailableError as exc:
|
||||
raise HTTPException(status_code=503, detail="Simulation Worker недоступен.") from exc
|
||||
except SimulationWorkerRejectedError as exc:
|
||||
raise HTTPException(status_code=409, detail=str(exc)) from exc
|
||||
except SimulationWorkerGatewayError as exc:
|
||||
raise HTTPException(
|
||||
status_code=502,
|
||||
detail="Simulation Worker не подтвердил команду ровера.",
|
||||
) from exc
|
||||
|
||||
return router
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,11 @@ from k1link.simulation import (
|
|||
RunKind,
|
||||
RunState,
|
||||
)
|
||||
from k1link.web.polygon_api import StartStockRoverRequest, build_polygon_router
|
||||
from k1link.web.polygon_api import (
|
||||
AckermannCommandRequest,
|
||||
StartStockRoverRequest,
|
||||
build_polygon_router,
|
||||
)
|
||||
|
||||
SHA_A = "a" * 64
|
||||
SHA_B = "b" * 64
|
||||
|
|
@ -241,6 +245,35 @@ class _FakeWorkerGateway:
|
|||
self.active_run_id = None
|
||||
return self._status()
|
||||
|
||||
def command(
|
||||
self,
|
||||
*,
|
||||
run_id: str,
|
||||
speed_mps: float,
|
||||
steering_normalized: float,
|
||||
idempotency_key: str,
|
||||
) -> dict[str, Any]:
|
||||
self.calls.append(("command", idempotency_key))
|
||||
assert run_id == self.active_run_id
|
||||
return {
|
||||
"schema_version": "missioncore.command-acceptance/v1",
|
||||
"run_id": run_id,
|
||||
"command_id": "cmd-test",
|
||||
"sequence": 1,
|
||||
"issued_at_sim_ns": 1_000_000,
|
||||
"valid_until_sim_ns": 251_000_000,
|
||||
"speed_mps": speed_mps,
|
||||
"steering_normalized": steering_normalized,
|
||||
"authority_scope": "virtual-only",
|
||||
"delivery": {
|
||||
"provider": "px4-ros2-offboard",
|
||||
"mode": "speed-steering",
|
||||
"armed": True,
|
||||
"offboard": True,
|
||||
"ttl_expired_count": 0,
|
||||
},
|
||||
}
|
||||
|
||||
def _status(self) -> dict[str, Any]:
|
||||
return {
|
||||
"schema_version": "missioncore.simulation-worker-status/v1",
|
||||
|
|
@ -297,12 +330,34 @@ def test_polygon_worker_api_fails_closed_then_proxies_virtual_only_lifecycle() -
|
|||
live = _endpoint(enabled, "/api/v1/polygon/worker/live", "GET")()
|
||||
assert live["run_id"] == running["active_run_id"]
|
||||
assert live["source"]["quality"] == "diagnostic"
|
||||
command = _endpoint(
|
||||
enabled,
|
||||
"/api/v1/polygon/worker/runs/{run_id}/commands",
|
||||
"POST",
|
||||
)(
|
||||
run_id=running["active_run_id"],
|
||||
request=AckermannCommandRequest(speed_mps=1.0, steering_normalized=-0.25),
|
||||
idempotency_key="command-001",
|
||||
)
|
||||
assert command["authority_scope"] == "virtual-only"
|
||||
assert command["delivery"]["offboard"] is True
|
||||
stopped = _endpoint(enabled, "/api/v1/polygon/worker/runs/{run_id}/stop", "POST")(
|
||||
run_id=running["active_run_id"],
|
||||
idempotency_key="stop-001",
|
||||
)
|
||||
assert stopped["active_run_id"] is None
|
||||
assert worker.calls == [("start", "start-001"), ("stop", "stop-001")]
|
||||
assert worker.calls == [
|
||||
("start", "start-001"),
|
||||
("command", "command-001"),
|
||||
("stop", "stop-001"),
|
||||
]
|
||||
|
||||
|
||||
def test_polygon_worker_command_rejects_values_outside_lab_envelope() -> None:
|
||||
with pytest.raises(ValueError):
|
||||
AckermannCommandRequest(speed_mps=1.51, steering_normalized=0)
|
||||
with pytest.raises(ValueError):
|
||||
AckermannCommandRequest(speed_mps=0, steering_normalized=-1.01)
|
||||
|
||||
|
||||
def test_polygon_worker_status_is_explicitly_unavailable_when_not_registered() -> None:
|
||||
|
|
|
|||
|
|
@ -0,0 +1,195 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
from math import isnan
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from k1link.simulation.contracts import (
|
||||
AckermannControlSetpoint,
|
||||
AuthorityProfile,
|
||||
ProviderPin,
|
||||
QualificationRun,
|
||||
ReproducibilityTier,
|
||||
RunKind,
|
||||
RunState,
|
||||
)
|
||||
from k1link.simulation.px4_rover_control import (
|
||||
Px4RoverControlSnapshot,
|
||||
Ros2Px4AckermannControl,
|
||||
)
|
||||
from k1link.simulation.run_store import QualificationRunStore
|
||||
from k1link.simulation.worker_agent import SimulationWorkerAgent, WorkerAgentError
|
||||
|
||||
|
||||
class _Collector:
|
||||
def collect(self, run_id: str) -> dict[str, object]:
|
||||
return {"run_id": run_id, "sim_time_ns": 1_000_000_000}
|
||||
|
||||
|
||||
class _Controller:
|
||||
def __init__(self) -> None:
|
||||
self.commands: list[object] = []
|
||||
|
||||
def submit(self, command: AckermannControlSetpoint) -> Px4RoverControlSnapshot:
|
||||
self.commands.append(command)
|
||||
return Px4RoverControlSnapshot(
|
||||
run_id=command.run_id,
|
||||
armed=True,
|
||||
offboard=True,
|
||||
ttl_expired_count=0,
|
||||
)
|
||||
|
||||
def snapshot(self, run_id: str) -> Px4RoverControlSnapshot:
|
||||
return Px4RoverControlSnapshot(
|
||||
run_id=run_id,
|
||||
armed=True,
|
||||
offboard=True,
|
||||
ttl_expired_count=0,
|
||||
)
|
||||
|
||||
|
||||
class _Message:
|
||||
pass
|
||||
|
||||
|
||||
class _Messages:
|
||||
OffboardControlMode = _Message
|
||||
RoverSpeedSetpoint = _Message
|
||||
RoverSteeringSetpoint = _Message
|
||||
RoverAttitudeSetpoint = _Message
|
||||
RoverRateSetpoint = _Message
|
||||
|
||||
|
||||
class _Publisher:
|
||||
def __init__(self) -> None:
|
||||
self.messages: list[_Message] = []
|
||||
|
||||
def publish(self, message: _Message) -> None:
|
||||
self.messages.append(message)
|
||||
|
||||
|
||||
def _running_store(root: Path) -> QualificationRunStore:
|
||||
store = QualificationRunStore(root)
|
||||
admitted = store.create(
|
||||
QualificationRun(
|
||||
run_id="s1c-command-test",
|
||||
episode_id="episode-s1c-command-test",
|
||||
kind=RunKind.SIMULATION_CLOSED_LOOP,
|
||||
state=RunState.ADMITTED,
|
||||
scenario_generation="stock-rover",
|
||||
scenario_sha256="a" * 64,
|
||||
profile_generation="stock-rover-lifecycle-v1",
|
||||
profile_sha256="b" * 64,
|
||||
mission_core_commit="c" * 40,
|
||||
providers=(
|
||||
ProviderPin("px4-autopilot", "v1.17.0", "v1.17.0"),
|
||||
ProviderPin("gazebo", "harmonic", "8.14.0"),
|
||||
),
|
||||
host_profile_id="mission-gpu-s0",
|
||||
host_profile_sha256="d" * 64,
|
||||
seed=42,
|
||||
reproducibility_tier=ReproducibilityTier.R1,
|
||||
authority=AuthorityProfile(
|
||||
generation=1,
|
||||
command_ttl_max_ns=250_000_000,
|
||||
heartbeat_timeout_monotonic_ns=500_000_000,
|
||||
),
|
||||
clock_domain="gazebo:/clock",
|
||||
created_at_utc="2026-07-24T18:00:00Z",
|
||||
)
|
||||
)
|
||||
starting = store.transition(
|
||||
admitted.run_id,
|
||||
RunState.STARTING,
|
||||
expected_revision=0,
|
||||
observed_at_utc="2026-07-24T18:00:01Z",
|
||||
host_monotonic_ns=1,
|
||||
)
|
||||
store.transition(
|
||||
admitted.run_id,
|
||||
RunState.RUNNING,
|
||||
expected_revision=starting.revision,
|
||||
observed_at_utc="2026-07-24T18:00:02Z",
|
||||
host_monotonic_ns=2,
|
||||
sim_time_ns=0,
|
||||
)
|
||||
return store
|
||||
|
||||
|
||||
def test_worker_agent_journals_and_idempotently_delivers_virtual_command(
|
||||
tmp_path: Path,
|
||||
) -> None:
|
||||
agent = object.__new__(SimulationWorkerAgent)
|
||||
agent.store = _running_store(tmp_path)
|
||||
agent.collector = _Collector()
|
||||
controller = _Controller()
|
||||
agent.command_controller = controller
|
||||
agent._command_lock = threading.RLock()
|
||||
|
||||
accepted = agent.command(
|
||||
run_id="s1c-command-test",
|
||||
speed_mps=1.0,
|
||||
steering_normalized=-0.25,
|
||||
idempotency_key="browser-intent-001",
|
||||
)
|
||||
repeated = agent.command(
|
||||
run_id="s1c-command-test",
|
||||
speed_mps=1.0,
|
||||
steering_normalized=-0.25,
|
||||
idempotency_key="browser-intent-001",
|
||||
)
|
||||
|
||||
assert accepted == repeated
|
||||
assert accepted["authority_scope"] == "virtual-only"
|
||||
assert accepted["delivery"]["provider"] == "px4-ros2-offboard"
|
||||
assert accepted["delivery"]["armed"] is True
|
||||
commands = agent.store.list_commands("s1c-command-test")
|
||||
assert len(commands) == 1
|
||||
assert commands[0].valid_until_sim_ns - commands[0].issued_at_sim_ns == 250_000_000
|
||||
assert len(controller.commands) == 1
|
||||
|
||||
|
||||
def test_worker_agent_command_dispatch_fails_closed_on_unsafe_envelope() -> None:
|
||||
agent = object.__new__(SimulationWorkerAgent)
|
||||
|
||||
with pytest.raises(WorkerAgentError, match="outside"):
|
||||
agent.dispatch(
|
||||
{
|
||||
"schema_version": "missioncore.simulation-worker-request/v1",
|
||||
"request_id": "request-001",
|
||||
"operation": "command",
|
||||
"payload": {
|
||||
"run_id": "s1c-command-test",
|
||||
"speed_mps": 1.51,
|
||||
"steering_normalized": 0.0,
|
||||
"idempotency_key": "browser-intent-002",
|
||||
},
|
||||
}
|
||||
)
|
||||
|
||||
|
||||
def test_px4_adapter_publishes_speed_steering_without_direct_actuators() -> None:
|
||||
publishers = [_Publisher() for _ in range(5)]
|
||||
|
||||
Ros2Px4AckermannControl._publish_setpoint(
|
||||
_Messages,
|
||||
*publishers,
|
||||
123,
|
||||
1.0,
|
||||
-0.55,
|
||||
)
|
||||
|
||||
offboard = publishers[0].messages[0]
|
||||
speed = publishers[1].messages[0]
|
||||
steering = publishers[2].messages[0]
|
||||
attitude = publishers[3].messages[0]
|
||||
rate = publishers[4].messages[0]
|
||||
assert offboard.velocity is True
|
||||
assert offboard.direct_actuator is False
|
||||
assert speed.speed_body_x == 1.0
|
||||
assert isnan(speed.speed_body_y)
|
||||
assert steering.normalized_steering_setpoint == -0.55
|
||||
assert isnan(attitude.yaw_setpoint)
|
||||
assert isnan(rate.yaw_rate_setpoint)
|
||||
|
|
@ -42,6 +42,27 @@ def _status() -> dict[str, Any]:
|
|||
}
|
||||
|
||||
|
||||
def _command_acceptance() -> dict[str, Any]:
|
||||
return {
|
||||
"schema_version": "missioncore.command-acceptance/v1",
|
||||
"run_id": "s1c-command-test",
|
||||
"command_id": "cmd-001",
|
||||
"sequence": 1,
|
||||
"issued_at_sim_ns": 1_000_000,
|
||||
"valid_until_sim_ns": 251_000_000,
|
||||
"speed_mps": 1.0,
|
||||
"steering_normalized": -0.25,
|
||||
"authority_scope": "virtual-only",
|
||||
"delivery": {
|
||||
"provider": "px4-ros2-offboard",
|
||||
"mode": "speed-steering",
|
||||
"armed": True,
|
||||
"offboard": True,
|
||||
"ttl_expired_count": 0,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def _serve_once(
|
||||
socket_path: Path,
|
||||
result: dict[str, Any],
|
||||
|
|
@ -121,3 +142,29 @@ def test_unix_worker_gateway_reports_missing_worker_without_path_disclosure(
|
|||
with pytest.raises(SimulationWorkerUnavailableError) as failure:
|
||||
gateway.status()
|
||||
assert str(tmp_path) not in str(failure.value)
|
||||
|
||||
|
||||
def test_unix_worker_gateway_sends_bounded_virtual_rover_command() -> None:
|
||||
socket_path = Path(f"/tmp/mc-{uuid4().hex}.sock")
|
||||
thread, requests = _serve_once(socket_path, _command_acceptance())
|
||||
gateway = UnixSocketWorkerGateway(socket_path)
|
||||
|
||||
try:
|
||||
accepted = gateway.command(
|
||||
run_id="s1c-command-test",
|
||||
speed_mps=1.0,
|
||||
steering_normalized=-0.25,
|
||||
idempotency_key="command-001",
|
||||
)
|
||||
thread.join(timeout=2)
|
||||
|
||||
assert accepted["delivery"]["offboard"] is True
|
||||
assert requests[0]["operation"] == "command"
|
||||
assert requests[0]["payload"] == {
|
||||
"run_id": "s1c-command-test",
|
||||
"speed_mps": 1.0,
|
||||
"steering_normalized": -0.25,
|
||||
"idempotency_key": "command-001",
|
||||
}
|
||||
finally:
|
||||
socket_path.unlink(missing_ok=True)
|
||||
|
|
|
|||
Loading…
Reference in New Issue