feat(simulation): drive stock rover through PX4 offboard

2026-07-24 21:47:40 +03:00
parent 53fd1e3bfc
commit 101707d54c
12 changed files with 1329 additions and 15 deletions
@@ -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,
+  ));
+}