ReviewOS

also looking at this

stacks/drone

feat: add shared-clock scoped vehicle simulation sessions

#98
Merged chris wants to merge codex/cargo-fleet-runtime into main
42 files +2764 -127
resources/functions/drone/cargo/coordinator.tsadded+161-0
Changes to resources/functions/drone/cargo/coordinator.ts
@@ -0,0 +1,161 @@
1import type { GeodeticOrigin } from '../estimation/frames'
2import type { SensorVector } from '../sensors/types'
3import type { VehicleControllerContext, VehiclePayloadAck, VehiclePayloadCommand, VehiclePayloadPort } from '../vehicles/types'
4import { geodeticToEcef } from '../estimation/frames'
5import { authorityPolicy } from '../authority/supervisor'
6import { createVehicleMeasurementQualifier, vehicleReference } from '../vehicles/measurements'
7
8export interface CargoDeliveryDeclaration {
9 origin: GeodeticOrigin
10 site: { id: string, recipientId: string, containerId: string, positionM: SensorVector, radiusM: number }
11 releaseHeightM: number
12}
13export 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}
25export interface ModeledCargoMechanism {
26 evidence: 'modeled-cargo-mechanism'
27 request(command: VehiclePayloadCommand): void
28 cancel(eventId: string): void
29}
30function 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}
40export 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}
56function 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. */
61export 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}