also looking at this
feat: add shared-clock scoped vehicle simulation sessions
#98
42 files
+2764
-127
| @@ -1,5 +1,6 @@ | ||
| 1 | 1 | import type { VehicleMissionAction, VehicleMissionObservation, VehicleMissionPlan, VehicleMissionSnapshot, VehicleMissionTask, VehiclePayloadAck, VehiclePayloadCommand, VehicleVtolTransitionCommand, VehicleVtolTransitionObservation } from '../vehicles/types' |
| 2 | 2 | import type { MissionState } from './engine' |
| 3 | import { vehicleKinds } from '../vehicles/types' | |
| 3 | 4 | import { authorityStates } from '../authority/supervisor' |
| 4 | 5 | import { validVector } from '../controls/model' |
| 5 | 6 | import { missionGraphBudget } from './graph' |
| @@ -15,7 +16,7 @@ export function vehicleMissionContains(plan: VehicleMissionPlan, p: unknown) { | ||
| 15 | 16 | export function vehicleTaskTarget(plan: VehicleMissionPlan, task: VehicleMissionTask) { return task.kind === 'return' ? [...plan.rallyM] : task.kind === 'vtol-mode' && task.approach ? [...task.anchorM!] : 'positionM' in task ? [...task.positionM] : null } |
| 16 | 17 | export function validateVehicleMission(input: unknown): { plan: VehicleMissionPlan, worstCaseMs: number, requiredWh: number } { |
| 17 | 18 | function invalid(): never { throw new TypeError('Invalid vehicle mission or unsupported profile') } |
| 18 | if (!object(input) || !keys(input, ['version', 'vehicle', 'id', 'frame', 'altitude', 'terrain', 'origin', 'fence', 'rallyM', 'power', 'start', 'tasks']) || input.version !== 2 || !['fixed-wing', 'vtol', 'ground', 'surface', 'subsea'].includes(input.vehicle as string) || !id(input.id) || input.frame !== 'LOCAL_NED' || input.altitude !== 'ORIGIN_RELATIVE_DOWN' || input.terrain !== 'DECLARED_MODEL_SURFACE' || !id(input.start)) invalid() | |
| 19 | if (!object(input) || !keys(input, ['version', 'vehicle', 'id', 'frame', 'altitude', 'terrain', 'origin', 'fence', 'rallyM', 'power', 'start', 'tasks']) || input.version !== 2 || !vehicleKinds.some(kind => kind === input.vehicle) || !id(input.id) || input.frame !== 'LOCAL_NED' || input.altitude !== 'ORIGIN_RELATIVE_DOWN' || input.terrain !== 'DECLARED_MODEL_SURFACE' || !id(input.start)) invalid() | |
| 19 | 20 | const p = input as unknown as VehicleMissionPlan |
| 20 | 21 | if (!object(p.origin) || !keys(p.origin, ['latitudeDeg', 'longitudeDeg', 'ellipsoidHeightM']) || !number(p.origin.latitudeDeg, -90, 90) || !number(p.origin.longitudeDeg, -180, 180) || !number(p.origin.ellipsoidHeightM, -500, 10000) |
| 21 | 22 | || !object(p.fence) || !keys(p.fence, ['radiusM', 'minDownM', 'maxDownM']) || !number(p.fence.radiusM, 1, 5000) || !number(p.fence.minDownM, -1000, 0) || !number(p.fence.maxDownM, 0, 10) || p.fence.minDownM >= p.fence.maxDownM || !vehicleMissionContains(p, p.rallyM) |
| @@ -39,17 +40,19 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 39 | 40 | edges = [t.then, t.otherwise] |
| 40 | 41 | } |
| 41 | 42 | else if (t.kind === 'payload') { |
| 42 | fields.push('operation', 'containerId', 'timeoutMs', 'next') | |
| 43 | const supported = p.vehicle === 'fixed-wing' || p.vehicle === 'vtol' ? ['camera-still'] : p.vehicle === 'ground' ? ['camera-still', 'material-drop', 'mower-enable', 'mower-disable'] : p.vehicle === 'surface' ? ['camera-still', 'water-sample'] : ['camera-still', 'inspection-capture'] | |
| 43 | fields.push('operation', 'containerId', 'timeoutMs', 'next', 'onFailure') | |
| 44 | const supported = p.vehicle === 'quadcopter' ? ['camera-still', 'material-drop'] : p.vehicle === 'fixed-wing' || p.vehicle === 'vtol' ? ['camera-still'] : p.vehicle === 'ground' ? ['camera-still', 'material-drop', 'mower-enable', 'mower-disable'] : p.vehicle === 'surface' ? ['camera-still', 'water-sample'] : ['camera-still', 'inspection-capture'] | |
| 44 | 45 | if (!supported.includes(t.operation) || (t.containerId !== null && !id(t.containerId)) || (t.operation === 'water-sample' && !id(t.containerId)) || !number(t.timeoutMs, 100, 10000) || !id(t.next)) invalid() |
| 45 | durationMs = t.timeoutMs; edges = [t.next] | |
| 46 | if (p.vehicle === 'quadcopter' && t.operation === 'material-drop' && !id(t.containerId)) invalid() | |
| 47 | if (t.onFailure !== undefined && (p.vehicle !== 'quadcopter' || t.operation !== 'material-drop' || !id(t.onFailure))) invalid() | |
| 48 | durationMs = t.timeoutMs; edges = t.onFailure === undefined ? [t.next] : [t.next, t.onFailure] | |
| 46 | 49 | } |
| 47 | 50 | else if (['takeoff', 'waypoint', 'land', 'stop', 'loiter', 'return'].includes(t.kind)) { |
| 48 | 51 | fields = [...fields, 'acceptanceM', 'maxArrivalSpeedMps', 'settleMs', 'timeoutMs', 'next'] |
| 49 | 52 | if (t.kind !== 'return') { fields.push('positionM'); if (!vehicleMissionContains(p, (t as { positionM?: unknown }).positionM)) invalid() } |
| 50 | 53 | if (!('acceptanceM' in t) || !number(t.acceptanceM, 0.05, 150) || !number(t.maxArrivalSpeedMps, 0.05, 50) || !number(t.settleMs, 0, 10000) || !number(t.timeoutMs, 1000, 60000) || t.settleMs >= t.timeoutMs) invalid() |
| 51 | if ((t.kind === 'takeoff' || t.kind === 'land') && p.vehicle !== 'fixed-wing' && p.vehicle !== 'vtol') invalid() | |
| 52 | if (t.kind === 'stop' && (p.vehicle === 'fixed-wing' || p.vehicle === 'vtol' || t.maxArrivalSpeedMps > 0.5)) invalid() | |
| 54 | if ((t.kind === 'takeoff' || t.kind === 'land') && !['quadcopter', 'fixed-wing', 'vtol'].includes(p.vehicle)) invalid() | |
| 55 | if (t.kind === 'stop' && (['quadcopter', 'fixed-wing', 'vtol'].includes(p.vehicle) || t.maxArrivalSpeedMps > 0.5)) invalid() | |
| 53 | 56 | if (t.kind === 'land' && t.maxArrivalSpeedMps > 2) invalid() |
| 54 | 57 | const terminal = t.kind === 'land' || t.kind === 'stop' |
| 55 | 58 | if (!terminal && !id(t.next)) invalid() |
| @@ -61,6 +64,13 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 61 | 64 | nodes.push({ id: t.id, durationMs, edges }) |
| 62 | 65 | } |
| 63 | 66 | for (const task of p.tasks) { |
| 67 | if (task.kind === 'payload' && task.onFailure !== undefined) { | |
| 68 | const approach = p.tasks.find(candidate => candidate.id === task.onFailure) | |
| 69 | if (!approach || approach.kind !== 'waypoint' || !approach.next || approach.positionM[2] >= 0) invalid() | |
| 70 | const land = p.tasks.find(candidate => candidate.id === approach.next) | |
| 71 | if (!land || land.kind !== 'land' || land.next !== undefined || land.positionM[2] !== 0 || land.positionM[0] !== approach.positionM[0] || land.positionM[1] !== approach.positionM[1]) invalid() | |
| 72 | if (p.tasks.some(candidate => 'next' in candidate && candidate.next === approach.id || candidate.kind === 'branch' && [candidate.then, candidate.otherwise].includes(approach.id))) invalid() | |
| 73 | } | |
| 64 | 74 | if (task.kind !== 'vtol-mode' || task.onFailure === undefined) continue |
| 65 | 75 | const hover = p.tasks.find(candidate => candidate.id === task.onFailure) |
| 66 | 76 | if (!hover || hover.kind !== 'vtol-mode' || hover.mode !== 'hover' || hover.onFailure !== undefined || hover.approach !== undefined) invalid() |
| @@ -110,9 +120,10 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 110 | 120 | let targetM: VehicleMissionSnapshot['targetM'] = null, holdM: VehicleMissionSnapshot['targetM'] = null, pending: VehiclePayloadCommand | null = null |
| 111 | 121 | let transition: VehicleVtolTransitionCommand | null = null |
| 112 | 122 | let recovery: VehicleMissionSnapshot['recovery'] |
| 123 | let payloadFailure: VehicleMissionSnapshot['payloadFailure'] | |
| 113 | 124 | let transitionObservation: { capturedAtMs: number, measurementCapturedAtMs: number, signature: string } | null = null |
| 114 | 125 | const rows: VehicleMissionTrace['rows'] = [] |
| 115 | function snapshot(): VehicleMissionSnapshot { return { version: 2, vehicle: plan.vehicle, evidence: 'owned-vehicle-mission-simulation', physicalFlightEnabled: false, state, taskId, taskKind: tasks.get(taskId)!.kind, targetM: targetM ? [...targetM] : null, payload: pending ? structuredClone(pending) : null, reason, elapsedMs: startedAt === null || lastNow === null ? 0 : lastNow - startedAt, ...(hasTransitions ? { transition: transition ? structuredClone(transition) : null } : {}), ...(recovery ? { recovery: { ...recovery } } : {}) } } | |
| 126 | function snapshot(): VehicleMissionSnapshot { return { version: 2, vehicle: plan.vehicle, evidence: 'owned-vehicle-mission-simulation', physicalFlightEnabled: false, state, taskId, taskKind: tasks.get(taskId)!.kind, targetM: targetM ? [...targetM] : null, payload: pending ? structuredClone(pending) : null, reason, elapsedMs: startedAt === null || lastNow === null ? 0 : lastNow - startedAt, ...(hasTransitions ? { transition: transition ? structuredClone(transition) : null } : {}), ...(recovery ? { recovery: { ...recovery } } : {}), ...(payloadFailure ? { payloadFailure: { ...payloadFailure } } : {}) } } | |
| 116 | 127 | function block(why: string) { state = 'blocked'; reason = why; targetM = null; pending = null; transition = null; transitionObservation = null } |
| 117 | 128 | function advance(next: string) { |
| 118 | 129 | taskId = next; taskElapsedMs = 0; settledMs = 0; pending = null; transition = null; transitionObservation = null; reason = 'task-entered' |
| @@ -145,6 +156,14 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 145 | 156 | targetM = [...transition.positionM]; reason = 'vtol-transition-recovery-requested' |
| 146 | 157 | return true |
| 147 | 158 | } |
| 159 | function divertPayload(task: Extract<VehicleMissionTask, { kind: 'payload' }>, o: VehicleMissionObservation, cause: NonNullable<VehicleMissionSnapshot['payloadFailure']>['reason']) { | |
| 160 | if (!task.onFailure || payloadFailure || !pending || !o.healthy || !o.positionM || !o.velocityMps || pending.reference !== o.reference) return false | |
| 161 | const approach = tasks.get(task.onFailure)! | |
| 162 | if (approach.kind !== 'waypoint' || !approach.next) return false | |
| 163 | payloadFailure = { eventId: pending.eventId, taskId: task.id, operation: task.operation, containerId: task.containerId, reason: cause, triggeredAtMs: o.nowMs, approachTaskId: approach.id, landingTaskId: approach.next } | |
| 164 | advance(approach.id); reason = 'payload-failure-diversion' | |
| 165 | return true | |
| 166 | } | |
| 148 | 167 | return { |
| 149 | 168 | snapshot, |
| 150 | 169 | trace(): VehicleMissionTrace { return structuredClone({ version: rows.some(row => row.observation.positionM === null) ? 3 : 2, evidence: 'owned-vehicle-mission-simulation', plan, rows }) }, |
| @@ -207,7 +226,8 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 207 | 226 | const t = tasks.get(taskId)!, executionDt = previousState === 'ready' || previousState === 'paused' ? 0 : dt |
| 208 | 227 | taskElapsedMs += executionDt |
| 209 | 228 | if ('timeoutMs' in t && taskElapsedMs >= t.timeoutMs) { |
| 210 | if (t.kind === 'vtol-mode' && transition && t.onFailure) { | |
| 229 | if (t.kind === 'payload' && t.onFailure) { if (!divertPayload(t, o, 'timeout')) block('payload-outcome-uncertain') } | |
| 230 | else if (t.kind === 'vtol-mode' && transition && t.onFailure) { | |
| 211 | 231 | readTransitionObservation(o) |
| 212 | 232 | if (active(state) && !beginRecovery(t, o, 'timeout')) block('vtol-transition-outcome-uncertain') |
| 213 | 233 | } |
| @@ -237,7 +257,8 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 237 | 257 | pending ??= { eventId: `${plan.id}:${reference}:${t.id}`, taskId: t.id, operation: t.operation, containerId: t.containerId, issuedAtMs: o.nowMs, expiresAtMs: o.nowMs + t.timeoutMs - taskElapsedMs, positionM: [...o.positionM], reference: reference! } |
| 238 | 258 | const ack = o.payloadAck |
| 239 | 259 | if (ack) { |
| 240 | if (ack.eventId !== pending.eventId || ack.taskId !== pending.taskId || ack.operation !== pending.operation || ack.containerId !== pending.containerId || ack.reference !== pending.reference || ack.capturedAtMs < pending.issuedAtMs || ack.capturedAtMs >= pending.expiresAtMs || ack.capturedAtMs > o.nowMs || o.nowMs - ack.capturedAtMs > 200 || Math.hypot(...ack.positionM.map((n, i) => n - pending!.positionM[i]!)) > 1 || ack.outcome !== 'confirmed') block('payload-outcome-uncertain') | |
| 260 | const scopeMismatch = ack.eventId !== pending.eventId || ack.taskId !== pending.taskId || ack.operation !== pending.operation || ack.containerId !== pending.containerId || ack.reference !== pending.reference || ack.capturedAtMs < pending.issuedAtMs || ack.capturedAtMs >= pending.expiresAtMs || ack.capturedAtMs > o.nowMs || o.nowMs - ack.capturedAtMs > 200 || Math.hypot(...ack.positionM.map((n, i) => n - pending!.positionM[i]!)) > 1 | |
| 261 | if (scopeMismatch || ack.outcome !== 'confirmed') { if (!divertPayload(t, o, scopeMismatch ? 'scope-mismatch' : ack.outcome === 'rejected' ? 'rejected' : 'uncertain')) block('payload-outcome-uncertain') } | |
| 241 | 262 | else { advance(t.next); reason = 'payload-confirmed' } |
| 242 | 263 | } |
| 243 | 264 | } |
| @@ -251,7 +272,7 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 251 | 272 | if (t.next) advance(t.next) |
| 252 | 273 | else { |
| 253 | 274 | const abortLanded = t.kind === 'land' && recovery?.abortRequestedAtMs !== undefined |
| 254 | state = abortLanded ? 'aborted' : 'completed'; reason = t.kind === 'land' ? abortLanded ? 'vtol-abort-landed' : recovery ? 'vtol-recovery-landed' : 'landed' : 'stopped'; targetM = null | |
| 275 | state = abortLanded ? 'aborted' : 'completed'; reason = t.kind === 'land' ? payloadFailure ? 'payload-diversion-landed' : abortLanded ? 'vtol-abort-landed' : recovery ? 'vtol-recovery-landed' : 'landed' : 'stopped'; targetM = null | |
| 255 | 276 | } |
| 256 | 277 | } |
| 257 | 278 | } |