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/missions/vehicle.tsmodified+31-10
Changes to resources/functions/drone/missions/vehicle.ts
@@ -1,5 +1,6 @@
11import type { VehicleMissionAction, VehicleMissionObservation, VehicleMissionPlan, VehicleMissionSnapshot, VehicleMissionTask, VehiclePayloadAck, VehiclePayloadCommand, VehicleVtolTransitionCommand, VehicleVtolTransitionObservation } from '../vehicles/types'
22import type { MissionState } from './engine'
3import { vehicleKinds } from '../vehicles/types'
34import { authorityStates } from '../authority/supervisor'
45import { validVector } from '../controls/model'
56import { missionGraphBudget } from './graph'
@@ -15,7 +16,7 @@ export function vehicleMissionContains(plan: VehicleMissionPlan, p: unknown) {
1516export 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 }
1617export function validateVehicleMission(input: unknown): { plan: VehicleMissionPlan, worstCaseMs: number, requiredWh: number } {
1718 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()
1920 const p = input as unknown as VehicleMissionPlan
2021 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)
2122 || !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
3940 edges = [t.then, t.otherwise]
4041 }
4142 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']
4445 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]
4649 }
4750 else if (['takeoff', 'waypoint', 'land', 'stop', 'loiter', 'return'].includes(t.kind)) {
4851 fields = [...fields, 'acceptanceM', 'maxArrivalSpeedMps', 'settleMs', 'timeoutMs', 'next']
4952 if (t.kind !== 'return') { fields.push('positionM'); if (!vehicleMissionContains(p, (t as { positionM?: unknown }).positionM)) invalid() }
5053 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()
5356 if (t.kind === 'land' && t.maxArrivalSpeedMps > 2) invalid()
5457 const terminal = t.kind === 'land' || t.kind === 'stop'
5558 if (!terminal && !id(t.next)) invalid()
@@ -61,6 +64,13 @@ export function validateVehicleMission(input: unknown): { plan: VehicleMissionPl
6164 nodes.push({ id: t.id, durationMs, edges })
6265 }
6366 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 }
6474 if (task.kind !== 'vtol-mode' || task.onFailure === undefined) continue
6575 const hover = p.tasks.find(candidate => candidate.id === task.onFailure)
6676 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) {
110120 let targetM: VehicleMissionSnapshot['targetM'] = null, holdM: VehicleMissionSnapshot['targetM'] = null, pending: VehiclePayloadCommand | null = null
111121 let transition: VehicleVtolTransitionCommand | null = null
112122 let recovery: VehicleMissionSnapshot['recovery']
123 let payloadFailure: VehicleMissionSnapshot['payloadFailure']
113124 let transitionObservation: { capturedAtMs: number, measurementCapturedAtMs: number, signature: string } | null = null
114125 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 } } : {}) } }
116127 function block(why: string) { state = 'blocked'; reason = why; targetM = null; pending = null; transition = null; transitionObservation = null }
117128 function advance(next: string) {
118129 taskId = next; taskElapsedMs = 0; settledMs = 0; pending = null; transition = null; transitionObservation = null; reason = 'task-entered'
@@ -145,6 +156,14 @@ export function createVehicleMissionEngine(input: unknown) {
145156 targetM = [...transition.positionM]; reason = 'vtol-transition-recovery-requested'
146157 return true
147158 }
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 }
148167 return {
149168 snapshot,
150169 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) {
207226 const t = tasks.get(taskId)!, executionDt = previousState === 'ready' || previousState === 'paused' ? 0 : dt
208227 taskElapsedMs += executionDt
209228 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) {
211231 readTransitionObservation(o)
212232 if (active(state) && !beginRecovery(t, o, 'timeout')) block('vtol-transition-outcome-uncertain')
213233 }
@@ -237,7 +257,8 @@ export function createVehicleMissionEngine(input: unknown) {
237257 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! }
238258 const ack = o.payloadAck
239259 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') }
241262 else { advance(t.next); reason = 'payload-confirmed' }
242263 }
243264 }
@@ -251,7 +272,7 @@ export function createVehicleMissionEngine(input: unknown) {
251272 if (t.next) advance(t.next)
252273 else {
253274 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
255276 }
256277 }
257278 }