also looking at this
feat: add shared-clock scoped vehicle simulation sessions
#98
42 files
+2764
-127
| @@ -0,0 +1,161 @@ | ||
| 1 | import type { GeodeticOrigin } from '../estimation/frames' | |
| 2 | import type { SensorVector } from '../sensors/types' | |
| 3 | import type { VehicleControllerContext, VehiclePayloadAck, VehiclePayloadCommand, VehiclePayloadPort } from '../vehicles/types' | |
| 4 | import { geodeticToEcef } from '../estimation/frames' | |
| 5 | import { authorityPolicy } from '../authority/supervisor' | |
| 6 | import { createVehicleMeasurementQualifier, vehicleReference } from '../vehicles/measurements' | |
| 7 | ||
| 8 | export interface CargoDeliveryDeclaration { | |
| 9 | origin: GeodeticOrigin | |
| 10 | site: { id: string, recipientId: string, containerId: string, positionM: SensorVector, radiusM: number } | |
| 11 | releaseHeightM: number | |
| 12 | } | |
| 13 | export interface CargoDeliveryCustody { | |
| 14 | outcome: 'not-released' | 'in-transit' | 'received' | 'uncertain' | |
| 15 | containerId: string | |
| 16 | siteId: string | |
| 17 | recipientId: string | |
| 18 | requestedAtMs: number | null | |
| 19 | mechanismCapturedAtMs: number | null | |
| 20 | receiptCapturedAtMs: number | null | |
| 21 | acknowledgedAtMs: number | null | |
| 22 | reason: string | null | |
| 23 | mechanismOutcome: 'not-requested' | 'requested' | 'open' | 'jammed' | |
| 24 | } | |
| 25 | export interface ModeledCargoMechanism { | |
| 26 | evidence: 'modeled-cargo-mechanism' | |
| 27 | request(command: VehiclePayloadCommand): void | |
| 28 | cancel(eventId: string): void | |
| 29 | } | |
| 30 | function record(input: unknown, fields: string[]): Record<string, unknown> { | |
| 31 | if (!input || typeof input !== 'object' || Object.getPrototypeOf(input) !== Object.prototype || Reflect.ownKeys(input).length !== fields.length) throw new TypeError('cargo-delivery-declaration') | |
| 32 | const result: Record<string, unknown> = {} | |
| 33 | for (const field of fields) { | |
| 34 | const d = Object.getOwnPropertyDescriptor(input, field) | |
| 35 | if (!d?.enumerable || !Object.hasOwn(d, 'value')) throw new TypeError('cargo-delivery-declaration') | |
| 36 | result[field] = d.value | |
| 37 | } | |
| 38 | return result | |
| 39 | } | |
| 40 | export function captureCargoDeliveryDeclaration(input: unknown): CargoDeliveryDeclaration { | |
| 41 | const top = record(input, ['origin', 'site', 'releaseHeightM']), site = record(top.site, ['id', 'recipientId', 'containerId', 'positionM', 'radiusM']) | |
| 42 | const origin = record(top.origin, ['latitudeDeg', 'longitudeDeg', 'ellipsoidHeightM']) | |
| 43 | if (!['id', 'recipientId', 'containerId'].every(key => typeof site[key] === 'string' && /^[a-z][a-z0-9-]{0,63}$/.test(site[key] as string)) | |
| 44 | || typeof site.radiusM !== 'number' || !Number.isFinite(site.radiusM) || site.radiusM < .1 || site.radiusM > 2 | |
| 45 | || typeof top.releaseHeightM !== 'number' || !Number.isFinite(top.releaseHeightM) || top.releaseHeightM < .3 || top.releaseHeightM > 3 | |
| 46 | || !Object.values(origin).every(value => typeof value === 'number' && Number.isFinite(value)) || !Array.isArray(site.positionM) || site.positionM.length !== 3 || Reflect.ownKeys(site.positionM).length !== 4) throw new TypeError('cargo-delivery-declaration') | |
| 47 | const positionM = [0, 1, 2].map(i => { | |
| 48 | const d = Object.getOwnPropertyDescriptor(site.positionM, String(i)) | |
| 49 | if (!d?.enumerable || !Object.hasOwn(d, 'value') || typeof d.value !== 'number' || !Number.isFinite(d.value) || Math.abs(d.value) > 5000) throw new TypeError('cargo-delivery-declaration') | |
| 50 | return d.value | |
| 51 | }) as SensorVector | |
| 52 | const capturedOrigin = origin as unknown as GeodeticOrigin | |
| 53 | geodeticToEcef(capturedOrigin) | |
| 54 | return { origin: { ...capturedOrigin }, site: { id: site.id as string, recipientId: site.recipientId as string, containerId: site.containerId as string, positionM, radiusM: site.radiusM }, releaseHeightM: top.releaseHeightM } | |
| 55 | } | |
| 56 | function finiteTuple(value: unknown, size: number) { | |
| 57 | return Array.isArray(value) && value.length === size && Array.from({ length: size }, (_, i) => i).every(i => Object.hasOwn(value, i) && typeof value[i] === 'number' && Number.isFinite(value[i])) | |
| 58 | } | |
| 59 | ||
| 60 | /** Captured modeled release/custody policy behind the shared vehicle payload port. No physical IO. */ | |
| 61 | export function createCargoDeliveryCoordinator(input: unknown, mechanism: ModeledCargoMechanism) { | |
| 62 | const declaration = captureCargoDeliveryDeclaration(input), { site } = declaration | |
| 63 | if (!mechanism || mechanism.evidence !== 'modeled-cargo-mechanism' || typeof mechanism.request !== 'function' || typeof mechanism.cancel !== 'function') throw new TypeError('cargo-delivery-modeled-mechanism') | |
| 64 | const delivery: CargoDeliveryCustody = { outcome: 'not-released', containerId: site.containerId, siteId: site.id, recipientId: site.recipientId, requestedAtMs: null, mechanismCapturedAtMs: null, receiptCapturedAtMs: null, acknowledgedAtMs: null, reason: null, mechanismOutcome: 'not-requested' } | |
| 65 | const measurements = createVehicleMeasurementQualifier('quadcopter') | |
| 66 | let pending: VehiclePayloadCommand | null = null, observed: VehicleControllerContext | null = null | |
| 67 | let closed = false, requested = false, lastPollMs = -1, lastSequence = -1, readyCount = 0, readyStartMs: number | null = null, readyReference: string | null = null, refused: string | null = null | |
| 68 | let lastObservedAtMs: number | null = null, releaseAttempted = false, readyCaptureStartMs: number | null = null | |
| 69 | function ready(context: VehicleControllerContext) { | |
| 70 | const e = context.estimate, m = context.measurement, p = e.localPosition, a = e.attitude, q = a?.quaternionWxyz, rate = context.controlRate | |
| 71 | if (!context.enabled || context.estimate.evidence !== 'fixture' || !context.estimate.health.localPosition || !context.estimate.health.tilt || !m || !p || !q || !rate | |
| 72 | || e.physicalFlightEnabled !== false || e.nowMs !== context.nowMs || a?.reference !== 'GRAVITY_ALIGNED_RELATIVE_YAW' || p.frame !== 'LOCAL_NED' || p.altitudeDatum !== 'WGS84_ELLIPSOID' | |
| 73 | || p.capturedAtMs === null || !Number.isFinite(p.capturedAtMs) || p.capturedAtMs < 0 || p.capturedAtMs > context.nowMs || context.nowMs - p.capturedAtMs > 200 || p.predictedToMs !== context.nowMs | |
| 74 | || !finiteTuple(p.positionM, 3) || !finiteTuple(p.velocityMps, 3) || !finiteTuple(q, 4) || Math.abs(Math.hypot(...q) - 1) > 1e-6 || !finiteTuple(rate.angularRateRadS, 3) | |
| 75 | || !Array.isArray(p.covarianceBlocks) || p.covarianceBlocks.length !== 3 || !p.covarianceBlocks.every(b => b && [b.positionM2, b.positionVelocityM2S, b.velocityM2S2].every(Number.isFinite) && b.positionM2 >= 0 && b.velocityM2S2 >= 0 && b.positionVelocityM2S ** 2 <= b.positionM2 * b.velocityM2S2 + 1e-12) | |
| 76 | || m.reference !== vehicleReference(context.estimate) || m.capturedAtMs > context.nowMs || context.nowMs - m.capturedAtMs > 80 || m.receivedAtMs > context.nowMs | |
| 77 | || rate.evidence !== 'modeled-qualified-imu' || rate.reference !== m.reference || !Number.isSafeInteger(rate.capturedAtMs) || rate.capturedAtMs > context.nowMs || context.nowMs - rate.capturedAtMs > 60 || rate.capturedAtMs !== context.estimate.attitude?.capturedAtMs | |
| 78 | || m.values.siteId !== site.id || m.values.recipientId !== site.recipientId || m.values.siteClear !== true || m.values.recipientPresent !== true || m.values.loadAttached !== true | |
| 79 | || !p.origin || p.origin.latitudeDeg !== declaration.origin.latitudeDeg || p.origin.longitudeDeg !== declaration.origin.longitudeDeg || p.origin.ellipsoidHeightM !== declaration.origin.ellipsoidHeightM) return false | |
| 80 | const speed = Math.hypot(...p.velocityMps), sigma = Math.sqrt(Math.max(...p.covarianceBlocks.map(block => block.positionM2))) | |
| 81 | const horizontal = Math.hypot(p.positionM[0]! - site.positionM[0], p.positionM[1]! - site.positionM[1]), heightM = site.positionM[2] - p.positionM[2]! | |
| 82 | return speed <= .18 && horizontal + 3 * sigma + speed * .08 <= site.radiusM && Math.abs(heightM - declaration.releaseHeightM) <= .2 | |
| 83 | && Math.acos(Math.max(-1, Math.min(1, 1 - 2 * (q[1] ** 2 + q[2] ** 2)))) <= .2 && Math.hypot(...rate.angularRateRadS) <= .5 | |
| 84 | } | |
| 85 | function cancel() { | |
| 86 | const command = pending | |
| 87 | pending = null | |
| 88 | if (command && releaseAttempted) { | |
| 89 | try { mechanism.cancel(command.eventId) } | |
| 90 | catch { delivery.reason ??= 'mechanism-cancel-unavailable' } | |
| 91 | } | |
| 92 | } | |
| 93 | const port: VehiclePayloadPort = { | |
| 94 | observe(input) { | |
| 95 | const context = structuredClone(input) | |
| 96 | if (lastObservedAtMs !== null && context.nowMs - lastObservedAtMs > authorityPolicy.schedulerGapMs) { readyCount = 0; readyStartMs = null; readyCaptureStartMs = null } | |
| 97 | lastObservedAtMs = context.nowMs | |
| 98 | try { context.measurement = measurements.qualify(context.measurement, context.nowMs, vehicleReference(context.estimate)) } | |
| 99 | catch { context.measurement = null; context.measurementPose = null } | |
| 100 | observed = context | |
| 101 | if (pending && (context.mission.payloadFailure?.eventId === pending.eventId || !context.enabled)) { | |
| 102 | cancel() | |
| 103 | if (delivery.mechanismOutcome === 'open' || delivery.outcome === 'uncertain') { delivery.outcome = 'uncertain'; delivery.reason ??= 'receipt-not-confirmed' } | |
| 104 | else { delivery.outcome = 'not-released'; delivery.reason ??= 'receipt-not-confirmed' } | |
| 105 | } | |
| 106 | const sequence = context.measurement?.sequence ?? -1, reference = context.measurement?.reference ?? null | |
| 107 | const qualified = ready(context) && (readyReference === null || reference === readyReference) | |
| 108 | if (!qualified) { readyCount = 0; readyStartMs = null; readyCaptureStartMs = null } | |
| 109 | if (sequence === lastSequence && reference === readyReference) return | |
| 110 | if (qualified && (lastSequence < 0 || sequence > lastSequence)) { readyCount++; readyStartMs ??= context.nowMs; readyCaptureStartMs ??= context.measurement!.capturedAtMs } | |
| 111 | else { readyCount = 0; readyStartMs = null; readyCaptureStartMs = null } | |
| 112 | lastSequence = sequence; readyReference = reference | |
| 113 | }, | |
| 114 | request(command) { | |
| 115 | if (closed || requested || pending !== null) throw new TypeError('cargo-delivery-command-replayed') | |
| 116 | requested = true; pending = structuredClone(command); delivery.requestedAtMs = command.issuedAtMs; delivery.mechanismOutcome = 'requested' | |
| 117 | const c = observed, scoped = c?.mission.payload | |
| 118 | if (!c || !scoped || c.mission.version !== 2 || c.mission.vehicle !== 'quadcopter' || c.mission.evidence !== 'owned-vehicle-mission-simulation' || c.mission.physicalFlightEnabled !== false || c.mission.state !== 'running' || c.mission.taskKind !== 'payload' || command.taskId !== c.mission.taskId || command.operation !== 'material-drop' || command.containerId !== site.containerId | |
| 119 | || typeof command.eventId !== 'string' || command.eventId.length < 1 || command.eventId.length > 160 || !Number.isSafeInteger(command.expiresAtMs) || command.expiresAtMs - c.nowMs > 10000 | |
| 120 | || !finiteTuple(command.positionM, 3) || !c.estimate.localPosition || !command.positionM.every((value, i) => value === c.estimate.localPosition!.positionM[i]) | |
| 121 | || JSON.stringify(scoped) !== JSON.stringify(command) || command.reference !== vehicleReference(c.estimate) || command.issuedAtMs !== c.nowMs || command.expiresAtMs <= c.nowMs | |
| 122 | || !ready(c) || readyCount < 26 || readyStartMs === null || c.nowMs - readyStartMs < 500 || readyCaptureStartMs === null || !c.measurement || c.measurement.capturedAtMs - readyCaptureStartMs < 500) { refused = 'release-evidence-unqualified'; delivery.reason = refused } | |
| 123 | else { | |
| 124 | releaseAttempted = true | |
| 125 | delivery.outcome = 'uncertain' | |
| 126 | try { mechanism.request(structuredClone(command)) } | |
| 127 | catch { refused = 'mechanism-transport-unavailable'; delivery.reason = refused } | |
| 128 | } | |
| 129 | }, | |
| 130 | poll(nowMs): VehiclePayloadAck | null { | |
| 131 | if (!Number.isSafeInteger(nowMs) || nowMs < lastPollMs) throw new RangeError('cargo-delivery-clock') | |
| 132 | lastPollMs = nowMs | |
| 133 | const c = observed, m = c?.measurement, pose = c?.measurementPose | |
| 134 | if (!pending || closed || !c?.enabled || c.nowMs !== nowMs || !m || m.reference !== pending.reference || m.capturedAtMs < pending.issuedAtMs || m.capturedAtMs >= pending.expiresAtMs | |
| 135 | || nowMs >= pending.expiresAtMs || nowMs - m.capturedAtMs > 80 || m.receivedAtMs > nowMs || !pose || pose.reference !== m.reference || pose.capturedAtMs !== m.capturedAtMs || pose.positionPredictedToMs !== m.capturedAtMs || !finiteTuple(pose.positionM, 3) || !pose.positionM.every(value => Math.abs(value) <= 1e6)) return null | |
| 136 | if (releaseAttempted && m.values.mechanismEventId === pending.eventId && m.values.mechanismJammed === true && m.values.loadAttached === true) { | |
| 137 | delivery.mechanismCapturedAtMs ??= m.capturedAtMs; delivery.mechanismOutcome = 'jammed'; delivery.outcome = 'not-released'; refused = 'mechanism-jammed'; delivery.reason = refused | |
| 138 | } | |
| 139 | if (releaseAttempted && m.values.mechanismEventId === pending.eventId && m.values.mechanismOpen === true && m.values.loadAttached === false) { delivery.mechanismCapturedAtMs ??= m.capturedAtMs; delivery.mechanismOutcome = 'open'; delivery.outcome = 'in-transit' } | |
| 140 | const received = releaseAttempted && delivery.mechanismOutcome === 'open' && delivery.mechanismCapturedAtMs !== null && m.values.mechanismEventId === pending.eventId && m.values.mechanismOpen === true && m.values.mechanismJammed === false && m.values.loadAttached === false | |
| 141 | && m.values.receiptEventId === pending.eventId && m.values.receiptContainerId === pending.containerId | |
| 142 | && m.values.recipientId === site.recipientId && m.values.siteId === site.id && m.values.recipientPresent === true && m.values.siteClear === true | |
| 143 | if (releaseAttempted && m.values.receiptEventId !== '' && !received) { refused = 'receipt-scope-mismatch'; delivery.reason = refused; delivery.outcome = 'uncertain' } | |
| 144 | if (!refused && !received) return null | |
| 145 | const command = pending | |
| 146 | if (refused) cancel() | |
| 147 | else pending = null | |
| 148 | delivery.acknowledgedAtMs = nowMs | |
| 149 | if (refused && releaseAttempted && delivery.outcome !== 'not-released') delivery.outcome = 'uncertain' | |
| 150 | if (received && !refused) { delivery.receiptCapturedAtMs = m.capturedAtMs; delivery.outcome = 'received' } | |
| 151 | return { eventId: command.eventId, taskId: command.taskId, operation: command.operation, containerId: command.containerId, reference: command.reference, capturedAtMs: m.capturedAtMs, | |
| 152 | positionM: [...pose.positionM], outcome: refused ? delivery.outcome === 'uncertain' ? 'uncertain' : 'rejected' : 'confirmed', | |
| 153 | metadata: { modeledOnly: true, expectedSiteId: site.id, expectedRecipientId: site.recipientId, observedSiteId: typeof m.values.siteId === 'string' ? m.values.siteId : 'unqualified', observedRecipientId: typeof m.values.recipientId === 'string' ? m.values.recipientId : 'unqualified' } } | |
| 154 | }, | |
| 155 | close() { | |
| 156 | closed = true; cancel(); observed = null | |
| 157 | if (delivery.outcome === 'in-transit' || delivery.outcome === 'uncertain') { delivery.outcome = 'uncertain'; delivery.reason ??= 'receipt-not-confirmed' } | |
| 158 | }, | |
| 159 | } | |
| 160 | return { port, snapshot: () => structuredClone({ evidence: 'modeled-cargo-delivery-coordinator' as const, physicalFlightEnabled: false as const, identityAuthenticated: false as const, declaration, delivery, readiness: { distinctCaptures: readyCount, sinceMs: readyStartMs, sinceCaptureMs: readyCaptureStartMs, reference: readyReference } }) } | |
| 161 | } | |