SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(simulation): drive stock rover through PX4 offboard

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-24 21:47:40 +03:00
parent 53fd1e3bfc
commit 101707d54c
12 changed files with 1329 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/control-station/src/core/polygon/liveWorker.ts
+134
View File
@@ -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,
));
}
apps/control-station/src/styles/workspaces.css
+38
View File
@@ -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;
apps/control-station/src/workspaces/PolygonLivePanel.tsx
+128 -3
View File
@@ -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>}
apps/control-station/test/polygonRunArchive.test.mjs
+49 -1
View File
@@ -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,
}));
});