also looking at this
feat: coordinate modeled two-carrier cargo handover
#103
25 files
+1654
-13
| @@ -1,3 +1,4 @@ | ||
| 1 | import type { SensorVector } from '../sensors/types' | |
| 1 | 2 | import type { VehicleMissionAction, VehicleMissionObservation, VehicleMissionPlan, VehicleMissionSnapshot, VehicleMissionTask, VehiclePayloadAck, VehiclePayloadCommand, VehicleVtolTransitionCommand, VehicleVtolTransitionObservation } from '../vehicles/types' |
| 2 | 3 | import type { MissionState } from './engine' |
| 3 | 4 | import { vehicleKinds } from '../vehicles/types' |
| @@ -41,10 +42,13 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 41 | 42 | } |
| 42 | 43 | else if (t.kind === 'payload') { |
| 43 | 44 | 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'] | |
| 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() | |
| 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() | |
| 45 | const receiving = t.operation === 'material-receive' | |
| 46 | if (receiving) fields.push('positionM') | |
| 47 | const supported = p.vehicle === 'quadcopter' ? ['camera-still', 'material-drop', 'material-receive'] : 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'] | |
| 48 | if (!supported.includes(t.operation) || (t.containerId !== null && !id(t.containerId)) || (t.operation === 'water-sample' && !id(t.containerId)) || !number(t.timeoutMs, 100, receiving ? 60000 : 10000) || !id(t.next)) invalid() | |
| 49 | if (p.vehicle === 'quadcopter' && ['material-drop', 'material-receive'].includes(t.operation) && !id(t.containerId)) invalid() | |
| 50 | if (receiving && (!Number.isSafeInteger(t.timeoutMs) || !vehicleMissionContains(p, t.positionM) || t.positionM[2] < -1 || t.positionM[2] > 0)) invalid() | |
| 51 | if (t.onFailure !== undefined && (p.vehicle !== 'quadcopter' || !['material-drop', 'material-receive'].includes(t.operation) || !id(t.onFailure))) invalid() | |
| 48 | 52 | durationMs = t.timeoutMs; edges = t.onFailure === undefined ? [t.next] : [t.next, t.onFailure] |
| 49 | 53 | } |
| 50 | 54 | else if (['takeoff', 'waypoint', 'land', 'stop', 'loiter', 'return'].includes(t.kind)) { |
| @@ -86,11 +90,18 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl | ||
| 86 | 90 | return { plan: structuredClone(p), worstCaseMs, requiredWh } |
| 87 | 91 | } |
| 88 | 92 | export interface VehicleMissionTrace { version: 2 | 3, evidence: 'owned-vehicle-mission-simulation', plan: VehicleMissionPlan, rows: { observation: VehicleMissionObservation, action: VehicleMissionAction | null, result: VehicleMissionSnapshot }[] } |
| 93 | function validAckPosition(value: unknown): value is SensorVector { | |
| 94 | return validVector(value) && [0, 1, 2].every(i => Object.hasOwn(value, i)) | |
| 95 | } | |
| 89 | 96 | function validAck(value: unknown): value is VehiclePayloadAck { |
| 90 | if (!object(value) || !keys(value, ['eventId', 'taskId', 'operation', 'containerId', 'capturedAtMs', 'reference', 'positionM', 'outcome', 'metadata']) || typeof value.eventId !== 'string' || value.eventId.length > 200 || !id(value.taskId) || !['camera-still', 'water-sample', 'material-drop', 'mower-enable', 'mower-disable', 'inspection-capture'].includes(value.operation as string) || (value.containerId !== null && !id(value.containerId)) || !Number.isSafeInteger(value.capturedAtMs) || (value.capturedAtMs as number) < 0 || typeof value.reference !== 'string' || value.reference.length > 120 || !validVector(value.positionM) || !['confirmed', 'rejected', 'uncertain'].includes(value.outcome as string)) return false | |
| 97 | if (!object(value) || !keys(value, ['eventId', 'taskId', 'operation', 'containerId', 'capturedAtMs', 'reference', 'positionM', 'outcome', 'metadata']) || typeof value.eventId !== 'string' || value.eventId.length > 200 || !id(value.taskId) || !['camera-still', 'water-sample', 'material-drop', 'material-receive', 'mower-enable', 'mower-disable', 'inspection-capture'].includes(value.operation as string) || (value.containerId !== null && !id(value.containerId)) || !Number.isSafeInteger(value.capturedAtMs) || (value.capturedAtMs as number) < 0 || typeof value.reference !== 'string' || value.reference.length > 120 || !validAckPosition(value.positionM) || !['confirmed', 'rejected', 'uncertain'].includes(value.outcome as string)) return false | |
| 91 | 98 | if (value.metadata !== undefined && (!object(value.metadata) || Object.keys(value.metadata).length > 32 || Object.entries(value.metadata).some(([key, item]) => !/^[a-zA-Z][a-zA-Z0-9_-]{0,39}$/.test(key) || !(typeof item === 'boolean' || (typeof item === 'number' && Number.isFinite(item)) || (typeof item === 'string' && item.length <= 160))))) return false |
| 92 | 99 | return true |
| 93 | 100 | } |
| 101 | function receivingAtPad(task: Extract<VehicleMissionTask, { operation: 'material-receive' }>, o: VehicleMissionObservation) { | |
| 102 | return o.onGround && o.positionM !== null && o.velocityMps !== null && Math.hypot(...o.velocityMps) <= .1 | |
| 103 | && Math.hypot(...o.positionM.map((n, i) => n - task.positionM[i]!)) <= .2 | |
| 104 | } | |
| 94 | 105 | function validTransition(value: unknown): value is VehicleVtolTransitionObservation { |
| 95 | 106 | return object(value) && keys(value, ['evidence', 'eventId', 'taskId', 'mode', 'reference', 'capturedAtMs', 'measurementCapturedAtMs', 'phase', 'outcome', 'reason']) |
| 96 | 107 | && value.evidence === 'modeled-vtol-controller-observation' && typeof value.eventId === 'string' && value.eventId.length <= 256 && id(value.taskId) && ['hover', 'cruise'].includes(value.mode as string) |
| @@ -253,11 +264,13 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 253 | 264 | else reason = 'vtol-transition-progress' |
| 254 | 265 | } |
| 255 | 266 | } |
| 267 | else if (t.kind === 'payload' && t.operation === 'material-receive' && !receivingAtPad(t, o)) block('material-receive-interlocks') | |
| 256 | 268 | else if (t.kind === 'payload') { |
| 257 | 269 | 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! } |
| 258 | 270 | const ack = o.payloadAck |
| 259 | 271 | if (ack) { |
| 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 | |
| 272 | const separationM = Math.hypot(...ack.positionM.map((n, i) => n - pending!.positionM[i]!)) | |
| 273 | 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 || !Number.isFinite(separationM) || separationM > 1 | |
| 261 | 274 | if (scopeMismatch || ack.outcome !== 'confirmed') { if (!divertPayload(t, o, scopeMismatch ? 'scope-mismatch' : ack.outcome === 'rejected' ? 'rejected' : 'uncertain')) block('payload-outcome-uncertain') } |
| 262 | 275 | else { advance(t.next); reason = 'payload-confirmed' } |
| 263 | 276 | } |
| @@ -277,6 +290,12 @@ export function createVehicleMissionEngine(input: unknown) { | ||
| 277 | 290 | } |
| 278 | 291 | } |
| 279 | 292 | } |
| 293 | // Intake must inhibit the first control frame, including entry directly from start. | |
| 294 | const entered = tasks.get(taskId)! | |
| 295 | if (state === 'running' && entered.kind === 'payload' && entered.operation === 'material-receive') { | |
| 296 | if (!receivingAtPad(entered, o)) block('material-receive-interlocks') | |
| 297 | else pending ??= { eventId: `${plan.id}:${reference}:${entered.id}`, taskId: entered.id, operation: entered.operation, containerId: entered.containerId, issuedAtMs: o.nowMs, expiresAtMs: o.nowMs + entered.timeoutMs - taskElapsedMs, positionM: [...o.positionM], reference: reference! } | |
| 298 | } | |
| 280 | 299 | const result = snapshot() |
| 281 | 300 | rows.push({ observation: o, action: action ?? null, result: structuredClone(result) }) |
| 282 | 301 | return result |