also looking at this
feat: coordinate modeled two-carrier cargo handover
#103
25 files
+1654
-13
| @@ -0,0 +1,132 @@ | ||
| 1 | import type { GeodeticOrigin } from '../estimation/frames' | |
| 2 | import type { SensorVector } from '../sensors/types' | |
| 3 | import type { VehicleControllerContext, VehiclePayloadCommand, VehiclePayloadPort, VehiclePayloadAck } from '../vehicles/types' | |
| 4 | import { createVehicleMeasurementQualifier, vehicleReference } from '../vehicles/measurements' | |
| 5 | ||
| 6 | interface IntakeDeclaration { origin: GeodeticOrigin, padM: SensorVector, receiverId: string, containerId: string, massKg: number, bootId: bigint, sessionId: bigint } | |
| 7 | export interface ModeledCargoIntakeGrant { evidence: 'modeled-cargo-intake-grant-v1', receiverId: string, bootId: bigint, sessionId: bigint, command: VehiclePayloadCommand, sourceEventId: string, issuedAtMs: number, expiresAtMs: number } | |
| 8 | export interface ModeledCargoIntakeMechanism { evidence: 'modeled-cargo-intake-mechanism', request(grant: ModeledCargoIntakeGrant): void, cancel(eventId: string): void } | |
| 9 | const finiteTuple = (v: unknown, size: number): v is number[] => Array.isArray(v) && v.length === size && Array.from({ length: size }, (_, i) => i).every(i => Object.hasOwn(v, i) && typeof v[i] === 'number' && Number.isFinite(v[i])) | |
| 10 | function record(input: unknown, fields: string[]) { | |
| 11 | if (!input || typeof input !== 'object' || Object.getPrototypeOf(input) !== Object.prototype || Reflect.ownKeys(input).length !== fields.length) throw new TypeError() | |
| 12 | const result: Record<string, unknown> = {} | |
| 13 | for (const field of fields) { const d = Object.getOwnPropertyDescriptor(input, field); if (!d?.enumerable || !Object.hasOwn(d, 'value')) throw new TypeError(); result[field] = d.value } | |
| 14 | return result | |
| 15 | } | |
| 16 | function capture(input: unknown): IntakeDeclaration { | |
| 17 | try { | |
| 18 | const v = record(input, ['origin', 'padM', 'receiverId', 'containerId', 'massKg', 'bootId', 'sessionId']), o = record(v.origin, ['latitudeDeg', 'longitudeDeg', 'ellipsoidHeightM']) | |
| 19 | if (o.latitudeDeg !== 0 || o.longitudeDeg !== 0 || o.ellipsoidHeightM !== 100 || !['receiverId', 'containerId'].every(key => typeof v[key] === 'string' && /^[a-z][a-z0-9-]{0,39}$/.test(v[key] as string)) | |
| 20 | || typeof v.massKg !== 'number' || !Number.isFinite(v.massKg) || v.massKg <= 0 || v.massKg > .25 || ![v.bootId, v.sessionId].every(n => typeof n === 'bigint' && n > 0n && n <= 0xffffffffffffffffn)) throw new TypeError() | |
| 21 | if (!Array.isArray(v.padM) || Reflect.ownKeys(v.padM).length !== 4) throw new TypeError() | |
| 22 | const padM = [0, 1, 2].map(i => { const d = Object.getOwnPropertyDescriptor(v.padM, String(i)); if (!d?.enumerable || !Object.hasOwn(d, 'value') || typeof d.value !== 'number' || !Number.isFinite(d.value) || Math.abs(d.value) > 12) throw new TypeError(); return d.value }) as SensorVector | |
| 23 | if (padM[2] < -1 || padM[2] > 0) throw new TypeError() | |
| 24 | return { origin: { latitudeDeg: 0, longitudeDeg: 0, ellipsoidHeightM: 100 }, padM, receiverId: v.receiverId as string, containerId: v.containerId as string, massKg: v.massKg, bootId: v.bootId as bigint, sessionId: v.sessionId as bigint } | |
| 25 | } | |
| 26 | catch { throw new TypeError('cargo-intake-declaration') } | |
| 27 | } | |
| 28 | ||
| 29 | /** Captured receipt policy. The modeled world separately gates every physical intake against live authority. */ | |
| 30 | export function createCargoIntakeCoordinator(input: unknown, mechanism: ModeledCargoIntakeMechanism) { | |
| 31 | const declaration = capture(input) | |
| 32 | let requestMechanism: ModeledCargoIntakeMechanism['request'], cancelMechanism: ModeledCargoIntakeMechanism['cancel'] | |
| 33 | try { | |
| 34 | const m = record(mechanism, ['evidence', 'request', 'cancel']) | |
| 35 | if (m.evidence !== 'modeled-cargo-intake-mechanism' || typeof m.request !== 'function' || typeof m.cancel !== 'function') throw new TypeError() | |
| 36 | requestMechanism = (m.request as ModeledCargoIntakeMechanism['request']).bind(mechanism) | |
| 37 | cancelMechanism = (m.cancel as ModeledCargoIntakeMechanism['cancel']).bind(mechanism) | |
| 38 | } | |
| 39 | catch { throw new TypeError('cargo-intake-mechanism') } | |
| 40 | const qualifier = createVehicleMeasurementQualifier('quadcopter') | |
| 41 | let observed: VehicleControllerContext | null = null, pending: VehiclePayloadCommand | null = null, grant: ModeledCargoIntakeGrant | null = null | |
| 42 | let closed = false, requested = false, attempted = false, lastNow = -1, lastPoll = -1, lastSequence = -1, captures = 0, sinceMs: number | null = null, sinceCaptureMs: number | null = null | |
| 43 | let readyReference: string | null = null, sourceEventId: string | null = null | |
| 44 | const custody: { outcome: 'not-requested' | 'pending' | 'uncertain' | 'received' | 'rejected', requestedAtMs: number | null, grantedAtMs: number | null, mechanismAtMs: number | null, receiptCapturedAtMs: number | null, acknowledgedAtMs: number | null, sourceEventId: string | null, reason: string | null } = { outcome: 'not-requested', requestedAtMs: null, grantedAtMs: null, mechanismAtMs: null, receiptCapturedAtMs: null, acknowledgedAtMs: null, sourceEventId: null, reason: null } | |
| 45 | function clearReadiness() { captures = 0; sinceMs = null; sinceCaptureMs = null; sourceEventId = null } | |
| 46 | function cancel() { if (grant) { const eventId = grant.command.eventId; grant = null; try { cancelMechanism(eventId) } catch { custody.reason ??= 'intake-cancel-unavailable' } } } | |
| 47 | function qualified(c: VehicleControllerContext) { | |
| 48 | const e = c.estimate, p = e.localPosition, a = e.attitude, m = c.measurement, rate = c.controlRate | |
| 49 | if (!c.enabled || e.evidence !== 'fixture' || e.physicalFlightEnabled !== false || !e.health.localPosition || !e.health.tilt || e.nowMs !== c.nowMs || !p || !a || !m || !rate | |
| 50 | || !vehicleReference(e).startsWith(`${declaration.bootId}:`) || m.reference !== vehicleReference(e) || p.frame !== 'LOCAL_NED' || p.altitudeDatum !== 'WGS84_ELLIPSOID' | |
| 51 | || !p.origin || p.origin.latitudeDeg !== 0 || p.origin.longitudeDeg !== 0 || p.origin.ellipsoidHeightM !== 100 || p.capturedAtMs === null || !Number.isFinite(p.capturedAtMs) || p.capturedAtMs < 0 || c.nowMs - p.capturedAtMs > 200 || p.capturedAtMs > c.nowMs || p.predictedToMs !== c.nowMs | |
| 52 | || !finiteTuple(p.positionM, 3) || !finiteTuple(p.velocityMps, 3) || !finiteTuple(a.quaternionWxyz, 4) || Math.abs(Math.hypot(...a.quaternionWxyz) - 1) > 1e-6 || a.reference !== 'GRAVITY_ALIGNED_RELATIVE_YAW' | |
| 53 | || m.capturedAtMs > c.nowMs || c.nowMs - m.capturedAtMs > 80 || m.receivedAtMs > c.nowMs || rate.evidence !== 'modeled-qualified-imu' || !Number.isFinite(rate.capturedAtMs) || rate.capturedAtMs < 0 || rate.reference !== m.reference || rate.capturedAtMs !== a.capturedAtMs || rate.capturedAtMs > c.nowMs || c.nowMs - rate.capturedAtMs > 60 || !finiteTuple(rate.angularRateRadS, 3) | |
| 54 | || m.values.receiverId !== declaration.receiverId || m.values.siteClear !== true || m.values.onSurface !== true || m.values.stopped !== true) return false | |
| 55 | const blocks = p.covarianceBlocks | |
| 56 | if (!Array.isArray(blocks) || blocks.length !== 3 || !blocks.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)) return false | |
| 57 | const sigma = Math.sqrt(Math.max(...blocks.map(b => b.positionM2))) | |
| 58 | return Math.hypot(...p.velocityMps) <= .1 && Math.hypot(...p.positionM.map((n, i) => n - declaration.padM[i]!)) + 3 * sigma <= .2 | |
| 59 | && Math.acos(Math.max(-1, Math.min(1, 1 - 2 * (a.quaternionWxyz[1] ** 2 + a.quaternionWxyz[2] ** 2)))) <= .15 && Math.hypot(...rate.angularRateRadS) <= .2 | |
| 60 | } | |
| 61 | function ack(nowMs: number, outcome: VehiclePayloadAck['outcome']): VehiclePayloadAck | null { | |
| 62 | const command = pending!, c = observed!, m = c.measurement!, pose = c.measurementPose! | |
| 63 | const result: VehiclePayloadAck = { eventId: command.eventId, taskId: command.taskId, operation: command.operation, containerId: command.containerId, reference: command.reference, capturedAtMs: m.capturedAtMs, positionM: [...pose.positionM], outcome, | |
| 64 | metadata: { modeledOnly: true, receiverId: declaration.receiverId, sourceEventId: custody.sourceEventId ?? 'unavailable', reason: custody.reason ?? 'captured-intake' } } | |
| 65 | // Consume the terminal command before calling the injected adapter. It may | |
| 66 | // synchronously close or poll this port, but cannot consume the same ACK. | |
| 67 | pending = null | |
| 68 | if (outcome !== 'confirmed') cancel() | |
| 69 | else grant = null | |
| 70 | if (closed) return null | |
| 71 | custody.acknowledgedAtMs = nowMs | |
| 72 | return result | |
| 73 | } | |
| 74 | const port: VehiclePayloadPort = { | |
| 75 | observe(input) { | |
| 76 | if (closed) return | |
| 77 | const c = structuredClone(input) | |
| 78 | if (!Number.isSafeInteger(c.nowMs) || c.nowMs < 0 || c.nowMs < lastNow) throw new TypeError('cargo-intake-clock') | |
| 79 | if (lastNow >= 0 && c.nowMs - lastNow > 40) clearReadiness() | |
| 80 | lastNow = c.nowMs | |
| 81 | try { c.measurement = qualifier.qualify(c.measurement, c.nowMs, vehicleReference(c.estimate)) } catch { c.measurement = null; c.measurementPose = null } | |
| 82 | observed = c | |
| 83 | if (pending && (!c.enabled || c.mission.payloadFailure?.eventId === pending.eventId || c.mission.state !== 'running' || c.mission.taskId !== pending.taskId || c.mission.payload?.eventId !== pending.eventId)) { cancel(); custody.outcome = attempted ? 'uncertain' : 'rejected'; custody.reason ??= 'intake-authority-unavailable' } | |
| 84 | const m = c.measurement, event = m?.values.stationSourceEventId, sequence = m?.sequence ?? -1 | |
| 85 | const ready = qualified(c) && m?.values.loadAttached === false && m.values.stationReady === true && m.values.stationContainerId === declaration.containerId | |
| 86 | && typeof event === 'string' && event.length > 0 && event.length <= 160 && (readyReference === null || readyReference === m.reference) && (sourceEventId === null || sourceEventId === event) | |
| 87 | if (!ready) clearReadiness() | |
| 88 | if (sequence !== lastSequence) { | |
| 89 | if (ready && sequence > lastSequence) { captures++; sinceMs ??= c.nowMs; sinceCaptureMs ??= m!.capturedAtMs; sourceEventId = event as string; readyReference = m!.reference } | |
| 90 | else clearReadiness() | |
| 91 | lastSequence = sequence | |
| 92 | } | |
| 93 | }, | |
| 94 | request(input) { | |
| 95 | if (closed || requested) throw new TypeError('cargo-intake-command-replayed') | |
| 96 | requested = true; pending = structuredClone(input); custody.requestedAtMs = pending.issuedAtMs; custody.outcome = 'pending' | |
| 97 | const c = observed | |
| 98 | if (!c || c.mission.state !== 'running' || c.mission.vehicle !== 'quadcopter' || c.mission.taskKind !== 'payload' || !c.mission.payload || JSON.stringify(c.mission.payload) !== JSON.stringify(pending) | |
| 99 | || pending.operation !== 'material-receive' || pending.containerId !== declaration.containerId || pending.reference !== vehicleReference(c.estimate) || pending.issuedAtMs !== c.nowMs || !Number.isSafeInteger(pending.expiresAtMs) || pending.expiresAtMs <= c.nowMs || pending.expiresAtMs - c.nowMs > 60000 || !qualified(c)) { | |
| 100 | custody.outcome = 'rejected'; custody.reason = 'intake-command-unqualified' | |
| 101 | } | |
| 102 | }, | |
| 103 | poll(nowMs) { | |
| 104 | if (!Number.isSafeInteger(nowMs) || nowMs < 0 || nowMs < lastPoll) throw new TypeError('cargo-intake-clock') | |
| 105 | lastPoll = nowMs | |
| 106 | const c = observed, m = c?.measurement, pose = c?.measurementPose | |
| 107 | if (closed || !pending || !c || c.nowMs !== nowMs || !qualified(c) || !m || !pose || pose.reference !== pending.reference || m.reference !== pending.reference || pose.capturedAtMs !== m.capturedAtMs || pose.positionPredictedToMs !== m.capturedAtMs || !finiteTuple(pose.positionM, 3) || !pose.positionM.every(value => Math.abs(value) <= 1e6) || !finiteTuple(pending.positionM, 3) || !pending.positionM.every(value => Math.abs(value) <= 1e6) || Math.hypot(...pose.positionM.map((value, i) => value - pending!.positionM[i]!)) > 1 | |
| 108 | || m.capturedAtMs < pending.issuedAtMs || m.capturedAtMs >= pending.expiresAtMs || nowMs >= pending.expiresAtMs) return null | |
| 109 | if (custody.outcome === 'rejected') return ack(nowMs, 'rejected') | |
| 110 | if (custody.outcome === 'uncertain' && custody.reason !== null) return ack(nowMs, 'uncertain') | |
| 111 | if (!attempted && captures >= 26 && sinceMs !== null && nowMs - sinceMs >= 500 && sinceCaptureMs !== null && m.capturedAtMs - sinceCaptureMs >= 500 && sourceEventId !== null) { | |
| 112 | attempted = true; custody.outcome = 'uncertain'; custody.grantedAtMs = nowMs; custody.sourceEventId = sourceEventId | |
| 113 | grant = { evidence: 'modeled-cargo-intake-grant-v1', receiverId: declaration.receiverId, bootId: declaration.bootId, sessionId: declaration.sessionId, command: structuredClone(pending), sourceEventId, issuedAtMs: nowMs, expiresAtMs: Math.min(nowMs + 40, pending.expiresAtMs) } | |
| 114 | try { requestMechanism(structuredClone(grant)) } catch { custody.reason = 'intake-transport-unavailable' } | |
| 115 | if (closed || !pending) return null | |
| 116 | // An attempted mechanism is uncertain until a later captured observation, never this request. | |
| 117 | return null | |
| 118 | } | |
| 119 | if (!attempted || !grant) return null | |
| 120 | const atMs = m.values.intakeAtMs | |
| 121 | const correlated = m.values.intakeEventId === pending.eventId && m.values.intakeSourceEventId === grant.sourceEventId | |
| 122 | && typeof atMs === 'number' && Number.isSafeInteger(atMs) && atMs > grant.issuedAtMs && atMs < grant.expiresAtMs && atMs <= m.capturedAtMs | |
| 123 | if (correlated && m.values.intakeOutcome === 'attached' && m.values.loadAttached === true && m.values.intakeMassKg === declaration.massKg) { custody.outcome = 'received'; custody.mechanismAtMs = atMs; custody.receiptCapturedAtMs = m.capturedAtMs; return ack(nowMs, 'confirmed') } | |
| 124 | if (correlated && m.values.intakeOutcome === 'refused' && m.values.loadAttached === false && m.values.intakeMassKg === 0) { custody.outcome = 'rejected'; custody.mechanismAtMs = atMs as number; custody.receiptCapturedAtMs = m.capturedAtMs; custody.reason = 'point-joint-refused'; return ack(nowMs, 'rejected') } | |
| 125 | if (m.values.intakeEventId !== '' || nowMs > grant.expiresAtMs + 120) { custody.outcome = 'uncertain'; custody.reason = 'intake-receipt-unqualified'; return ack(nowMs, 'uncertain') } | |
| 126 | return null | |
| 127 | }, | |
| 128 | close() { if (closed) return; closed = true; cancel(); observed = null; pending = null; if (custody.outcome === 'pending') { custody.outcome = 'rejected'; custody.reason ??= 'intake-not-attempted' } | |
| 129 | else if (custody.outcome === 'uncertain') custody.reason ??= 'intake-receipt-not-confirmed' }, | |
| 130 | } | |
| 131 | return { port, snapshot: () => structuredClone({ evidence: 'modeled-cargo-intake-coordinator' as const, physicalFlightEnabled: false as const, identityAuthenticated: false as const, declaration, custody, grant, readiness: { captures, sinceMs, sinceCaptureMs, reference: readyReference, sourceEventId } }) } | |
| 132 | } | |