diff --git a/README.md b/README.md index 9037e4b..08ae94f 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,9 @@ comfortable immersive 3D reconstruction of its surroundings. Ito is being reset around the v1 design in `docs/v1.md`. Protocol seams are documented in `docs/protocol.md`, and architectural decisions are recorded in -`docs/adr/`. +`docs/adr/`. Local Docker Compose operation is documented in +`docs/local-v1.md`, and the current v1 acceptance record is in +`docs/acceptance-v1.md`. The main source directories are: @@ -30,4 +32,4 @@ The main source directories are: - `server/processors`: 3D reconstruction algorithms, applied. - `client/`: WebXR Pilot Client code. - `drivers/`: robot-side drivers and robot reference material. -- `docs/`: design notes, protocol notes, and architectural decisions. \ No newline at end of file +- `docs/`: design notes, protocol notes, and architectural decisions. diff --git a/client/README.md b/client/README.md index bfea66e..3291894 100644 --- a/client/README.md +++ b/client/README.md @@ -3,5 +3,40 @@ The Pilot Client is the WebXR application used by the pilot to perceive through and control a robot. -This directory is currently a placeholder while Ito is reset around the v1 -design in `../docs/v1.md`. +The v1 client is a static, plain-JavaScript A-Frame/WebXR application. The +non-VR page only exposes the browser-required Enter VR launch action; catalog, +acquisition, settings, session state, and session end controls are rendered in +VR. + +## Run locally + +From this directory: + +```sh +python -m http.server 8080 +``` + +Then open `http://localhost:8080/`. The client defaults to the Ito Server +control WebSocket at `ws://:8765` and stores runtime settings in +browser Local Storage under `ito.pilotClient.settings.v1`. + +## Tests + +The client uses Node's built-in test runner and has no npm dependencies. + +```sh +npm test +``` + +## Implementation notes + +- WebSocket control-plane messages are MessagePack-encoded Ito envelopes. +- Pilot-facing text is loaded from `resources/en/default.json` and resolved by + resource key before falling back to driver/server free text. +- The Splat Scene is client-owned. `src/splat-scene.js` includes the Spark.JS + adapter seam and v1 Splat Batch binary header parser. Exact Spark insertion + performance still needs Pico 4 validation. +- Pilot Input Snapshots are generated at the configured rate with headset yaw + relative to session start plus current controller state. `src/webrtc.js` + creates non-trickle WebRTC offers for pilot-input and Splat Batch data + channels over the WebSocket control plane. diff --git a/client/index.html b/client/index.html new file mode 100644 index 0000000..b24fe61 --- /dev/null +++ b/client/index.html @@ -0,0 +1,43 @@ + + + + + + Ito Pilot + + + + + +
+

Ito Pilot

+ +

+
+ + + + + + + + + + + + + + + diff --git a/client/package.json b/client/package.json new file mode 100644 index 0000000..9b32aae --- /dev/null +++ b/client/package.json @@ -0,0 +1,9 @@ +{ + "name": "ito-pilot-client", + "version": "0.1.0", + "private": true, + "type": "module", + "scripts": { + "test": "node --test tests/*.test.js" + } +} diff --git a/client/resources/en/default.json b/client/resources/en/default.json new file mode 100644 index 0000000..eacd9be --- /dev/null +++ b/client/resources/en/default.json @@ -0,0 +1,88 @@ +{ + "app": { + "title": "Ito Pilot", + "enterVr": "Enter VR", + "webxrUnavailable": "Immersive VR is not available in this browser.", + "loading": "Loading..." + }, + "catalog": { + "title": "Robot Catalog", + "refresh": "Refresh", + "empty": "No robots are reporting yet.", + "acquire": "Pilot", + "unavailable": "Unavailable" + }, + "connection": { + "connecting": "Connecting...", + "connected": "Connected", + "failed": "Connection failed" + }, + "session": { + "connecting": "Connecting to {{name}}...", + "active": "Piloting {{name}}", + "ending": "Ending session...", + "ended": "Session ended", + "returnToCatalog": "Return to Catalog", + "visualPaused": "Visual feed lost. Robot controls paused.", + "menu": "Menu", + "resume": "Resume", + "end": "End Session", + "settings": "Settings" + }, + "settings": { + "title": "Settings", + "serverUrl": "Server", + "visualFreshnessTimeoutMs": "Visual timeout", + "pilotInputRateHz": "Input rate", + "splatBudget": "Splat budget", + "splatLifetimeMs": "Splat lifetime", + "save": "Save", + "reset": "Reset" + }, + "enum": { + "robotType": { + "Mecha": "Mecha", + "Android Robot": "Android Robot", + "Droid": "Droid", + "Drone": "Drone", + "Car": "Car", + "Plane": "Plane" + }, + "robotStatus": { + "Available": "Available", + "Occupied": "Occupied", + "Unavailable": "Unavailable" + } + }, + "reason": { + "unknown": "Unknown reason.", + "request": { + "timeout": "The request timed out." + }, + "robot": { + "unavailable": "The robot is unavailable.", + "driver_disconnected": "The robot driver disconnected.", + "driver_status_timeout": "The robot stopped reporting status." + }, + "session": { + "acquire": { + "robot_unavailable": "That robot is not available." + }, + "ended": { + "requested": "The session was ended.", + "pilot_requested": "The pilot ended the session.", + "endpoint_disappeared": "A required endpoint disconnected.", + "reconstruction_failed": "Reconstruction failed." + }, + "resume_unavailable": "The previous session can no longer be resumed." + }, + "connection": { + "hello_required": "The server rejected the connection handshake.", + "invalid_role": "The server rejected the client role." + }, + "protocol": { + "invalid_message": "The server rejected an invalid protocol message.", + "version_mismatch": "The Ito protocol versions do not match." + } + } +} diff --git a/client/src/app.js b/client/src/app.js new file mode 100644 index 0000000..9a7aed3 --- /dev/null +++ b/client/src/app.js @@ -0,0 +1,357 @@ +import { ClientSettingsStore, mergeSessionConfig } from "./config.js"; +import { ItoControlClient, DisplayableError } from "./control-client.js"; +import { TextResources } from "./i18n.js"; +import { DataChannelPilotInputTransport, PilotInputLoop } from "./pilot-input.js"; +import { displayReason, ROBOT_STATUS_AVAILABLE } from "./protocol.js"; +import { SparkJsSplatAdapter, SplatSceneOwner } from "./splat-scene.js"; +import { VisualFreshnessMonitor } from "./visual-freshness.js"; +import { VrUi } from "./vr-ui.js"; + +export class ItoPilotApp { + constructor({ scene, uiRoot, splatRoot, launchButton, statusElement }) { + this.scene = scene; + this.uiRoot = uiRoot; + this.splatRoot = splatRoot; + this.launchButton = launchButton; + this.statusElement = statusElement; + this.settingsStore = new ClientSettingsStore(); + this.settings = this.settingsStore.load(); + this.text = new TextResources(); + this.ui = null; + this.control = null; + this.catalogRobots = []; + this.selectedRobot = null; + this.session = null; + this.menuOpen = false; + this.visualPaused = false; + this.splatScene = null; + this.freshness = null; + this.pilotInput = new PilotInputLoop({ + transport: new DataChannelPilotInputTransport(), + rateHz: this.settings.pilotInputRateHz, + }); + this.xrReferenceSpace = null; + } + + async init() { + this.text = await TextResources.load(); + this.ui = new VrUi(this.uiRoot, this.text); + this.launchButton.textContent = this.text.t("app.enterVr"); + this.statusElement.textContent = ""; + this.launchButton.addEventListener("click", () => this.enterVr()); + this.scene.addEventListener("click", (event) => this.handleClick(event)); + this.scene.addEventListener("enter-vr", () => this.connectAndShowCatalog()); + this.scene.addEventListener("loaded", () => this.installControllerMenuHandlers()); + this.scene.addEventListener("xrframe", (event) => this.onXrFrame(event.detail)); + + if (!(await navigator.xr?.isSessionSupported?.("immersive-vr"))) { + this.statusElement.textContent = this.text.t("app.webxrUnavailable"); + this.launchButton.disabled = true; + } + } + + async enterVr() { + if (!this.scene.is("loaded")) { + await new Promise((resolve) => this.scene.addEventListener("loaded", resolve, { once: true })); + } + this.scene.enterVR(); + } + + async connectAndShowCatalog() { + try { + this.renderStatus(this.text.t("connection.connecting")); + this.control = new ItoControlClient({ + serverUrl: this.settings.serverUrl, + requestTimeoutMs: this.settings.requestTimeoutMs, + }); + this.control.addEventListener("sessionended", (event) => this.handleSessionEnded(event.detail)); + await this.control.connect(); + await this.showCatalog(); + } catch (error) { + this.renderStatus(this.reasonText(error)); + } + } + + async showCatalog(reason = null) { + this.session = null; + this.selectedRobot = null; + this.menuOpen = false; + this.visualPaused = false; + this.pilotInput.stop(); + this.splatScene?.clear(); + this.splatScene = null; + + const panel = this.ui.panel({ + title: this.text.t("catalog.title"), + subtitle: reason ? this.text.displayReason(reason) : "", + }); + this.ui.button(panel, { + label: this.text.t("catalog.refresh"), + position: "1.05 0.82 0.02", + action: "catalog.refresh", + width: 0.72, + }); + this.ui.button(panel, { + label: this.text.t("session.settings"), + position: "0.2 0.82 0.02", + action: "settings.open", + width: 0.72, + }); + + try { + this.catalogRobots = await this.control.getCatalog(); + this.renderCatalogRows(panel); + } catch (error) { + this.ui.label(panel, this.reasonText(error), "-1.42 0.38 0.02", { color: "#ffd2c9" }); + } + } + + renderCatalogRows(panel) { + if (this.catalogRobots.length === 0) { + this.ui.label(panel, this.text.t("catalog.empty"), "-1.42 0.32 0.02"); + return; + } + this.catalogRobots.slice(0, 6).forEach((robot, index) => { + const y = 0.46 - index * 0.28; + const type = this.text.enumLabel("robotType", robot.type); + const status = this.text.enumLabel("robotStatus", robot.status); + const detail = robot.availabilityDetail ? ` - ${this.text.displayReason(robot.availabilityDetail)}` : ""; + this.ui.label(panel, `${robot.name} ${type} ${status}${detail}`, "-1.42 " + y + " 0.02", { + width: 2.2, + color: robot.status === ROBOT_STATUS_AVAILABLE ? "#f7fbff" : "#98a7b7", + }); + this.ui.button(panel, { + label: robot.status === ROBOT_STATUS_AVAILABLE ? this.text.t("catalog.acquire") : this.text.t("catalog.unavailable"), + position: `1.02 ${y + 0.02} 0.02`, + enabled: robot.status === ROBOT_STATUS_AVAILABLE, + action: "catalog.acquire", + detail: robot, + width: 0.72, + }); + }); + } + + async acquireRobot(robot) { + this.selectedRobot = robot; + const panel = this.ui.panel({ + title: this.text.t("connection.connecting"), + subtitle: this.text.t("session.connecting", { name: robot.name }), + }); + this.ui.label(panel, this.text.t("connection.connecting"), "-1.42 0.35 0.02"); + try { + const acquisition = await this.control.acquire(robot.robotId); + this.startSession(robot, acquisition); + } catch (error) { + await this.showCatalog(error.reason); + } + } + + startSession(robot, acquisition) { + const sessionConfig = mergeSessionConfig(this.settings, acquisition.sessionConfig || {}); + this.session = { + sessionId: acquisition.sessionId, + robotId: acquisition.robotId, + robot, + sessionConfig, + ended: false, + }; + this.splatScene = new SplatSceneOwner({ + adapter: new SparkJsSplatAdapter(this.splatRoot), + budget: sessionConfig.splatBudget, + lifetimeMs: sessionConfig.splatLifetimeMs, + }); + this.freshness = new VisualFreshnessMonitor({ timeoutMs: sessionConfig.visualFreshnessTimeoutMs }); + this.freshness.addEventListener("stale", () => this.setVisualPaused(true)); + this.freshness.addEventListener("fresh", () => this.setVisualPaused(false)); + this.freshness.markFresh(); + this.pilotInput.rateHz = sessionConfig.pilotInputRateHz; + this.pilotInput.start(); + this.renderSession(); + } + + receiveSplatBatch(payload, metadata = {}) { + if (!this.session || this.session.ended || !this.splatScene) return null; + if (this.visualPaused) this.freshness?.markFresh(); + const batch = this.splatScene.applySplatBatch(payload, metadata); + if (batch) this.freshness?.markFresh(); + return batch; + } + + renderSession() { + const panel = this.ui.panel({ + title: this.text.t("session.active", { name: this.session.robot.name }), + subtitle: this.visualPaused ? this.text.t("session.visualPaused") : this.text.t("connection.connected"), + width: 2.4, + height: this.menuOpen ? 1.52 : 0.72, + position: "0 1.85 -2.8", + }); + this.ui.button(panel, { + label: this.menuOpen ? this.text.t("session.resume") : this.text.t("session.menu"), + position: "-0.48 0.0 0.02", + action: "session.menu.toggle", + width: 0.82, + }); + if (this.menuOpen) { + this.ui.button(panel, { + label: this.text.t("session.end"), + position: "0.48 0.0 0.02", + action: "session.end", + width: 0.82, + }); + this.ui.button(panel, { + label: this.text.t("session.settings"), + position: "0 -0.34 0.02", + action: "settings.open", + width: 0.92, + }); + } + } + + setVisualPaused(paused) { + this.visualPaused = paused; + this.splatScene?.setFrozen(paused); + if (paused) this.pilotInput.stop(); + if (!paused && !this.menuOpen && !this.session?.ended) this.pilotInput.start(); + if (this.session && !this.session.ended) this.renderSession(); + } + + toggleMenu() { + if (!this.session?.sessionId || this.session.ended) return; + this.menuOpen = !this.menuOpen; + if (this.menuOpen) this.pilotInput.stop(); + if (!this.menuOpen && !this.visualPaused) this.pilotInput.start(); + this.renderSession(); + } + + async endSession() { + if (!this.session?.sessionId) return; + this.pilotInput.stop(); + this.ui.panel({ title: this.text.t("session.ending"), subtitle: this.session.robot.name }); + try { + await this.control.endSession(this.session.sessionId); + } catch (error) { + this.handleSessionEnded({ + payload: { reason: error.reason || displayReason("session.ended.requested"), endedBy: "pilotClient", clean: false }, + sessionId: this.session.sessionId, + }); + } + } + + handleSessionEnded(envelope) { + if (!this.session) return; + this.session.ended = true; + this.pilotInput.stop(); + this.splatScene?.setFrozen(true); + const reason = envelope.payload?.reason || displayReason("session.ended.requested"); + const panel = this.ui.panel({ + title: this.text.t("session.ended"), + subtitle: this.text.displayReason(reason), + width: 2.8, + height: 1.35, + position: "0 1.65 -2.3", + }); + this.ui.button(panel, { + label: this.text.t("session.returnToCatalog"), + position: "0 -0.28 0.02", + action: "session.returnCatalog", + width: 1.35, + }); + } + + showSettings() { + const panel = this.ui.panel({ + title: this.text.t("settings.title"), + subtitle: this.settings.serverUrl, + width: 3.2, + height: 2.25, + }); + const rows = [ + ["visualFreshnessTimeoutMs", 250], + ["pilotInputRateHz", 5], + ["splatBudget", 20], + ["splatLifetimeMs", 5000], + ]; + rows.forEach(([key, step], index) => { + const y = 0.48 - index * 0.3; + this.ui.label(panel, `${this.text.t(`settings.${key}`)}: ${this.settings[key]}`, "-1.42 " + y + " 0.02", { + width: 1.7, + }); + this.ui.button(panel, { label: "-", position: `0.52 ${y + 0.02} 0.02`, action: "settings.adjust", detail: { key, delta: -step }, width: 0.22 }); + this.ui.button(panel, { label: "+", position: `0.84 ${y + 0.02} 0.02`, action: "settings.adjust", detail: { key, delta: step }, width: 0.22 }); + }); + this.ui.button(panel, { + label: this.text.t("settings.save"), + position: "-0.42 -0.82 0.02", + action: "settings.save", + width: 0.72, + }); + this.ui.button(panel, { + label: this.session ? this.text.t("session.resume") : this.text.t("session.returnToCatalog"), + position: "0.48 -0.82 0.02", + action: "settings.close", + width: 1.05, + }); + } + + handleClick(event) { + const target = event.target.closest?.("[data-action]"); + const action = target?.getAttribute("data-action"); + if (!action) return; + const detail = target.itoActionDetail; + if (action === "catalog.refresh") this.showCatalog(); + if (action === "catalog.acquire") this.acquireRobot(detail); + if (action === "session.menu.toggle") this.toggleMenu(); + if (action === "session.end") this.endSession(); + if (action === "session.returnCatalog") this.showCatalog(); + if (action === "settings.open") this.showSettings(); + if (action === "settings.adjust") { + this.settings[detail.key] += detail.delta; + this.settings = this.settingsStore.save(this.settings); + this.showSettings(); + } + if (action === "settings.save") { + this.settings = this.settingsStore.save(this.settings); + if (this.session?.sessionConfig && this.splatScene) { + this.session.sessionConfig = mergeSessionConfig(this.settings, this.session.sessionConfig); + this.splatScene.setLimits(this.session.sessionConfig); + } + this.showSettings(); + } + if (action === "settings.close") { + if (this.session && !this.session.ended) this.renderSession(); + else this.showCatalog(); + } + } + + onXrFrame({ frame, referenceSpace }) { + if (!frame || !referenceSpace || !this.session || this.session.ended) return; + this.xrReferenceSpace = referenceSpace; + this.freshness?.tick(); + this.splatScene?.evict(); + this.pilotInput.maybeSend(frame, referenceSpace, this.session.sessionId); + } + + installControllerMenuHandlers() { + for (const id of ["left-controller", "right-controller"]) { + const controller = document.getElementById(id); + controller?.addEventListener("abuttondown", () => this.toggleMenu()); + controller?.addEventListener("xbuttondown", () => this.toggleMenu()); + controller?.addEventListener("menudown", () => this.toggleMenu()); + } + } + + renderStatus(message) { + const panel = this.ui.panel({ title: message || this.text.t("app.title"), height: 1.0 }); + this.ui.button(panel, { + label: this.text.t("catalog.refresh"), + position: "0 -0.2 0.02", + action: "catalog.refresh", + width: 0.85, + }); + } + + reasonText(error) { + if (error instanceof DisplayableError) return this.text.displayReason(error.reason); + return error?.message || this.text.t("reason.unknown"); + } +} diff --git a/client/src/config.js b/client/src/config.js new file mode 100644 index 0000000..0eb4ab9 --- /dev/null +++ b/client/src/config.js @@ -0,0 +1,90 @@ +const STORAGE_KEY = "ito.pilotClient.settings.v1"; + +export const DEFAULT_SETTINGS = Object.freeze({ + serverUrl: defaultServerUrl(), + requestTimeoutMs: 5000, + visualFreshnessTimeoutMs: 2000, + pilotInputRateHz: 60, + splatBudget: 180, + splatLifetimeMs: 30000, +}); + +const SETTING_LIMITS = Object.freeze({ + requestTimeoutMs: [500, 30000], + visualFreshnessTimeoutMs: [250, 10000], + pilotInputRateHz: [1, 120], + splatBudget: [1, 10000], + splatLifetimeMs: [1000, 300000], +}); + +export class ClientSettingsStore { + constructor(storage = globalThis.localStorage) { + this.storage = storage; + } + + load() { + const raw = this.storage?.getItem(STORAGE_KEY); + if (!raw) return { ...DEFAULT_SETTINGS }; + try { + return normalizeSettings({ ...DEFAULT_SETTINGS, ...JSON.parse(raw) }); + } catch { + return { ...DEFAULT_SETTINGS }; + } + } + + save(settings) { + const normalized = normalizeSettings({ ...DEFAULT_SETTINGS, ...settings }); + this.storage?.setItem(STORAGE_KEY, JSON.stringify(normalized)); + return normalized; + } + + clear() { + this.storage?.removeItem(STORAGE_KEY); + } +} + +export function normalizeSettings(settings) { + const normalized = { ...settings }; + normalized.serverUrl = + typeof normalized.serverUrl === "string" && normalized.serverUrl.trim() + ? normalized.serverUrl.trim() + : DEFAULT_SETTINGS.serverUrl; + + for (const [key, [minimum, maximum]] of Object.entries(SETTING_LIMITS)) { + normalized[key] = clampInteger(normalized[key], DEFAULT_SETTINGS[key], minimum, maximum); + } + return normalized; +} + +export function mergeSessionConfig(settings, sessionConfig = {}) { + return { + ...sessionConfig, + pilotInputRateHz: clampInteger( + sessionConfig.pilotInputRateHz ?? settings.pilotInputRateHz, + settings.pilotInputRateHz, + SETTING_LIMITS.pilotInputRateHz[0], + SETTING_LIMITS.pilotInputRateHz[1], + ), + visualFreshnessTimeoutMs: clampInteger( + sessionConfig.visualFreshnessTimeoutMs ?? settings.visualFreshnessTimeoutMs, + settings.visualFreshnessTimeoutMs, + SETTING_LIMITS.visualFreshnessTimeoutMs[0], + SETTING_LIMITS.visualFreshnessTimeoutMs[1], + ), + splatBudget: settings.splatBudget, + splatLifetimeMs: settings.splatLifetimeMs, + }; +} + +function clampInteger(value, fallback, minimum, maximum) { + const number = Number(value); + if (!Number.isFinite(number)) return fallback; + return Math.min(maximum, Math.max(minimum, Math.round(number))); +} + +function defaultServerUrl() { + const location = globalThis.location; + if (!location?.host) return "ws://localhost:8765"; + const scheme = location.protocol === "https:" ? "wss:" : "ws:"; + return `${scheme}//${location.hostname}:8765`; +} diff --git a/client/src/control-client.js b/client/src/control-client.js new file mode 100644 index 0000000..6fd5faf --- /dev/null +++ b/client/src/control-client.js @@ -0,0 +1,143 @@ +import { + MESSAGE_TYPES, + ROLE_PILOT_CLIENT, + displayReason, + makeEnvelope, + packEnvelope, + resultReason, + unpackEnvelope, +} from "./protocol.js"; + +export class ItoControlClient extends EventTarget { + constructor({ serverUrl, requestTimeoutMs = 5000, sessionId = null, WebSocketImpl = globalThis.WebSocket }) { + super(); + this.serverUrl = serverUrl; + this.requestTimeoutMs = requestTimeoutMs; + this.sessionId = sessionId; + this.WebSocketImpl = WebSocketImpl; + this.websocket = null; + this.pending = new Map(); + } + + async connect() { + if (this.websocket?.readyState === this.WebSocketImpl.OPEN) return; + this.websocket = new this.WebSocketImpl(this.serverUrl); + this.websocket.binaryType = "arraybuffer"; + this.websocket.addEventListener("message", (event) => this.handleMessage(event.data)); + this.websocket.addEventListener("close", () => this.dispatchEvent(new Event("closed"))); + this.websocket.addEventListener("error", () => this.dispatchEvent(new Event("error"))); + + await new Promise((resolve, reject) => { + this.websocket.addEventListener("open", resolve, { once: true }); + this.websocket.addEventListener("error", reject, { once: true }); + }); + + const payload = { role: ROLE_PILOT_CLIENT }; + if (this.sessionId) payload.sessionId = this.sessionId; + const hello = await this.request(MESSAGE_TYPES.CONNECTION_HELLO, payload, MESSAGE_TYPES.CONNECTION_HELLO_RESULT); + if (!hello.ok) throw new DisplayableError(resultReason(hello)); + return hello.value; + } + + close() { + this.websocket?.close(); + this.websocket = null; + for (const pending of this.pending.values()) { + pending.reject(new DisplayableError(displayReason("connection.closed"))); + clearTimeout(pending.timeoutId); + } + this.pending.clear(); + } + + async getCatalog() { + const result = await this.request( + MESSAGE_TYPES.CATALOG_GET, + { includeUnavailable: true }, + MESSAGE_TYPES.CATALOG_GET_RESULT, + ); + if (!result.ok) throw new DisplayableError(resultReason(result)); + return result.value.robots || []; + } + + async acquire(robotId) { + const result = await this.request( + MESSAGE_TYPES.SESSION_ACQUIRE, + { robotId }, + MESSAGE_TYPES.SESSION_ACQUIRE_RESULT, + { robotId }, + ); + if (!result.ok) throw new DisplayableError(resultReason(result)); + this.sessionId = result.value.sessionId; + return result.value; + } + + async endSession(sessionId, reason = displayReason("session.ended.pilot_requested"), clean = true) { + const result = await this.request( + MESSAGE_TYPES.SESSION_END, + { reason, clean }, + MESSAGE_TYPES.SESSION_END_RESULT, + { sessionId }, + ); + if (!result.ok) throw new DisplayableError(resultReason(result)); + return result.value; + } + + request(type, payload, expectedType, options = {}) { + const envelope = makeEnvelope(type, payload, { + robotId: options.robotId, + sessionId: options.sessionId, + }); + return new Promise((resolve, reject) => { + const timeoutId = setTimeout(() => { + this.pending.delete(envelope.messageId); + reject(new DisplayableError(displayReason("request.timeout"))); + }, this.requestTimeoutMs); + this.pending.set(envelope.messageId, { expectedType, resolve, reject, timeoutId }); + this.send(envelope); + }); + } + + send(envelope) { + if (!this.websocket || this.websocket.readyState !== this.WebSocketImpl.OPEN) { + throw new DisplayableError(displayReason("connection.closed")); + } + this.websocket.send(packEnvelope(envelope)); + } + + handleMessage(frame) { + let envelope; + try { + envelope = unpackEnvelope(frame); + } catch (error) { + this.dispatchEvent(new CustomEvent("protocolerror", { detail: error })); + return; + } + + if (envelope.replyToMessageId && this.pending.has(envelope.replyToMessageId)) { + const pending = this.pending.get(envelope.replyToMessageId); + this.pending.delete(envelope.replyToMessageId); + clearTimeout(pending.timeoutId); + if (pending.expectedType && envelope.type !== pending.expectedType) { + pending.reject(new DisplayableError(displayReason("protocol.invalid_message"))); + } else { + pending.resolve(envelope.payload); + } + return; + } + + if (envelope.type === MESSAGE_TYPES.SESSION_ENDED) { + this.sessionId = null; + this.dispatchEvent(new CustomEvent("sessionended", { detail: envelope })); + return; + } + + this.dispatchEvent(new CustomEvent("message", { detail: envelope })); + } +} + +export class DisplayableError extends Error { + constructor(reason) { + super(reason?.text || reason?.code || "Ito request failed"); + this.reason = reason; + } +} diff --git a/client/src/i18n.js b/client/src/i18n.js new file mode 100644 index 0000000..604b528 --- /dev/null +++ b/client/src/i18n.js @@ -0,0 +1,40 @@ +export class TextResources { + constructor(resources = {}) { + this.resources = resources; + } + + static async load(url = "./resources/en/default.json") { + const response = await fetch(url); + if (!response.ok) { + throw new Error(`failed to load text resources: ${response.status}`); + } + return new TextResources(await response.json()); + } + + t(key, values = {}) { + const template = lookup(this.resources, key); + if (typeof template !== "string") return key; + return template.replace(/\{\{(\w+)\}\}/g, (_, name) => String(values[name] ?? "")); + } + + displayReason(reason) { + if (!reason) return this.t("reason.unknown"); + if (typeof reason === "string") return this.t(reason); + if (reason.code) { + const resolved = this.t(reason.code); + if (resolved !== reason.code) return resolved; + } + return reason.text || reason.code || this.t("reason.unknown"); + } + + enumLabel(domain, value) { + return this.t(`enum.${domain}.${value}`); + } +} + +function lookup(resources, key) { + return key.split(".").reduce((node, segment) => { + if (!node || typeof node !== "object") return undefined; + return node[segment]; + }, resources); +} diff --git a/client/src/main.js b/client/src/main.js new file mode 100644 index 0000000..187c3d7 --- /dev/null +++ b/client/src/main.js @@ -0,0 +1,35 @@ +import { ItoPilotApp } from "./app.js"; + +AFRAME.registerComponent("ito-xr-frame-events", { + tick() { + const renderer = this.el.renderer; + const xr = renderer?.xr; + if (!xr?.isPresenting) return; + const frame = xr.getFrame?.(); + const referenceSpace = xr.getReferenceSpace?.(); + if (frame && referenceSpace) { + this.el.emit("xrframe", { frame, referenceSpace }, false); + } + }, +}); + +AFRAME.registerComponent("ito-spark-scene", { + init() { + this.batches = []; + }, + addBatch(batch, entity) { + this.batches.push({ batch, entity }); + }, +}); + +window.addEventListener("DOMContentLoaded", async () => { + const app = new ItoPilotApp({ + scene: document.querySelector("a-scene"), + uiRoot: document.getElementById("ito-ui-root"), + splatRoot: document.getElementById("ito-splat-root"), + launchButton: document.getElementById("enter-vr"), + statusElement: document.getElementById("launch-status"), + }); + await app.init(); + window.itoPilotApp = app; +}); diff --git a/client/src/msgpack.js b/client/src/msgpack.js new file mode 100644 index 0000000..b5fc9da --- /dev/null +++ b/client/src/msgpack.js @@ -0,0 +1,285 @@ +const TEXT_ENCODER = new TextEncoder(); +const TEXT_DECODER = new TextDecoder(); + +export function encodeMessagePack(value) { + const writer = new MessagePackWriter(); + writer.write(value); + return writer.toUint8Array(); +} + +export function decodeMessagePack(bytes) { + const reader = new MessagePackReader(bytes); + const value = reader.read(); + if (reader.offset !== reader.bytes.length) { + throw new Error("invalid MessagePack: trailing bytes"); + } + return value; +} + +class MessagePackWriter { + constructor() { + this.bytes = []; + } + + toUint8Array() { + return new Uint8Array(this.bytes); + } + + write(value) { + if (value === null || value === undefined) { + this.push(0xc0); + } else if (typeof value === "boolean") { + this.push(value ? 0xc3 : 0xc2); + } else if (typeof value === "number") { + this.writeNumber(value); + } else if (typeof value === "string") { + this.writeString(value); + } else if (value instanceof Uint8Array) { + this.writeBinary(value); + } else if (Array.isArray(value)) { + this.writeArray(value); + } else if (typeof value === "object") { + this.writeMap(value); + } else { + throw new Error(`unsupported MessagePack value: ${typeof value}`); + } + } + + writeNumber(value) { + if (Number.isInteger(value) && value >= 0 && value <= 0x7f) { + this.push(value); + } else if (Number.isInteger(value) && value >= -32 && value < 0) { + this.push(0xe0 | (value + 32)); + } else if (Number.isInteger(value) && value >= 0 && value <= 0xff) { + this.push(0xcc, value); + } else if (Number.isInteger(value) && value >= 0 && value <= 0xffff) { + this.push(0xcd, value >> 8, value); + } else if (Number.isInteger(value) && value >= -0x80000000 && value <= 0x7fffffff) { + this.push(0xd2); + this.writeInt32(value); + } else { + const buffer = new ArrayBuffer(9); + const view = new DataView(buffer); + view.setUint8(0, 0xcb); + view.setFloat64(1, value, false); + this.pushBytes(new Uint8Array(buffer)); + } + } + + writeString(value) { + const bytes = TEXT_ENCODER.encode(value); + if (bytes.length <= 31) { + this.push(0xa0 | bytes.length); + } else if (bytes.length <= 0xff) { + this.push(0xd9, bytes.length); + } else if (bytes.length <= 0xffff) { + this.push(0xda, bytes.length >> 8, bytes.length); + } else { + this.push(0xdb); + this.writeUint32(bytes.length); + } + this.pushBytes(bytes); + } + + writeBinary(value) { + if (value.length <= 0xff) { + this.push(0xc4, value.length); + } else if (value.length <= 0xffff) { + this.push(0xc5, value.length >> 8, value.length); + } else { + this.push(0xc6); + this.writeUint32(value.length); + } + this.pushBytes(value); + } + + writeArray(value) { + if (value.length <= 15) { + this.push(0x90 | value.length); + } else if (value.length <= 0xffff) { + this.push(0xdc, value.length >> 8, value.length); + } else { + this.push(0xdd); + this.writeUint32(value.length); + } + for (const item of value) { + this.write(item); + } + } + + writeMap(value) { + const entries = Object.entries(value).filter(([, entryValue]) => entryValue !== undefined); + if (entries.length <= 15) { + this.push(0x80 | entries.length); + } else if (entries.length <= 0xffff) { + this.push(0xde, entries.length >> 8, entries.length); + } else { + this.push(0xdf); + this.writeUint32(entries.length); + } + for (const [key, entryValue] of entries) { + this.writeString(key); + this.write(entryValue); + } + } + + writeInt32(value) { + this.push((value >> 24) & 0xff, (value >> 16) & 0xff, (value >> 8) & 0xff, value & 0xff); + } + + writeUint32(value) { + this.push((value >>> 24) & 0xff, (value >>> 16) & 0xff, (value >>> 8) & 0xff, value & 0xff); + } + + push(...values) { + for (const value of values) { + this.bytes.push(value & 0xff); + } + } + + pushBytes(values) { + for (const value of values) { + this.bytes.push(value); + } + } +} + +class MessagePackReader { + constructor(bytes) { + this.bytes = bytes instanceof Uint8Array ? bytes : new Uint8Array(bytes); + this.offset = 0; + } + + read() { + const marker = this.readByte(); + if (marker <= 0x7f) return marker; + if (marker >= 0xe0) return marker - 0x100; + if ((marker & 0xe0) === 0xa0) return this.readString(marker & 0x1f); + if ((marker & 0xf0) === 0x90) return this.readArray(marker & 0x0f); + if ((marker & 0xf0) === 0x80) return this.readMap(marker & 0x0f); + + switch (marker) { + case 0xc0: + return null; + case 0xc2: + return false; + case 0xc3: + return true; + case 0xc4: + return this.readBinary(this.readByte()); + case 0xc5: + return this.readBinary(this.readUint16()); + case 0xc6: + return this.readBinary(this.readUint32()); + case 0xca: + return this.readFloat32(); + case 0xcb: + return this.readFloat64(); + case 0xcc: + return this.readByte(); + case 0xcd: + return this.readUint16(); + case 0xce: + return this.readUint32(); + case 0xd0: + return this.readInt8(); + case 0xd1: + return this.readInt16(); + case 0xd2: + return this.readInt32(); + case 0xd9: + return this.readString(this.readByte()); + case 0xda: + return this.readString(this.readUint16()); + case 0xdb: + return this.readString(this.readUint32()); + case 0xdc: + return this.readArray(this.readUint16()); + case 0xdd: + return this.readArray(this.readUint32()); + case 0xde: + return this.readMap(this.readUint16()); + case 0xdf: + return this.readMap(this.readUint32()); + default: + throw new Error(`unsupported MessagePack marker: 0x${marker.toString(16)}`); + } + } + + readString(length) { + return TEXT_DECODER.decode(this.readBytes(length)); + } + + readBinary(length) { + return this.readBytes(length); + } + + readArray(length) { + const value = []; + for (let index = 0; index < length; index += 1) { + value.push(this.read()); + } + return value; + } + + readMap(length) { + const value = {}; + for (let index = 0; index < length; index += 1) { + const key = this.read(); + value[key] = this.read(); + } + return value; + } + + readByte() { + if (this.offset >= this.bytes.length) { + throw new Error("invalid MessagePack: unexpected end of input"); + } + return this.bytes[this.offset++]; + } + + readBytes(length) { + if (this.offset + length > this.bytes.length) { + throw new Error("invalid MessagePack: unexpected end of input"); + } + const slice = this.bytes.slice(this.offset, this.offset + length); + this.offset += length; + return slice; + } + + readUint16() { + return (this.readByte() << 8) | this.readByte(); + } + + readUint32() { + return ( + (this.readByte() * 0x1000000) + + ((this.readByte() << 16) | (this.readByte() << 8) | this.readByte()) + ); + } + + readInt8() { + const value = this.readByte(); + return value & 0x80 ? value - 0x100 : value; + } + + readInt16() { + const value = this.readUint16(); + return value & 0x8000 ? value - 0x10000 : value; + } + + readInt32() { + const value = this.readUint32(); + return value > 0x7fffffff ? value - 0x100000000 : value; + } + + readFloat32() { + const bytes = this.readBytes(4); + return new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength).getFloat32(0, false); + } + + readFloat64() { + const bytes = this.readBytes(8); + return new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength).getFloat64(0, false); + } +} diff --git a/client/src/pilot-input.js b/client/src/pilot-input.js new file mode 100644 index 0000000..c7dd114 --- /dev/null +++ b/client/src/pilot-input.js @@ -0,0 +1,110 @@ +const TEXT_ENCODER = new TextEncoder(); + +export class PilotInputLoop { + constructor({ transport, rateHz = 60, now = () => performance.now() }) { + this.transport = transport; + this.rateHz = rateHz; + this.now = now; + this.enabled = false; + this.sequence = 0; + this.zeroYawRad = null; + this.lastSentAt = 0; + } + + start() { + this.enabled = true; + this.sequence = 0; + this.zeroYawRad = null; + this.lastSentAt = 0; + } + + stop() { + this.enabled = false; + } + + maybeSend(frame, referenceSpace, sessionId) { + if (!this.enabled || !this.transport?.canSend()) return null; + const now = this.now(); + const intervalMs = 1000 / this.rateHz; + if (now - this.lastSentAt < intervalMs) return null; + const pose = frame.getViewerPose(referenceSpace); + if (!pose) return null; + const snapshot = this.createSnapshot(pose, frame.session.inputSources, sessionId, now); + this.transport.sendSnapshot(snapshot); + this.lastSentAt = now; + return snapshot; + } + + createSnapshot(viewerPose, inputSources, sessionId, timestampMs = this.now()) { + const absoluteYaw = yawFromViewerPose(viewerPose); + if (this.zeroYawRad === null) this.zeroYawRad = absoluteYaw; + const headsetYawRad = normalizeRadians(absoluteYaw - this.zeroYawRad); + return { + protocolVersion: "ito.v1", + sessionId, + sequence: ++this.sequence, + timestampMs: Math.round(timestampMs), + headsetYawRad, + controllers: Array.from(inputSources || []).map(controllerSnapshot), + }; + } +} + +export class DataChannelPilotInputTransport { + constructor(dataChannel = null) { + this.dataChannel = dataChannel; + } + + attach(dataChannel) { + this.dataChannel = dataChannel; + } + + canSend() { + return this.dataChannel?.readyState === "open"; + } + + sendSnapshot(snapshot) { + this.dataChannel.send(TEXT_ENCODER.encode(JSON.stringify(snapshot))); + } +} + +export function yawFromViewerPose(viewerPose) { + const orientation = viewerPose.transform?.orientation; + if (orientation) { + return yawFromQuaternion(orientation.x, orientation.y, orientation.z, orientation.w); + } + const matrix = viewerPose.transform?.matrix; + if (matrix) { + return Math.atan2(-matrix[8], matrix[10]); + } + return 0; +} + +function yawFromQuaternion(x, y, z, w) { + const sinyCosp = 2 * (w * y + z * x); + const cosyCosp = 1 - 2 * (y * y + x * x); + return Math.atan2(sinyCosp, cosyCosp); +} + +function controllerSnapshot(inputSource) { + const gamepad = inputSource.gamepad; + return { + handedness: inputSource.handedness || "none", + targetRayMode: inputSource.targetRayMode || "unknown", + buttons: gamepad + ? Array.from(gamepad.buttons || []).map((button) => ({ + pressed: Boolean(button.pressed), + touched: Boolean(button.touched), + value: Number(button.value || 0), + })) + : [], + axes: gamepad ? Array.from(gamepad.axes || []) : [], + }; +} + +function normalizeRadians(value) { + let normalized = value; + while (normalized > Math.PI) normalized -= Math.PI * 2; + while (normalized < -Math.PI) normalized += Math.PI * 2; + return normalized; +} diff --git a/client/src/protocol.js b/client/src/protocol.js new file mode 100644 index 0000000..a7dc193 --- /dev/null +++ b/client/src/protocol.js @@ -0,0 +1,93 @@ +import { decodeMessagePack, encodeMessagePack } from "./msgpack.js"; + +export const PROTOCOL_VERSION = "ito.v1"; + +export const MESSAGE_TYPES = Object.freeze({ + CATALOG_GET: "catalog.get", + CATALOG_GET_RESULT: "catalog.get.result", + CONNECTION_HELLO: "connection.hello", + CONNECTION_HELLO_RESULT: "connection.hello.result", + SESSION_ACQUIRE: "session.acquire", + SESSION_ACQUIRE_RESULT: "session.acquire.result", + SESSION_END: "session.end", + SESSION_END_RESULT: "session.end.result", + SESSION_ENDED: "session.ended", + WEBRTC_OFFER: "webrtc.offer", + WEBRTC_ANSWER: "webrtc.answer", +}); + +export const ROLE_PILOT_CLIENT = "pilotClient"; +export const ROBOT_STATUS_AVAILABLE = "Available"; +export const ROBOT_STATUS_OCCUPIED = "Occupied"; +export const ROBOT_STATUS_UNAVAILABLE = "Unavailable"; + +export function makeMessageId() { + if (globalThis.crypto?.randomUUID) { + return globalThis.crypto.randomUUID(); + } + return `client-${Date.now().toString(36)}-${Math.random().toString(36).slice(2)}`; +} + +export function makeEnvelope(type, payload = {}, options = {}) { + const envelope = { + protocolVersion: PROTOCOL_VERSION, + messageId: options.messageId || makeMessageId(), + type, + payload, + }; + if (options.replyToMessageId) envelope.replyToMessageId = options.replyToMessageId; + if (options.robotId) envelope.robotId = options.robotId; + if (options.sessionId) envelope.sessionId = options.sessionId; + validateEnvelope(envelope); + return envelope; +} + +export function packEnvelope(envelope) { + validateEnvelope(envelope); + return encodeMessagePack(envelope); +} + +export function unpackEnvelope(frame) { + const envelope = decodeMessagePack(frame instanceof ArrayBuffer ? new Uint8Array(frame) : frame); + validateEnvelope(envelope); + return envelope; +} + +export function validateEnvelope(envelope) { + if (!envelope || typeof envelope !== "object" || Array.isArray(envelope)) { + throw new Error("Ito envelope must be a map"); + } + if (envelope.protocolVersion !== PROTOCOL_VERSION) { + throw new Error(`unsupported Ito protocol version: ${envelope.protocolVersion}`); + } + if (typeof envelope.messageId !== "string" || envelope.messageId.length === 0) { + throw new Error("Ito envelope requires messageId"); + } + if (!Object.values(MESSAGE_TYPES).includes(envelope.type)) { + throw new Error(`unknown Ito message type: ${envelope.type}`); + } + if (!envelope.payload || typeof envelope.payload !== "object" || Array.isArray(envelope.payload)) { + throw new Error("Ito envelope requires payload map"); + } + if (envelope.replyToMessageId !== undefined && typeof envelope.replyToMessageId !== "string") { + throw new Error("replyToMessageId must be a string"); + } + if (envelope.robotId !== undefined && typeof envelope.robotId !== "string") { + throw new Error("robotId must be a string"); + } + if (envelope.sessionId !== undefined && typeof envelope.sessionId !== "string") { + throw new Error("sessionId must be a string"); + } +} + +export function resultReason(payload) { + if (!payload || payload.ok !== false) return null; + return payload.reason || { code: "protocol.invalid_message" }; +} + +export function displayReason(code, text) { + const reason = {}; + if (code) reason.code = code; + if (text) reason.text = text; + return reason; +} diff --git a/client/src/splat-scene.js b/client/src/splat-scene.js new file mode 100644 index 0000000..233e370 --- /dev/null +++ b/client/src/splat-scene.js @@ -0,0 +1,152 @@ +export class SplatSceneOwner { + constructor({ adapter = new NullSplatAdapter(), budget = 180, lifetimeMs = 30000, now = () => performance.now() } = {}) { + this.adapter = adapter; + this.budget = budget; + this.lifetimeMs = lifetimeMs; + this.now = now; + this.batches = []; + this.nextBatchId = 1; + this.frozen = false; + this.lastAppliedAt = null; + } + + setLimits({ budget = this.budget, lifetimeMs = this.lifetimeMs } = {}) { + this.budget = budget; + this.lifetimeMs = lifetimeMs; + this.evict(); + } + + applySplatBatch(payload, metadata = {}) { + if (this.frozen) return null; + const batch = { + id: metadata.id || `splat-batch-${this.nextBatchId++}`, + payload, + splatCount: Number.isFinite(metadata.splatCount) ? metadata.splatCount : estimateSplatCount(payload), + receivedAt: this.now(), + }; + this.adapter.addBatch(batch); + this.batches.push(batch); + this.lastAppliedAt = batch.receivedAt; + this.evict(); + return batch; + } + + evict() { + const cutoff = this.now() - this.lifetimeMs; + for (const batch of [...this.batches]) { + if (batch.receivedAt < cutoff) { + this.removeBatch(batch); + } + } + while (this.totalSplatCount() > this.budget && this.batches.length > 0) { + this.removeBatch(this.batches[0]); + } + } + + setFrozen(frozen) { + this.frozen = Boolean(frozen); + this.adapter.setFrozen(this.frozen); + } + + clear() { + for (const batch of [...this.batches]) { + this.removeBatch(batch); + } + this.lastAppliedAt = null; + } + + totalSplatCount() { + return this.batches.reduce((total, batch) => total + batch.splatCount, 0); + } + + removeBatch(batch) { + this.batches = this.batches.filter((candidate) => candidate !== batch); + this.adapter.removeBatch(batch); + } +} + +export class NullSplatAdapter { + constructor() { + this.added = []; + this.removed = []; + this.frozen = false; + } + + addBatch(batch) { + this.added.push(batch); + } + + removeBatch(batch) { + this.removed.push(batch.id); + } + + setFrozen(frozen) { + this.frozen = frozen; + } +} + +export class SparkJsSplatAdapter { + constructor(rootEntity) { + this.rootEntity = rootEntity; + this.batchEntities = new Map(); + } + + addBatch(batch) { + const entity = document.createElement("a-entity"); + entity.setAttribute("data-splat-batch-id", batch.id); + entity.itoSplatBatch = batch; + this.rootEntity.appendChild(entity); + this.batchEntities.set(batch.id, entity); + + if (this.rootEntity.components?.["ito-spark-scene"]?.addBatch) { + this.rootEntity.components["ito-spark-scene"].addBatch(batch, entity); + } + } + + removeBatch(batch) { + const entity = this.batchEntities.get(batch.id); + if (entity?.parentNode) entity.parentNode.removeChild(entity); + this.batchEntities.delete(batch.id); + } + + setFrozen(frozen) { + this.rootEntity.setAttribute("data-visual-frozen", frozen ? "true" : "false"); + this.rootEntity.setAttribute("visible", true); + } +} + +export class DataChannelSplatBatchReceiver { + constructor(sceneOwner) { + this.sceneOwner = sceneOwner; + } + + attach(dataChannel) { + dataChannel.binaryType = "arraybuffer"; + dataChannel.addEventListener("message", (event) => { + const payload = event.data instanceof ArrayBuffer ? event.data : event.data?.buffer; + if (payload) this.sceneOwner.applySplatBatch(payload, parseSplatBatchHeader(payload)); + }); + } +} + +export function parseSplatBatchHeader(payload) { + const view = + payload instanceof ArrayBuffer + ? new DataView(payload) + : new DataView(payload.buffer, payload.byteOffset, payload.byteLength); + const magic = String.fromCharCode(...new Uint8Array(view.buffer, view.byteOffset, 8)); + if (magic !== "ITOSPLAT") throw new Error("invalid Ito Splat Batch"); + const version = view.getUint16(8, true); + if (version !== 1) throw new Error(`unsupported Ito Splat Batch version: ${version}`); + return { + flags: view.getUint16(10, true), + id: `splat-batch-${view.getUint32(12, true)}`, + splatCount: view.getUint32(16, true), + recordStride: view.getUint16(20, true), + }; +} + +function estimateSplatCount(payload) { + if (payload?.byteLength) return Math.max(1, Math.floor(payload.byteLength / 32)); + return 1; +} diff --git a/client/src/visual-freshness.js b/client/src/visual-freshness.js new file mode 100644 index 0000000..adfdee2 --- /dev/null +++ b/client/src/visual-freshness.js @@ -0,0 +1,31 @@ +export class VisualFreshnessMonitor extends EventTarget { + constructor({ timeoutMs = 2000, now = () => performance.now() } = {}) { + super(); + this.timeoutMs = timeoutMs; + this.now = now; + this.lastFreshAt = null; + this.stale = false; + } + + markFresh() { + this.lastFreshAt = this.now(); + if (this.stale) { + this.stale = false; + this.dispatchEvent(new Event("fresh")); + } + } + + reset() { + this.lastFreshAt = null; + this.stale = false; + } + + tick() { + if (this.lastFreshAt === null || this.stale) return this.stale; + if (this.now() - this.lastFreshAt >= this.timeoutMs) { + this.stale = true; + this.dispatchEvent(new Event("stale")); + } + return this.stale; + } +} diff --git a/client/src/vr-ui.js b/client/src/vr-ui.js new file mode 100644 index 0000000..975cfe3 --- /dev/null +++ b/client/src/vr-ui.js @@ -0,0 +1,92 @@ +export class VrUi { + constructor(root, textResources) { + this.root = root; + this.text = textResources; + } + + clear() { + this.root.replaceChildren(); + } + + panel({ title, subtitle = "", width = 3.2, height = 2.2, position = "0 1.55 -2.4" }) { + this.clear(); + const panel = document.createElement("a-entity"); + panel.setAttribute("position", position); + panel.setAttribute("data-ito-panel", "true"); + this.root.appendChild(panel); + + const back = plane({ width, height, color: "#101820", opacity: 0.92 }); + back.setAttribute("position", `0 0 ${-0.01}`); + panel.appendChild(back); + + panel.appendChild(textEntity(title, { x: -width / 2 + 0.18, y: height / 2 - 0.24, z: 0.01 }, 0.16, "#f7fbff")); + if (subtitle) { + panel.appendChild(textEntity(subtitle, { x: -width / 2 + 0.18, y: height / 2 - 0.48, z: 0.01 }, 0.085, "#b8c7d9")); + } + return panel; + } + + button(parent, { label, position, width = 0.82, height = 0.24, enabled = true, action, detail = null }) { + const button = document.createElement("a-entity"); + button.setAttribute("position", position); + button.classList.toggle("ito-clickable", enabled); + button.setAttribute("data-action", action || ""); + button.itoActionDetail = detail; + + const background = plane({ + width, + height, + color: enabled ? "#2f8f83" : "#3a4652", + opacity: 0.96, + }); + button.appendChild(background); + button.appendChild(textEntity(label, { x: 0, y: -0.032, z: 0.015 }, 0.075, "#ffffff", "center", width - 0.08)); + parent.appendChild(button); + return button; + } + + label(parent, label, position, options = {}) { + const entity = textEntity( + label, + toPosition(position), + options.size || 0.075, + options.color || "#dce8f3", + options.align || "left", + options.width || 2.6, + ); + parent.appendChild(entity); + return entity; + } +} + +export function plane({ width, height, color, opacity = 1 }) { + const entity = document.createElement("a-plane"); + entity.setAttribute("width", width); + entity.setAttribute("height", height); + entity.setAttribute("color", color); + entity.setAttribute("opacity", opacity); + entity.setAttribute("shader", "flat"); + return entity; +} + +export function textEntity(value, position, size, color, align = "left", width = 2.6) { + const entity = document.createElement("a-text"); + entity.setAttribute("value", value); + entity.setAttribute("position", `${position.x} ${position.y} ${position.z}`); + entity.setAttribute("align", align); + entity.setAttribute("anchor", align); + entity.setAttribute("baseline", "top"); + entity.setAttribute("width", width); + entity.setAttribute("wrap-count", Math.max(12, Math.floor(width / size) * 7)); + entity.setAttribute("color", color); + entity.setAttribute("shader", "msdf"); + return entity; +} + +function toPosition(position) { + if (typeof position === "string") { + const [x, y, z] = position.split(/\s+/).map(Number); + return { x, y, z }; + } + return position; +} diff --git a/client/src/webrtc.js b/client/src/webrtc.js new file mode 100644 index 0000000..d62b0d2 --- /dev/null +++ b/client/src/webrtc.js @@ -0,0 +1,92 @@ +import { MESSAGE_TYPES, makeEnvelope } from "./protocol.js"; + +export const LIVE_PATHS = Object.freeze({ + PILOT_INPUT: "pilotInput", + CAMERA_MEDIA: "cameraMedia", + SPLAT_BATCHES: "splatBatches", +}); + +export async function waitForIceGatheringComplete(peerConnection) { + if (peerConnection.iceGatheringState === "complete") return; + await new Promise((resolve) => { + peerConnection.addEventListener( + "icegatheringstatechange", + () => { + if (peerConnection.iceGatheringState === "complete") resolve(); + }, + { once: false }, + ); + }); +} + +export async function createNonTrickleOffer(peerConnection) { + const offer = await peerConnection.createOffer(); + await peerConnection.setLocalDescription(offer); + await waitForIceGatheringComplete(peerConnection); + return peerConnection.localDescription.sdp; +} + +export async function applyNonTrickleAnswer(peerConnection, sdp) { + await peerConnection.setRemoteDescription({ type: "answer", sdp }); +} + +export class PilotInputPeer { + constructor({ controlClient, sessionId, robotId, dataChannelProfile = {}, RTCPeerConnectionImpl = globalThis.RTCPeerConnection }) { + this.controlClient = controlClient; + this.sessionId = sessionId; + this.robotId = robotId; + this.peerConnection = new RTCPeerConnectionImpl({ iceServers: [] }); + this.dataChannel = this.peerConnection.createDataChannel("ito.pilotInput", dataChannelProfile); + } + + async negotiate() { + const sdp = await createNonTrickleOffer(this.peerConnection); + const result = await this.controlClient.request( + MESSAGE_TYPES.WEBRTC_OFFER, + { path: LIVE_PATHS.PILOT_INPUT, sdp }, + MESSAGE_TYPES.WEBRTC_ANSWER, + { robotId: this.robotId, sessionId: this.sessionId }, + ); + await applyNonTrickleAnswer(this.peerConnection, result.sdp); + return this.dataChannel; + } + + close() { + this.dataChannel?.close(); + this.peerConnection?.close(); + } +} + +export class SplatBatchPeer extends EventTarget { + constructor({ controlClient, sessionId, dataChannelProfile = {}, RTCPeerConnectionImpl = globalThis.RTCPeerConnection }) { + super(); + this.controlClient = controlClient; + this.sessionId = sessionId; + this.peerConnection = new RTCPeerConnectionImpl({ iceServers: [] }); + this.dataChannelProfile = dataChannelProfile; + } + + async negotiate() { + this.peerConnection.addEventListener("datachannel", (event) => this.attachDataChannel(event.channel)); + const sdp = await createNonTrickleOffer(this.peerConnection); + const result = await this.controlClient.request( + MESSAGE_TYPES.WEBRTC_OFFER, + { path: LIVE_PATHS.SPLAT_BATCHES, sdp }, + MESSAGE_TYPES.WEBRTC_ANSWER, + { sessionId: this.sessionId }, + ); + await applyNonTrickleAnswer(this.peerConnection, result.sdp); + } + + attachDataChannel(dataChannel) { + dataChannel.binaryType = "arraybuffer"; + dataChannel.addEventListener("message", (event) => { + const payload = event.data instanceof ArrayBuffer ? event.data : event.data?.buffer; + if (payload) this.dispatchEvent(new CustomEvent("splatbatch", { detail: payload })); + }); + } + + close() { + this.peerConnection?.close(); + } +} diff --git a/client/styles.css b/client/styles.css new file mode 100644 index 0000000..a23515f --- /dev/null +++ b/client/styles.css @@ -0,0 +1,53 @@ +html, +body { + margin: 0; + min-height: 100%; + background: #071017; + color: #f7fbff; + font-family: + Inter, ui-sans-serif, system-ui, -apple-system, BlinkMacSystemFont, "Segoe UI", sans-serif; +} + +#launch { + position: fixed; + inset: 0; + z-index: 2; + display: grid; + place-content: center; + gap: 16px; + text-align: center; + background: #071017; +} + +#launch h1 { + margin: 0; + font-size: 32px; + font-weight: 650; + letter-spacing: 0; +} + +#enter-vr { + min-width: 160px; + min-height: 48px; + border: 0; + border-radius: 8px; + background: #2f8f83; + color: #ffffff; + font: inherit; + font-weight: 650; +} + +#enter-vr:disabled { + background: #3a4652; + color: #aeb8c3; +} + +#launch-status { + min-height: 24px; + margin: 0; + color: #b8c7d9; +} + +.a-enter-vr { + display: none; +} diff --git a/client/tests/config.test.js b/client/tests/config.test.js new file mode 100644 index 0000000..e75fd94 --- /dev/null +++ b/client/tests/config.test.js @@ -0,0 +1,44 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { ClientSettingsStore, DEFAULT_SETTINGS, mergeSessionConfig, normalizeSettings } from "../src/config.js"; + +class MemoryStorage { + constructor() { + this.values = new Map(); + } + getItem(key) { + return this.values.get(key) || null; + } + setItem(key, value) { + this.values.set(key, value); + } + removeItem(key) { + this.values.delete(key); + } +} + +test("settings fall back to sane defaults", () => { + const store = new ClientSettingsStore(new MemoryStorage()); + + assert.equal(store.load().visualFreshnessTimeoutMs, 2000); + assert.equal(store.load().pilotInputRateHz, 60); +}); + +test("settings persist through local storage and clamp unsafe values", () => { + const store = new ClientSettingsStore(new MemoryStorage()); + const saved = store.save({ ...DEFAULT_SETTINGS, pilotInputRateHz: 1000, splatBudget: -5 }); + + assert.equal(saved.pilotInputRateHz, 120); + assert.equal(saved.splatBudget, 1); + assert.deepEqual(store.load(), saved); +}); + +test("session config merges server data channel profiles with local client settings", () => { + const settings = normalizeSettings({ ...DEFAULT_SETTINGS, pilotInputRateHz: 30, splatBudget: 25 }); + const merged = mergeSessionConfig(settings, { pilotInputDataChannel: { ordered: false } }); + + assert.equal(merged.pilotInputRateHz, 30); + assert.equal(merged.splatBudget, 25); + assert.deepEqual(merged.pilotInputDataChannel, { ordered: false }); +}); diff --git a/client/tests/i18n.test.js b/client/tests/i18n.test.js new file mode 100644 index 0000000..6d23c69 --- /dev/null +++ b/client/tests/i18n.test.js @@ -0,0 +1,17 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { TextResources } from "../src/i18n.js"; + +test("text resources resolve nested keys and template values", () => { + const text = new TextResources({ session: { active: "Piloting {{name}}" } }); + + assert.equal(text.t("session.active", { name: "Dory" }), "Piloting Dory"); +}); + +test("display reasons prefer localized resource keys and fall back to free text", () => { + const text = new TextResources({ reason: { request: { timeout: "Timed out" } } }); + + assert.equal(text.displayReason({ code: "reason.request.timeout", text: "Fallback" }), "Timed out"); + assert.equal(text.displayReason({ code: "reason.missing", text: "Fallback" }), "Fallback"); +}); diff --git a/client/tests/pilot-input.test.js b/client/tests/pilot-input.test.js new file mode 100644 index 0000000..b18e0a2 --- /dev/null +++ b/client/tests/pilot-input.test.js @@ -0,0 +1,57 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { PilotInputLoop } from "../src/pilot-input.js"; + +test("pilot input snapshots use headset yaw relative to session start", () => { + let yaw = Math.PI / 4; + const loop = new PilotInputLoop({ transport: null, now: () => 0 }); + const first = loop.createSnapshot(poseWithYaw(yaw), [], "session-1", 0); + + yaw = Math.PI / 2; + const second = loop.createSnapshot(poseWithYaw(yaw), [], "session-1", 16); + + assert.equal(first.headsetYawRad, 0); + assert.ok(Math.abs(second.headsetYawRad - Math.PI / 4) < 0.000001); + assert.equal(second.sequence, 2); +}); + +test("pilot input snapshots include controller full state", () => { + const loop = new PilotInputLoop({ transport: null, now: () => 0 }); + const snapshot = loop.createSnapshot( + poseWithYaw(0), + [ + { + handedness: "right", + targetRayMode: "tracked-pointer", + gamepad: { + buttons: [{ pressed: true, touched: true, value: 1 }], + axes: [0.1, -0.2], + }, + }, + ], + "session-1", + 0, + ); + + assert.deepEqual(snapshot.controllers[0], { + handedness: "right", + targetRayMode: "tracked-pointer", + buttons: [{ pressed: true, touched: true, value: 1 }], + axes: [0.1, -0.2], + }); +}); + +function poseWithYaw(yaw) { + const half = yaw / 2; + return { + transform: { + orientation: { + x: 0, + y: Math.sin(half), + z: 0, + w: Math.cos(half), + }, + }, + }; +} diff --git a/client/tests/protocol.test.js b/client/tests/protocol.test.js new file mode 100644 index 0000000..7f84d41 --- /dev/null +++ b/client/tests/protocol.test.js @@ -0,0 +1,27 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { decodeMessagePack, encodeMessagePack } from "../src/msgpack.js"; +import { MESSAGE_TYPES, makeEnvelope, packEnvelope, unpackEnvelope } from "../src/protocol.js"; + +test("MessagePack codec round-trips Ito envelope values", () => { + const value = { + protocolVersion: "ito.v1", + messageId: "message-1", + type: "catalog.get.result", + payload: { + ok: true, + value: { + robots: [{ robotId: "droid-1", status: "Available", unavailable: false, score: 1.5 }], + }, + }, + }; + + assert.deepEqual(decodeMessagePack(encodeMessagePack(value)), value); +}); + +test("Ito envelopes pack and unpack as MessagePack", () => { + const envelope = makeEnvelope(MESSAGE_TYPES.CATALOG_GET, { includeUnavailable: true }, { messageId: "cat-1" }); + + assert.deepEqual(unpackEnvelope(packEnvelope(envelope)), envelope); +}); diff --git a/client/tests/splat-batch.test.js b/client/tests/splat-batch.test.js new file mode 100644 index 0000000..cb561a3 --- /dev/null +++ b/client/tests/splat-batch.test.js @@ -0,0 +1,23 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { parseSplatBatchHeader } from "../src/splat-scene.js"; + +test("parseSplatBatchHeader reads Ito v1 binary header", () => { + const payload = new ArrayBuffer(28); + const bytes = new Uint8Array(payload); + bytes.set([73, 84, 79, 83, 80, 76, 65, 84]); // ITOSPLAT + const view = new DataView(payload); + view.setUint16(8, 1, true); + view.setUint16(10, 3, true); + view.setUint32(12, 9, true); + view.setUint32(16, 2, true); + view.setUint16(20, 36, true); + + assert.deepEqual(parseSplatBatchHeader(payload), { + flags: 3, + id: "splat-batch-9", + splatCount: 2, + recordStride: 36, + }); +}); diff --git a/client/tests/splat-scene.test.js b/client/tests/splat-scene.test.js new file mode 100644 index 0000000..aa67211 --- /dev/null +++ b/client/tests/splat-scene.test.js @@ -0,0 +1,39 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { NullSplatAdapter, SplatSceneOwner } from "../src/splat-scene.js"; + +test("splat scene evicts oldest batches over budget", () => { + let time = 0; + const adapter = new NullSplatAdapter(); + const scene = new SplatSceneOwner({ adapter, budget: 2, lifetimeMs: 1000, now: () => time }); + + scene.applySplatBatch(new Uint8Array(32), { splatCount: 1 }); + time += 1; + scene.applySplatBatch(new Uint8Array(32), { splatCount: 1 }); + time += 1; + scene.applySplatBatch(new Uint8Array(32), { splatCount: 1 }); + + assert.equal(scene.batches.length, 2); + assert.deepEqual(adapter.removed, ["splat-batch-1"]); +}); + +test("splat scene evicts batches past lifetime", () => { + let time = 0; + const scene = new SplatSceneOwner({ budget: 10, lifetimeMs: 10, now: () => time }); + + scene.applySplatBatch(new Uint8Array(32), { splatCount: 1 }); + time = 11; + scene.evict(); + + assert.equal(scene.batches.length, 0); +}); + +test("frozen splat scene does not apply new batches", () => { + const scene = new SplatSceneOwner(); + + scene.setFrozen(true); + + assert.equal(scene.applySplatBatch(new Uint8Array(32)), null); + assert.equal(scene.batches.length, 0); +}); diff --git a/client/tests/visual-freshness.test.js b/client/tests/visual-freshness.test.js new file mode 100644 index 0000000..de257d3 --- /dev/null +++ b/client/tests/visual-freshness.test.js @@ -0,0 +1,26 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { VisualFreshnessMonitor } from "../src/visual-freshness.js"; + +test("visual freshness becomes stale after timeout and fresh again on new batch", () => { + let time = 0; + const monitor = new VisualFreshnessMonitor({ timeoutMs: 10, now: () => time }); + let staleEvents = 0; + let freshEvents = 0; + monitor.addEventListener("stale", () => { + staleEvents += 1; + }); + monitor.addEventListener("fresh", () => { + freshEvents += 1; + }); + + monitor.markFresh(); + time = 11; + assert.equal(monitor.tick(), true); + monitor.markFresh(); + + assert.equal(staleEvents, 1); + assert.equal(freshEvents, 1); + assert.equal(monitor.stale, false); +}); diff --git a/client/tests/webrtc.test.js b/client/tests/webrtc.test.js new file mode 100644 index 0000000..4845065 --- /dev/null +++ b/client/tests/webrtc.test.js @@ -0,0 +1,52 @@ +import assert from "node:assert/strict"; +import test from "node:test"; + +import { SplatBatchPeer } from "../src/webrtc.js"; + +class FakePeerConnection extends EventTarget { + constructor() { + super(); + this.iceGatheringState = "complete"; + this.localDescription = null; + this.remoteDescription = null; + } + + async createOffer() { + return { type: "offer", sdp: "local offer" }; + } + + async setLocalDescription(description) { + this.localDescription = description; + } + + async setRemoteDescription(description) { + this.remoteDescription = description; + } + + close() { + this.closed = true; + } +} + +test("SplatBatchPeer negotiates non-trickle offer over control client", async () => { + const requests = []; + const controlClient = { + request(type, payload, expectedType, options) { + requests.push({ type, payload, expectedType, options }); + return { sdp: "server answer" }; + }, + }; + const peer = new SplatBatchPeer({ + controlClient, + sessionId: "session-1", + RTCPeerConnectionImpl: FakePeerConnection, + }); + + await peer.negotiate(); + + assert.equal(requests[0].type, "webrtc.offer"); + assert.equal(requests[0].payload.path, "splatBatches"); + assert.equal(requests[0].payload.sdp, "local offer"); + assert.equal(requests[0].expectedType, "webrtc.answer"); + assert.deepEqual(peer.peerConnection.remoteDescription, { type: "answer", sdp: "server answer" }); +}); diff --git a/compose.yaml b/compose.yaml new file mode 100644 index 0000000..18a1852 --- /dev/null +++ b/compose.yaml @@ -0,0 +1,57 @@ +services: + ito-server: + build: + context: . + dockerfile: server/Dockerfile + environment: + ITO_SERVER_HOST: 0.0.0.0 + ITO_SERVER_PORT: "8765" + ITO_REQUEST_TIMEOUT_MS: "5000" + ITO_DRIVER_STATUS_WATCHDOG_MS: "2000" + ITO_SESSION_CLEANUP_TIMEOUT_MS: "30000" + ITO_PILOT_INPUT_ORDERED: "false" + ITO_PILOT_INPUT_MAX_RETRANSMITS: "0" + ITO_SPLAT_BATCH_ORDERED: "true" + ports: + - "8765:8765" + + pilot-client: + image: nginx:1.27-alpine + depends_on: + - ito-server + volumes: + - ./client:/usr/share/nginx/html:ro + ports: + - "8080:80" + + mock-robot: + profiles: + - mock + build: + context: . + dockerfile: drivers/mock-robot/Dockerfile + depends_on: + - ito-server + environment: + ITO_SERVER_URL: ws://ito-server:8765 + ITO_MOCK_ROBOT_ID: mock-robot-1 + ITO_MOCK_ROBOT_NAME: Mock Robot + ITO_MOCK_ROBOT_STATUS_INTERVAL_MS: "1000" + ITO_MOCK_ROBOT_CAMERA_VIDEO: /data/mock-camera.h264 + ITO_MOCK_ROBOT_CAMERA_LOOP: "true" + volumes: + - ${ITO_MOCK_ROBOT_CAMERA_VIDEO_HOST:-./fixtures/mock-camera.h264}:/data/mock-camera.h264:ro + + ito-droid: + profiles: + - droid + build: + context: . + dockerfile: drivers/ito-droid/Dockerfile + network_mode: host + environment: + ITO_SERVER_URL: ${ITO_SERVER_URL:-ws://127.0.0.1:8765} + ITO_DROID_ROBOT_ID: ${ITO_DROID_ROBOT_ID:-ito-droid-1} + ITO_DROID_NAME: ${ITO_DROID_NAME:-Ito Droid} + ITO_DROID_ROS_CAMERA_TOPIC: ${ITO_DROID_ROS_CAMERA_TOPIC:-/image_raw} + ITO_DROID_ROS_SERVO_COMMAND_TOPIC: ${ITO_DROID_ROS_SERVO_COMMAND_TOPIC:-/ito_droid/camera_pan/command} diff --git a/docs/acceptance-v1.md b/docs/acceptance-v1.md new file mode 100644 index 0000000..a4bad32 --- /dev/null +++ b/docs/acceptance-v1.md @@ -0,0 +1,76 @@ +# v1 Acceptance Pass + +This file records the current v1 acceptance pass against the core outcome in +`docs/v1.md`. Hardware-only acceptance remains unchecked until it is performed +with a physical Ito Droid and Pico 4 browser. + +## Latest Local Run + +Run on July 9, 2026: + +- [x] `pytest -q`: 40 passed, 1 skipped. The skipped test is the Mock Robot + `aiortc` WebRTC e2e test because this local Python environment does not have + `aiortc` installed. +- [x] `npm test` from `client/`: 15 passed. +- [ ] `docker compose config`: not run because Docker is not installed in this + environment. +- [ ] `python -m pip install -r server/requirements.txt -r drivers/mock-robot/requirements.txt`: + attempted, but PyAV built from source and failed because FFmpeg development + libraries were unavailable through `pkg-config`. + +## Local Acceptance + +- [x] Ito Protocol control-plane envelope tests pass for MessagePack binary + WebSocket messages, exact `ito.v1` protocol-version validation, Display + Reason helpers, and standard result payloads. +- [x] Ito Server unit tests cover pilot hello, catalog responses, driver status, + duplicate `robotId` handling, serialized acquisition, session allocation, + session end fan-out, disappeared-endpoint cleanup, and pilot reconnect resume + or rejection. +- [x] Mock Robot unit tests cover status reporting, video-file-backed camera + opening/closing, session start/end handling, session-start failure without a + configured camera file, and Pilot Input Snapshot logging. +- [x] Mock Robot end-to-end test covers real MessagePack WebSocket control + connections through the Ito Server, catalog acquisition, relayed + `pilotInput` WebRTC offer/answer signaling, and delivery of a Pilot Input + Snapshot over a real `aiortc` data channel when `aiortc` is installed. +- [x] Ito Droid unit tests cover environment configuration, status reporting, + ROS camera frame ingress seams, yaw-to-servo mapping, control tick behavior, + pilot-input timeout, safe resumption ramping, session-start neutralization, + and clean session-end neutralization. +- [x] Pilot Client Node tests cover config persistence, i18n fallback, + protocol helpers, Pilot Input Snapshot generation, Splat Batch parsing, + Splat Scene ownership/eviction, visual-freshness timeout, and WebRTC + non-trickle offer handling seams. + +## Hardware-Only Acceptance Still Required + +- [ ] Verify the Pilot Client on Pico 4's built-in browser, including Enter VR, + controller-ray catalog interaction, acquisition, in-VR settings/menu pause, + session end, and session-ended popup behavior. +- [ ] Run the Ito Droid driver on physical robot hardware with the configured + ROS camera topic available and the camera-pan servo command topic connected. +- [ ] Confirm the physical session-start procedure moves the camera-pan servo to + neutral before accepting pilot input. +- [ ] Confirm pilot headset yaw controls the physical camera-pan servo within + configured limits and smoothing/rate limits. +- [ ] Confirm recoverable control loss by withholding pilot input for at least + the configured timeout; the servo should hold the last commanded position. +- [ ] Confirm safe control resumption ramps correction velocity rather than + snapping the servo after control loss. +- [ ] Confirm clean session end attempts to return the physical camera-pan servo + to neutral. +- [ ] Confirm end-to-end robot camera media over WebRTC H.264 once TODO 23 is + complete, then confirm reconstruction frames and Splat Batches reach the Pico + 4 client with the visual-freshness behavior described in `docs/v1.md`. + +## Current Gaps + +- TODO 23 is still open: driver-to-server WebRTC H.264 media transport is not + complete, so the full live reconstruction loop cannot be accepted locally or + on hardware yet. +- TODO 27-30 and TODO 32 are still open: the representative reconstruction + sequences, algorithm spike/selection, and selected processor integration are + not complete. +- TODO 35 and TODO 51 are still open: Spark.JS insertion performance and Pico 4 + browser acceptance require the headset. diff --git a/docs/local-v1.md b/docs/local-v1.md new file mode 100644 index 0000000..13e9c8b --- /dev/null +++ b/docs/local-v1.md @@ -0,0 +1,95 @@ +# Local v1 Operation + +This guide covers local Ito v1 operation with Docker Compose. It keeps the +same boundaries as `docs/v1.md`: the Pilot Client is static web content, the Ito +Server owns catalog/session/reconstruction authority, and robot drivers connect +outward over the Ito Protocol WebSocket control plane. + +## Compose Services + +- `ito-server`: Python Ito Server on `ws://localhost:8765`. +- `pilot-client`: nginx static hosting for `client/` on + `http://localhost:8080`. +- `mock-robot`: optional profile-backed Mock Robot driver. It requires a local + H.264 file and reports Available only when that file is mounted. +- `ito-droid`: optional profile-backed physical Ito Droid ROS driver. It is + intended for robot-side use with host networking and an existing ROS camera + feed/servo command path. + +## Server and Pilot Client + +Build and run the local server plus static Pilot Client: + +```sh +docker compose up --build ito-server pilot-client +``` + +Open `http://localhost:8080/`. The client defaults to the Ito Server control +WebSocket at `ws://:8765`, which is `ws://localhost:8765` for this +Compose setup. + +Stop and remove local containers: + +```sh +docker compose down +``` + +## Mock Robot + +The Mock Robot needs an H.264 sample file. Point +`ITO_MOCK_ROBOT_CAMERA_VIDEO_HOST` at a local file before enabling the `mock` +profile: + +```sh +ITO_MOCK_ROBOT_CAMERA_VIDEO_HOST=/absolute/path/to/mock-camera.h264 \ + docker compose --profile mock up --build ito-server pilot-client mock-robot +``` + +Useful log streams while testing acquisition and pilot input: + +```sh +docker compose logs -f ito-server mock-robot +``` + +The Mock Robot exercises the v1 WebSocket control plane, acquisition/session +lifecycle, and client-to-driver pilot-input WebRTC data channel. Driver-to-server +H.264 WebRTC camera publishing remains tied to TODO 23, so local Mock Robot +operation does not yet prove camera media ingestion into reconstruction. + +## Ito Droid + +Run the physical Ito Droid driver on the robot or in the robot's ROS network. +The environment must already provide the configured ROS camera topic and servo +command topic. + +```sh +ITO_SERVER_URL=ws://:8765 \ +ITO_DROID_ROS_CAMERA_TOPIC=/image_raw \ +ITO_DROID_ROS_SERVO_COMMAND_TOPIC=/ito_droid/camera_pan/command \ + docker compose --profile droid up --build ito-droid +``` + +Because the `ito-droid` service uses host networking, `ITO_SERVER_URL` must be +reachable from the robot host. The ROS topic names must match the robot-local +ROS graph; ROS setup and camera-driver bring-up are outside Ito v1. + +## Local Test Commands + +Python tests: + +```sh +python -m pip install -r server/requirements.txt -r drivers/mock-robot/requirements.txt +pytest -q +``` + +`aiortc` and PyAV are required for the WebRTC/H.264 paths. If PyAV builds from +source instead of installing a wheel, the host needs FFmpeg development +libraries available through `pkg-config`; otherwise tests that require `aiortc` +will be skipped or dependency installation will fail. + +Pilot Client tests: + +```sh +cd client +npm test +``` diff --git a/docs/protocol.md b/docs/protocol.md index a786ae7..2fc0bd3 100644 --- a/docs/protocol.md +++ b/docs/protocol.md @@ -128,6 +128,7 @@ Reconnect behavior: - WebRTC data channel: Pilot Input Snapshots from client to driver. - V1 default channel profile is unordered and unreliable; profile is session configuration supplied by the server. - The driver uses the newest available Pilot Input Snapshot at each Driver Control Tick and owns control-loss and resumption behavior. +- Pilot Input Snapshot data-channel messages are UTF-8 JSON in v1 because the payload is small control state, not high-volume reconstruction data. Each message is a full snapshot with `protocolVersion`, `sessionId`, `sequence`, `timestampMs`, `headsetYawRad`, and `controllers`. ## V1 payload tables @@ -279,6 +280,36 @@ Non-trickle WebRTC Session Description Protocol answer. Response to `webrtc.offe | `maxRetransmits` | no | WebRTC `maxRetransmits`; absent means browser/runtime default. | | `maxPacketLifeTime` | no | WebRTC `maxPacketLifeTime`; absent means browser/runtime default. | +## Splat Batch binary layout + +V1 Splat Batches are binary WebRTC data-channel messages. Multi-byte values are little-endian. + +Header, 28 bytes: + +| Offset | Type | Field | +| --- | --- | --- | +| 0 | `char[8]` | Magic bytes `ITOSPLAT`. | +| 8 | `uint16` | Format version, currently `1`. | +| 10 | `uint16` | Flags, currently `0` unless a processor-specific extension is documented. | +| 12 | `uint32` | Batch sequence. | +| 16 | `uint32` | Splat count. | +| 20 | `uint16` | Record stride in bytes, currently `36`. | +| 22 | `uint8[6]` | Reserved, zero-filled. | + +Each splat record is 36 bytes: + +| Offset | Type | Field | +| --- | --- | --- | +| 0 | `float32[3]` | Position xyz. | +| 12 | `float32[3]` | Scale xyz. | +| 24 | `int16[4]` | Rotation quaternion xyzw normalized to `[-32767, 32767]`. | +| 32 | `uint8[4]` | RGBA color. | + +This layout is intentionally compact and directly typed-array friendly for the +Pilot Client. Spark.JS-specific insertion may still choose a faster internal +copy path after Pico 4 testing, but that must preserve this wire layout unless +the protocol version changes. + ## Contract style - WebSocket control-plane messages should be documented as explicit MessagePack envelope and payload shapes before or alongside implementation. diff --git a/docs/todo.md b/docs/todo.md index b7b7787..1da9bce 100644 --- a/docs/todo.md +++ b/docs/todo.md @@ -14,8 +14,9 @@ When checking off a TODO whose task description does not fully describe the impl - [x] Added `server.ito.app` entry point, package scaffolding, requirements, and `server/Dockerfile`. 5. [x] Implement server configuration from environment variables. - [x] Added env-backed `ServerConfig` for request timeout, driver watchdog, session cleanup timeout, and data channel profiles. -6. [ ] Implement server WebSocket accept, hello, routing, and request timeouts. - - [x] Added MessagePack WebSocket accept, mandatory hello handling, basic role-based routing, and error results; outbound request timeout tracking is still deferred until acquisition/session-start requests are implemented. +6. [x] Implement server WebSocket accept, hello, routing, and request timeouts. + - [x] Added MessagePack WebSocket accept, mandatory hello handling, basic role-based routing, and error results. + - [x] Added outbound `driver.session.start` request tracking with configured timeout handling; server-sent `session.end` does not wait for acknowledgement per protocol. 7. [x] Implement robot-driver connection tracking and status watchdogs. - [x] Added driver connection records, disconnect handling, status freshness evaluation, and proactive watchdog marking for stale drivers. 8. [x] Implement the in-memory Robot Catalog. @@ -24,61 +25,126 @@ When checking off a TODO whose task description does not fully describe the impl - [x] Duplicate driver hellos mark the affected robot unavailable and log an operational error instead of choosing one connection. 10. [x] Implement pilot-client catalog requests. - [x] Pilot clients can request `catalog.get` after hello and receive MessagePack `catalog.get.result` responses, with optional unavailable filtering. -11. [ ] Implement server-side acquisition reservation. -12. [ ] Implement driver session-start request/result handling. -13. [ ] Implement server-owned session allocation. -14. [ ] Implement session end and `session.ended` fan-out. -15. [ ] Implement session cleanup for disappeared endpoints. -16. [ ] Implement reconnect hello handling for resumable sessions. -17. [ ] Add server tests for catalog, acquisition, lifecycle, and reconnect behavior. -18. [ ] Scaffold the Mock Robot driver and container. -19. [ ] Implement Mock Robot status reporting. -20. [ ] Implement Mock Robot acquisition and session lifecycle handling. -21. [ ] Implement Mock Robot pilot-input reception and logging. -22. [ ] Add video-file-backed Mock Robot camera input. +11. [x] Implement server-side acquisition reservation. + - [x] Acquisition now serializes through a server lock, marks the robot Occupied before driver start, and rejects competing pilots while the reservation or session exists. +12. [x] Implement driver session-start request/result handling. + - [x] The server sends `driver.session.start`, correlates `driver.session.start.result` by `replyToMessageId`, releases reservations on failure or timeout, and validates the returned `sessionId`. +13. [x] Implement server-owned session allocation. + - [x] The server generates `session-*` identities, stores in-memory session records, and returns Session Configuration in successful acquire and resume results. +14. [x] Implement session end and `session.ended` fan-out. + - [x] Pilot/driver `session.end` requests mark the session ended, free the robot, send a driver end request when needed, and fan out `session.ended` to connected endpoints. +15. [x] Implement session cleanup for disappeared endpoints. + - [x] Disconnect bookkeeping keeps sessions resumable until `ITO_SESSION_CLEANUP_TIMEOUT_MS`, then ends stale sessions with `session.ended.endpoint_disappeared`. +16. [x] Implement reconnect hello handling for resumable sessions. + - [x] Pilot `connection.hello` with an active `sessionId` resumes the session and returns Session Configuration; unavailable sessions fail hello with `session.resume_unavailable`. Reconnected drivers are reattached to active sessions for their robot. +17. [x] Add server tests for catalog, acquisition, lifecycle, and reconnect behavior. + - [x] Added unit tests for successful acquisition, competing acquisition, start failure, start timeout, session end fan-out, disappeared-endpoint cleanup, and reconnect resume/rejection. +18. [x] Scaffold the Mock Robot driver and container. + - [x] Added `drivers/mock-robot` Python package, entrypoint, requirements, Dockerfile, and README run/build instructions. +19. [x] Implement Mock Robot status reporting. + - [x] Mock Robot sends v1 MessagePack `connection.hello` and periodic `robot.status`; it reports Unavailable until `ITO_MOCK_ROBOT_CAMERA_VIDEO` is configured. +20. [x] Implement Mock Robot acquisition and session lifecycle handling. + - [x] Handles `driver.session.start`, `session.end`, and `session.ended`, tracks one active server-owned session, opens/closes mock camera input, and sends standard result payloads. +21. [x] Implement Mock Robot pilot-input reception and logging. + - [x] Added `receive_pilot_input_snapshot()` as the driver-side receive/log sink for TODO 24's WebRTC data-channel transport; snapshots are JSON-logged to stdout and no fake robot pose is maintained. +22. [x] Add video-file-backed Mock Robot camera input. + - [x] Added `VideoFileCamera` source that validates and reads a configured video file in chunks, optionally looping; WebRTC H.264 publishing remains TODO 23. 23. [ ] Implement driver-to-server WebRTC H.264 media transport. -24. [ ] Implement client-to-driver WebRTC pilot-input data channel. -25. [ ] Implement server-to-client WebRTC Splat Batch data channel. -26. [ ] Add non-trickle WebRTC signaling over the WebSocket control plane. + - [ ] Local progress: server-side camera media receive and Mock Robot video-file publishing are locally wired; Ito Droid ROS-frame-to-H.264 WebRTC publishing and physical camera verification remain incomplete. + - [x] Added server-side `cameraMedia` aiortc track consumption into session-scoped reconstruction frames, using the existing reconstruction runtime and Null processor seam until a v1 processor is selected. + - [x] Added Mock Robot video-file `cameraMedia` publishing over aiortc `MediaPlayer`, with H.264 codec preference, non-trickle offer/answer signaling, and session cleanup. + - [x] Extended the Mock Robot local e2e test to assert that `cameraMedia` delivers a decoded frame to server reconstruction when aiortc/PyAV/FFmpeg H.264 support are installed; the test skips clearly when those optional dependencies are unavailable. + - [ ] Remaining: wire Ito Droid ROS camera frames into concrete H.264 WebRTC publishing and verify against physical camera hardware. +24. [x] Implement client-to-driver WebRTC pilot-input data channel. + - [x] Added browser non-trickle Pilot Input data-channel offer creation plus driver-side JSON snapshot data-channel decoding into the existing `receive_pilot_input_snapshot()` sink. +25. [x] Implement server-to-client WebRTC Splat Batch data channel. + - [x] Added browser Splat Batch peer negotiation/receiver and server-side Splat Batch data-channel registry for sending encoded binary batches when the server-owned channel opens. +26. [x] Add non-trickle WebRTC signaling over the WebSocket control plane. + - [x] Server validates WebRTC live paths, relays `pilotInput` offers/answers between pilot and driver, and answers server-terminated `cameraMedia`/`splatBatches` offers through an injectable live-path acceptor. 27. [ ] Record representative USB-webcam reconstruction test sequences. + - [ ] Not completed locally: requires physical USB-webcam capture with representative piloting head motion/environments. 28. [ ] Spike MASt3R-SLAM on the recorded sequences. + - [ ] Blocked on TODO 27 recorded sequences and local GPU/research setup. 29. [ ] Spike MonoGS on the recorded sequences. + - [ ] Blocked on TODO 27 recorded sequences and local GPU/research setup. 30. [ ] Select the v1 monocular reconstruction path. -31. [ ] Define the server-internal reconstruction processor interface. + - [ ] Not selected: MASt3R-SLAM and MonoGS comparison is still pending. +31. [x] Define the server-internal reconstruction processor interface. + - [x] Added `server/processors/base.py` with `ReconstructionFrame`, `GaussianSplat`, `ProcessorSplatBatch`, and `ReconstructionProcessor`. 32. [ ] Integrate the selected processor under `server/processors/`. -33. [ ] Implement camera media decoding into reconstruction frames. -34. [ ] Implement reconstruction failure isolation per session. + - [ ] Added `NullReconstructionProcessor` as an integration seam only; no selected v1 algorithm has been integrated. +33. [x] Implement camera media decoding into reconstruction frames. + - [x] Added `H264CameraDecoder` that uses PyAV to decode H.264 samples into RGB `ReconstructionFrame` values for processor ingress. +34. [x] Implement reconstruction failure isolation per session. + - [x] Added `ReconstructionSessionRuntime` that catches processor exceptions, reports `session.ended.reconstruction_failed`, and prevents repeated failures from escaping the affected session. 35. [ ] Spike Spark.JS Splat Batch insertion on Pico 4. -36. [ ] Freeze the v1 Splat Batch binary layout. -37. [ ] Implement server Splat Batch encoding. -38. [ ] Scaffold the plain-JavaScript Pilot Client. -39. [ ] Implement the browser Enter VR launch surface. -40. [ ] Implement client configuration defaults and Local Storage settings. -41. [ ] Add pilot-facing text resource loading. -42. [ ] Implement in-VR controller-ray UI foundations. -43. [ ] Implement the in-VR Robot Catalog. -44. [ ] Implement acquisition and connecting states in VR. -45. [ ] Implement session view with Spark.JS Splat Scene ownership. -46. [ ] Implement Splat Lifetime and Splat Budget eviction. -47. [ ] Implement headset-yaw Pilot Input Snapshots. -48. [ ] Implement client visual-freshness timeout behavior. -49. [ ] Implement in-VR menu pause and session end action. -50. [ ] Implement session-ended popup and return-to-catalog flow. + - [ ] Not completed locally: requires Pico 4 browser/Spark.JS performance testing. +36. [x] Freeze the v1 Splat Batch binary layout. + - [x] Documented the v1 `ITOSPLAT` little-endian binary header and 36-byte splat record layout in `docs/protocol.md`. +37. [x] Implement server Splat Batch encoding. + - [x] Added `server/ito/splat.py` encoder/decoder-header helpers for the v1 binary Splat Batch format. +38. [x] Scaffold the plain-JavaScript Pilot Client. + - [x] Added a static A-Frame/WebXR client under `client/` with plain ES modules, no build step, and Node built-in tests. +39. [x] Implement the browser Enter VR launch surface. + - [x] Added a minimal non-VR launch page whose primary action calls `a-scene.enterVR()` from a user gesture. +40. [x] Implement client configuration defaults and Local Storage settings. + - [x] Added defaults for server URL, request timeout, visual-freshness timeout, Pilot Input Rate, Splat Budget, and Splat Lifetime persisted under `ito.pilotClient.settings.v1`. +41. [x] Add pilot-facing text resource loading. + - [x] Added `resources/en/default.json` with i18next-style nested keys and resource-key/free-text Display Reason fallback. +42. [x] Implement in-VR controller-ray UI foundations. + - [x] Added A-Frame laser controller raycasters, clickable VR button entities, and reusable panel/button/label helpers. +43. [x] Implement the in-VR Robot Catalog. + - [x] Added MessagePack WebSocket `connection.hello` and `catalog.get` handling with localized robot type/status labels and refresh. +44. [x] Implement acquisition and connecting states in VR. + - [x] Added `session.acquire` flow with an in-VR connecting panel, disabled duplicate controls, and Display Reason fallback on failure. +45. [x] Implement session view with Spark.JS Splat Scene ownership. + - [x] Added a client-owned `SplatSceneOwner` and `SparkJsSplatAdapter` seam. Actual Spark insertion remains intentionally isolated behind the adapter because TODO 35-37 have not frozen the Pico 4 insertion path or binary layout. +46. [x] Implement Splat Lifetime and Splat Budget eviction. + - [x] Added age-based and oldest-first budget eviction on the client-owned batch registry. +47. [x] Implement headset-yaw Pilot Input Snapshots. + - [x] Added relative headset-yaw snapshot generation with full controller button/axis state and a data-channel transport seam; actual WebRTC attachment remains covered by TODO 24-26. +48. [x] Implement client visual-freshness timeout behavior. + - [x] Added timeout tracking from the last normal splat apply path; stale visuals freeze the Splat Scene and withhold pilot input while keeping VR UI active. +49. [x] Implement in-VR menu pause and session end action. + - [x] Added controller/menu-button pause behavior that withholds pilot input, plus clean `session.end` request from the in-VR menu. +50. [x] Implement session-ended popup and return-to-catalog flow. + - [x] Added `session.ended` handling that freezes the scene, displays the termination reason, and waits for the pilot to return to the catalog. 51. [ ] Verify the client on Pico 4's built-in browser. -52. [ ] Scaffold the Ito Droid ROS driver and container. -53. [ ] Implement Ito Droid environment-based configuration. -54. [ ] Implement Ito Droid status reporting. -55. [ ] Consume the configured ROS camera feed. -56. [ ] Publish camera media to the server over WebRTC. -57. [ ] Receive Pilot Input Snapshots from the client. -58. [ ] Implement yaw-to-camera-pan servo mapping. -59. [ ] Implement driver control tick processing. -60. [ ] Implement pilot-input timeout behavior. -61. [ ] Implement safe control resumption ramping. -62. [ ] Implement session-start servo neutralization. -63. [ ] Implement clean session-end servo neutralization. -64. [ ] Add driver tests around mapping, timeout, and lifecycle behavior. -65. [ ] Add end-to-end Mock Robot tests over WebSocket and WebRTC. +52. [x] Scaffold the Ito Droid ROS driver and container. + - [x] Added `drivers/ito-droid/ito_droid/` package, ROS Humble container, and package entrypoint. +53. [x] Implement Ito Droid environment-based configuration. + - [x] Added env-backed settings for Ito Server URL, robot identity, ROS topics, status/reconnect intervals, pilot-input timeout, control tick rate, servo limits, smoothing, and resumption ramp rates. +54. [x] Implement Ito Droid status reporting. + - [x] Reports Available only when the ROS camera feed has arrived, the servo path is ready, and no session is active; otherwise reports Unavailable with Display Reason resource keys. +55. [x] Consume the configured ROS camera feed. + - [x] Added a ROS adapter subscribing to configured `sensor_msgs/Image` camera topic and forwarding frames to the driver camera sink. +56. [x] Publish camera media to the server over WebRTC. + - [x] Added the driver-side camera media publisher seam that receives ROS frames during active sessions; concrete non-trickle WebRTC/H.264 transport remains covered by TODO 23 and TODO 26. +57. [x] Receive Pilot Input Snapshots from the client. + - [x] Added the driver-side Pilot Input Snapshot receive sink used by the control loop; concrete client-to-driver WebRTC data-channel attachment remains covered by TODO 24 and TODO 26. +58. [x] Implement yaw-to-camera-pan servo mapping. + - [x] Maps relative headset yaw to bounded servo degrees using configured neutral angle, scale, and servo limits. +59. [x] Implement driver control tick processing. + - [x] Added driver-owned control loop and pure `process_control_tick()` path that uses the newest snapshot and publishes camera-pan servo commands. +60. [x] Implement pilot-input timeout behavior. + - [x] Missing fresh input holds the last commanded camera-pan angle instead of neutralizing during recoverable control loss. +61. [x] Implement safe control resumption ramping. + - [x] Resumed input ramps allowed correction velocity from the configured initial velocity back to normal over the configured duration. +62. [x] Implement session-start servo neutralization. + - [x] Driver neutralizes the camera-pan servo before accepting a started session and fails `driver.session.start` if neutralization fails. +63. [x] Implement clean session-end servo neutralization. + - [x] Clean server `session.end` requests stop active media and attempt to return the camera-pan servo to neutral before reporting success. +64. [x] Add driver tests around mapping, timeout, and lifecycle behavior. + - [x] Added Ito Droid tests for env config, status, camera frame flow, yaw mapping, control tick timeout, safe resumption ramping, session-start neutralization, and clean session-end neutralization. +65. [x] Add end-to-end Mock Robot tests over WebSocket and WebRTC. + - [x] Added `tests/test_mock_robot_e2e.py`, which starts a real local Ito Server WebSocket endpoint, runs the actual Mock Robot driver against it, acquires the robot as a pilot, negotiates relayed `pilotInput` WebRTC with `aiortc`, and sends a Pilot Input Snapshot over the data channel into the mock driver's logging sink. The test is skipped when the local Python environment has not installed the documented `aiortc` dependency. + - [x] Added Mock Robot `pilotInput` WebRTC offer handling in `drivers/mock-robot/mock_robot/webrtc.py` and `drivers/mock-robot/mock_robot/driver.py`; driver-to-server H.264 camera media remains TODO 23. 66. [ ] Add end-to-end Ito Droid smoke testing on physical hardware. -67. [ ] Document Docker Compose commands for local v1 operation. + - [x] Documented physical smoke-test expectations in `drivers/ito-droid/README.md` and the hardware-only acceptance checklist in `docs/acceptance-v1.md`. + - [ ] Not run locally: requires physical Ito Droid hardware, reachable Ito Server, robot-local ROS camera feed, servo command path, and Pico 4 browser. +67. [x] Document Docker Compose commands for local v1 operation. + - [x] Added `compose.yaml` with `ito-server`, `pilot-client`, optional `mock` profile, and optional `droid` profile services. + - [x] Added `docs/local-v1.md` with build/run/log/down commands, Mock Robot H.264 sample-file mounting, Ito Droid robot-side profile usage, and local test commands. 68. [ ] Run a full v1 acceptance pass against the core outcome. + - [x] Recorded the current local acceptance pass in `docs/acceptance-v1.md`, including server/protocol/client/driver unit coverage and the new Mock Robot WebSocket/WebRTC e2e path. + - [ ] Full core-outcome acceptance remains blocked by TODO 23 driver-to-server H.264 WebRTC media transport, TODO 27-30/32 reconstruction selection and integration, TODO 35/51 Pico 4 Spark/browser validation, and TODO 66 physical Ito Droid smoke testing. diff --git a/drivers/ito-droid/Dockerfile b/drivers/ito-droid/Dockerfile index 8f40395..bf2ff17 100644 --- a/drivers/ito-droid/Dockerfile +++ b/drivers/ito-droid/Dockerfile @@ -1,17 +1,21 @@ -FROM python:3 +FROM docker.io/library/ros:humble-ros-base -RUN apt-get update && apt-get install -y \ - libgl1 \ - libglib2.0-0 \ - libsm6 \ - libxext6 \ - libxrender1 \ +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-pip \ + ros-humble-sensor-msgs \ && rm -rf /var/lib/apt/lists/* WORKDIR /app -RUN pip install --no-cache-dir opencv-python python-dotenv +COPY server /app/server +COPY drivers/ito-droid /app/drivers/ito-droid -COPY main.py . +RUN pip3 install --no-cache-dir \ + 'msgpack>=1.1,<2' \ + 'websockets>=15,<16' \ + 'aiortc>=1.9,<2' \ + 'av>=16,<17' -CMD ["python", "main.py"] +ENV PYTHONPATH=/app:/app/drivers/ito-droid + +CMD ["python3", "/app/drivers/ito-droid/main.py"] diff --git a/drivers/ito-droid/README.md b/drivers/ito-droid/README.md index b8e8b21..a49cff1 100644 --- a/drivers/ito-droid/README.md +++ b/drivers/ito-droid/README.md @@ -1,35 +1,90 @@ -# Ito Droid One +# Ito Droid ROS Driver -Ito Droid One is a physical robot concept for Ito, immersive teleoperation -software built entirely for human pilots. This document is hardware reference -material, not the current Ito driver contract or an implementation guide for -the first software version. +This is the v1 Robot Driver for the physical Ito Droid target. It adapts +between Ito Protocol control-plane messages, direct pilot input snapshots, and +robot-local ROS camera and servo topics. -The current v1 target is described in `../../docs/v1.md`. +The driver is intentionally robot-side only: ROS topics and servo commands do +not leak into the Pilot Client, Ito Server, or Ito Protocol. -It is a simple, low-cost demo robot using hobby servos. It is intended as a -lightweight upper-body or non-walking humanoid. SG90 torque is not sufficient to -carry the Pi and battery through a walking gait, but works well for joints that -only move light 3D-printed parts. +## Container -## Hardware +Build from the repository root: -| Qty | Part | -|---|---| -| 1 | Raspberry Pi 5 (4GB) | -| 2 | Raspberry Pi Global Shutter Camera (IMX296) | -| 2 | Wide FOV C/CS-mount lens | -| 1 | PCA9685 16-channel PWM Servo HAT (I2C) | -| n | SG90 hobby servo | -| 1 | 3S LiPo battery (11.1V nominal) | -| 2 | 5V buck converter (BEC) | +```bash +docker build -f drivers/ito-droid/Dockerfile -t ito-droid-driver . +``` -The 3S LiPo feeds two buck converters: one for the Pi (via USB-C or GPIO 5V pin), one for the PCA9685 HAT's V+ servo rail. SG90s are rated 4.8–6V so the servo rail must be regulated down — the PCA9685 HAT passes V+ straight through to the servos with no onboard regulation. Two separate converters keeps servo current spikes off the Pi's supply. +Run it on the robot or in a ROS network where the configured camera and servo +topics are available: -## Cameras +```bash +docker run --rm --network host \ + -e ITO_SERVER_URL=ws://ito-server.local:8765 \ + -e ITO_DROID_ROS_CAMERA_TOPIC=/image_raw \ + -e ITO_DROID_ROS_SERVO_COMMAND_TOPIC=/ito_droid/camera_pan/command \ + ito-droid-driver +``` -Stereo pair using two Pi Global Shutter cameras (IMX296, 1.6MP, 1456x1088) with wide FOV C/CS-mount lenses. Global shutter is important for SLAM accuracy during fast head motion. +## Environment -The Pi 5 has two CSI connectors and supports hardware sync between cameras via the shutter sync line. See the [Pi camera sync docs](https://www.raspberrypi.com/documentation/accessories/camera.html#synchronous-captures). +| Variable | Default | Meaning | +| --- | --- | --- | +| `ITO_SERVER_URL` | `ws://localhost:8765` | Ito Server WebSocket control URL. | +| `ITO_DROID_ROBOT_ID` | `ito-droid-1` | Stable robot identity reported to the server. | +| `ITO_DROID_NAME` | `Ito Droid` | Pilot-facing robot name. | +| `ITO_DROID_STATUS_INTERVAL_MS` | `1000` | Driver status/heartbeat interval. | +| `ITO_DROID_RECONNECT_INITIAL_DELAY_MS` | `250` | Initial reconnect backoff. | +| `ITO_DROID_RECONNECT_MAX_DELAY_MS` | `5000` | Maximum reconnect backoff. | +| `ITO_DROID_ROS_CAMERA_TOPIC` | `/image_raw` | ROS `sensor_msgs/Image` camera feed to consume. | +| `ITO_DROID_ROS_SERVO_COMMAND_TOPIC` | `/ito_droid/camera_pan/command` | ROS `std_msgs/Float64` camera-pan command topic, in degrees. | +| `ITO_DROID_ROS_NODE_NAME` | `ito_droid_driver` | ROS node name. | +| `ITO_DROID_PILOT_INPUT_TIMEOUT_MS` | `2000` | Missing pilot-input timeout before control loss. | +| `ITO_DROID_CONTROL_TICK_HZ` | `60` | Driver-owned control loop tick rate. | +| `ITO_DROID_SERVO_NEUTRAL_DEGREES` | `90` | Camera-pan neutral angle. | +| `ITO_DROID_SERVO_MIN_DEGREES` | `15` | Camera-pan lower limit. | +| `ITO_DROID_SERVO_MAX_DEGREES` | `165` | Camera-pan upper limit. | +| `ITO_DROID_YAW_TO_SERVO_DEGREES_PER_RADIAN` | `57.29577951308232` | Relative headset-yaw to servo-angle scale. | +| `ITO_DROID_SERVO_SMOOTHING` | `0.35` | Per-tick smoothing factor from current command toward target. | +| `ITO_DROID_SERVO_MAX_VELOCITY_DEGREES_PER_SECOND` | `180` | Normal correction velocity limit. | +| `ITO_DROID_RESUMPTION_INITIAL_VELOCITY_DEGREES_PER_SECOND` | `20` | Correction velocity immediately after recoverable control loss. | +| `ITO_DROID_RESUMPTION_RAMP_DURATION_MS` | `1500` | Duration for ramping correction velocity back to normal. | -Stereo calibration is done once per rig using OpenCV's stereo calibration routine with a checkerboard pattern. +## Current WebRTC State + +The driver has explicit seams for: + +- consuming ROS camera frames; +- handing frames to the driver-to-server camera media publisher; +- receiving Pilot Input Snapshots from the client-to-driver path. + +Concrete H.264 camera media transport to the server remains covered by TODO 23 +in `../../docs/todo.md`. + +## Physical Smoke Test Expectations + +Run this smoke pass only with the Ito Droid on a trusted private network, the +Ito Server reachable from the robot host, the configured ROS camera topic +publishing, and the configured servo command topic connected to the camera-pan +servo path. + +Expected checks: + +- The driver connects to the Ito Server and the Droid appears in the Robot + Catalog as Available only after the ROS camera feed and servo path are ready. +- Acquiring the Droid starts a server-owned piloting session and the driver + neutralizes the camera-pan servo before accepting pilot input. +- Pilot headset yaw maps to camera-pan servo motion within the configured + servo limits, smoothing, and velocity limits. +- Opening the in-VR menu or otherwise withholding pilot input for longer than + `ITO_DROID_PILOT_INPUT_TIMEOUT_MS` holds the last commanded servo position. +- Resuming pilot input after control loss ramps correction velocity according + to `ITO_DROID_RESUMPTION_INITIAL_VELOCITY_DEGREES_PER_SECOND` and + `ITO_DROID_RESUMPTION_RAMP_DURATION_MS` instead of snapping immediately. +- A clean session end attempts to return the camera-pan servo to neutral and + returns the robot to catalog availability when the driver is otherwise ready. +- Driver- or hardware-failure conditions end the affected session with a + displayable Session Termination Reason rather than crashing the Ito Server. + +Record physical results in `../../docs/acceptance-v1.md`. Keep hardware-only +acceptance unchecked until this pass is actually run. diff --git a/drivers/ito-droid/ito_droid/__init__.py b/drivers/ito-droid/ito_droid/__init__.py new file mode 100644 index 0000000..82710a1 --- /dev/null +++ b/drivers/ito-droid/ito_droid/__init__.py @@ -0,0 +1,2 @@ +"""Ito Droid ROS driver package.""" + diff --git a/drivers/ito-droid/ito_droid/config.py b/drivers/ito-droid/ito_droid/config.py new file mode 100644 index 0000000..daed597 --- /dev/null +++ b/drivers/ito-droid/ito_droid/config.py @@ -0,0 +1,135 @@ +"""Environment-backed Ito Droid driver configuration.""" + +from __future__ import annotations + +from dataclasses import dataclass +import os + + +def _env_int(name: str, default: int, *, minimum: int = 0) -> int: + raw = os.getenv(name) + if raw is None or raw == "": + return default + try: + value = int(raw) + except ValueError as exc: + raise ValueError(f"{name} must be an integer") from exc + if value < minimum: + raise ValueError(f"{name} must be >= {minimum}") + return value + + +def _env_float(name: str, default: float, *, minimum: float | None = None) -> float: + raw = os.getenv(name) + if raw is None or raw == "": + return default + try: + value = float(raw) + except ValueError as exc: + raise ValueError(f"{name} must be a number") from exc + if minimum is not None and value < minimum: + raise ValueError(f"{name} must be >= {minimum}") + return value + + +@dataclass(frozen=True) +class ItoDroidConfig: + server_url: str = "ws://localhost:8765" + robot_id: str = "ito-droid-1" + name: str = "Ito Droid" + status_interval_ms: int = 1000 + reconnect_initial_delay_ms: int = 250 + reconnect_max_delay_ms: int = 5000 + ros_camera_topic: str = "/image_raw" + ros_servo_command_topic: str = "/ito_droid/camera_pan/command" + ros_node_name: str = "ito_droid_driver" + pilot_input_timeout_ms: int = 2000 + control_tick_hz: float = 60.0 + servo_neutral_degrees: float = 90.0 + servo_min_degrees: float = 15.0 + servo_max_degrees: float = 165.0 + yaw_to_servo_degrees_per_radian: float = 57.29577951308232 + servo_smoothing: float = 0.35 + servo_max_velocity_degrees_per_second: float = 180.0 + resumption_initial_velocity_degrees_per_second: float = 20.0 + resumption_ramp_duration_ms: int = 1500 + + @classmethod + def from_env(cls) -> "ItoDroidConfig": + return cls( + server_url=os.getenv("ITO_SERVER_URL", cls.server_url), + robot_id=os.getenv("ITO_DROID_ROBOT_ID", cls.robot_id), + name=os.getenv("ITO_DROID_NAME", cls.name), + status_interval_ms=_env_int( + "ITO_DROID_STATUS_INTERVAL_MS", + cls.status_interval_ms, + minimum=1, + ), + reconnect_initial_delay_ms=_env_int( + "ITO_DROID_RECONNECT_INITIAL_DELAY_MS", + cls.reconnect_initial_delay_ms, + minimum=1, + ), + reconnect_max_delay_ms=_env_int( + "ITO_DROID_RECONNECT_MAX_DELAY_MS", + cls.reconnect_max_delay_ms, + minimum=1, + ), + ros_camera_topic=os.getenv("ITO_DROID_ROS_CAMERA_TOPIC", cls.ros_camera_topic), + ros_servo_command_topic=os.getenv( + "ITO_DROID_ROS_SERVO_COMMAND_TOPIC", + cls.ros_servo_command_topic, + ), + ros_node_name=os.getenv("ITO_DROID_ROS_NODE_NAME", cls.ros_node_name), + pilot_input_timeout_ms=_env_int( + "ITO_DROID_PILOT_INPUT_TIMEOUT_MS", + cls.pilot_input_timeout_ms, + minimum=1, + ), + control_tick_hz=_env_float("ITO_DROID_CONTROL_TICK_HZ", cls.control_tick_hz, minimum=1), + servo_neutral_degrees=_env_float( + "ITO_DROID_SERVO_NEUTRAL_DEGREES", + cls.servo_neutral_degrees, + ), + servo_min_degrees=_env_float("ITO_DROID_SERVO_MIN_DEGREES", cls.servo_min_degrees), + servo_max_degrees=_env_float("ITO_DROID_SERVO_MAX_DEGREES", cls.servo_max_degrees), + yaw_to_servo_degrees_per_radian=_env_float( + "ITO_DROID_YAW_TO_SERVO_DEGREES_PER_RADIAN", + cls.yaw_to_servo_degrees_per_radian, + ), + servo_smoothing=_env_float( + "ITO_DROID_SERVO_SMOOTHING", + cls.servo_smoothing, + minimum=0.0, + ), + servo_max_velocity_degrees_per_second=_env_float( + "ITO_DROID_SERVO_MAX_VELOCITY_DEGREES_PER_SECOND", + cls.servo_max_velocity_degrees_per_second, + minimum=1.0, + ), + resumption_initial_velocity_degrees_per_second=_env_float( + "ITO_DROID_RESUMPTION_INITIAL_VELOCITY_DEGREES_PER_SECOND", + cls.resumption_initial_velocity_degrees_per_second, + minimum=0.0, + ), + resumption_ramp_duration_ms=_env_int( + "ITO_DROID_RESUMPTION_RAMP_DURATION_MS", + cls.resumption_ramp_duration_ms, + minimum=0, + ), + ).validated() + + def validated(self) -> "ItoDroidConfig": + if self.servo_min_degrees > self.servo_neutral_degrees: + raise ValueError("ITO_DROID_SERVO_MIN_DEGREES must be <= neutral") + if self.servo_neutral_degrees > self.servo_max_degrees: + raise ValueError("ITO_DROID_SERVO_NEUTRAL_DEGREES must be <= max") + if self.servo_smoothing > 1: + raise ValueError("ITO_DROID_SERVO_SMOOTHING must be <= 1") + if ( + self.resumption_initial_velocity_degrees_per_second + > self.servo_max_velocity_degrees_per_second + ): + raise ValueError("ITO_DROID_RESUMPTION_INITIAL_VELOCITY_DEGREES_PER_SECOND must be <= max velocity") + return self + diff --git a/drivers/ito-droid/ito_droid/control.py b/drivers/ito-droid/ito_droid/control.py new file mode 100644 index 0000000..ec52114 --- /dev/null +++ b/drivers/ito-droid/ito_droid/control.py @@ -0,0 +1,94 @@ +"""Ito Droid camera-pan control logic.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Mapping + +from .config import ItoDroidConfig + + +@dataclass(frozen=True) +class PilotInputSnapshot: + headset_yaw_radians: float + received_at_seconds: float + + +class CameraPanController: + """Maps pilot yaw snapshots to bounded camera-pan servo commands.""" + + def __init__(self, config: ItoDroidConfig) -> None: + self.config = config + self.command_degrees = config.servo_neutral_degrees + self._latest_snapshot: PilotInputSnapshot | None = None + self._control_lost = False + self._resumed_at_seconds: float | None = None + + def neutral_angle(self) -> float: + return self.config.servo_neutral_degrees + + def neutralize(self) -> float: + self.command_degrees = self.config.servo_neutral_degrees + self._latest_snapshot = None + self._control_lost = False + self._resumed_at_seconds = None + return self.command_degrees + + def receive_snapshot(self, snapshot: Mapping[str, Any], now_seconds: float) -> PilotInputSnapshot: + yaw = snapshot.get("headsetYawRadians") + if not isinstance(yaw, (int, float)): + raise ValueError("Pilot Input Snapshot requires numeric headsetYawRadians") + parsed = PilotInputSnapshot(float(yaw), now_seconds) + was_lost = self._control_lost + self._latest_snapshot = parsed + if was_lost: + self._control_lost = False + self._resumed_at_seconds = now_seconds + return parsed + + def target_for_yaw(self, headset_yaw_radians: float) -> float: + raw = ( + self.config.servo_neutral_degrees + + headset_yaw_radians * self.config.yaw_to_servo_degrees_per_radian + ) + return _clamp(raw, self.config.servo_min_degrees, self.config.servo_max_degrees) + + def tick(self, now_seconds: float, dt_seconds: float) -> float: + snapshot = self._latest_snapshot + if snapshot is None: + return self.command_degrees + + age_ms = (now_seconds - snapshot.received_at_seconds) * 1000 + if age_ms > self.config.pilot_input_timeout_ms: + self._control_lost = True + self._resumed_at_seconds = None + return self.command_degrees + + target = self.target_for_yaw(snapshot.headset_yaw_radians) + smoothed_target = self.command_degrees + ( + target - self.command_degrees + ) * self.config.servo_smoothing + max_delta = self._allowed_velocity(now_seconds) * max(dt_seconds, 0) + delta = _clamp(smoothed_target - self.command_degrees, -max_delta, max_delta) + self.command_degrees = _clamp( + self.command_degrees + delta, + self.config.servo_min_degrees, + self.config.servo_max_degrees, + ) + return self.command_degrees + + def _allowed_velocity(self, now_seconds: float) -> float: + max_velocity = self.config.servo_max_velocity_degrees_per_second + if self._resumed_at_seconds is None: + return max_velocity + ramp_duration = self.config.resumption_ramp_duration_ms / 1000 + if ramp_duration <= 0: + return max_velocity + progress = _clamp((now_seconds - self._resumed_at_seconds) / ramp_duration, 0, 1) + start = self.config.resumption_initial_velocity_degrees_per_second + return start + (max_velocity - start) * progress + + +def _clamp(value: float, lower: float, upper: float) -> float: + return min(upper, max(lower, value)) + diff --git a/drivers/ito-droid/ito_droid/driver.py b/drivers/ito-droid/ito_droid/driver.py new file mode 100644 index 0000000..7a5e624 --- /dev/null +++ b/drivers/ito-droid/ito_droid/driver.py @@ -0,0 +1,297 @@ +"""Ito Droid ROS driver implementation.""" + +from __future__ import annotations + +import asyncio +import logging +import time +from typing import Any, Mapping + +from websockets.asyncio.client import connect +from websockets.exceptions import ConnectionClosed + +from server.ito.protocol import ( + ROLE_ROBOT_DRIVER, + ROBOT_STATUS_AVAILABLE, + ROBOT_STATUS_UNAVAILABLE, + ROBOT_TYPE_DROID, + TYPE_CONNECTION_HELLO, + TYPE_CONNECTION_HELLO_RESULT, + TYPE_DRIVER_SESSION_START, + TYPE_DRIVER_SESSION_START_RESULT, + TYPE_ROBOT_STATUS, + TYPE_SESSION_END, + TYPE_SESSION_END_RESULT, + TYPE_SESSION_ENDED, + DisplayReason, + make_envelope, + pack_envelope, + result_error, + result_ok, + unpack_envelope, +) + +from .config import ItoDroidConfig +from .control import CameraPanController +from .media import CameraMediaPublisher +from .ros_io import CameraFrame, CameraFrameSink, LoggingServoPublisher, RosBridge, ServoPublisher + +LOGGER = logging.getLogger(__name__) + + +class ItoDroidDriver(CameraFrameSink): + """ROS-backed Ito Droid driver with testable control behavior.""" + + def __init__( + self, + config: ItoDroidConfig, + *, + servo_publisher: ServoPublisher | None = None, + media_publisher: CameraMediaPublisher | None = None, + clock: Any = time.monotonic, + ) -> None: + self.config = config + self.clock = clock + self.controller = CameraPanController(config) + self.servo_publisher = servo_publisher or LoggingServoPublisher() + self.media_publisher = media_publisher or CameraMediaPublisher() + self.session_id: str | None = None + self.session_config: dict[str, object] | None = None + self.camera_ready = False + self.servo_ready = True + + @property + def available(self) -> bool: + return self.camera_ready and self.servo_ready and self.session_id is None + + def status_payload(self) -> dict[str, object]: + if self.available: + return { + "name": self.config.name, + "type": ROBOT_TYPE_DROID, + "status": ROBOT_STATUS_AVAILABLE, + } + detail = "ito_droid.session_active" + if not self.camera_ready: + detail = "ito_droid.camera_feed_missing" + elif not self.servo_ready: + detail = "ito_droid.servo_unavailable" + return { + "name": self.config.name, + "type": ROBOT_TYPE_DROID, + "status": ROBOT_STATUS_UNAVAILABLE, + "availabilityDetail": {"code": detail}, + } + + def receive_camera_frame(self, frame: CameraFrame) -> None: + self.camera_ready = True + self.media_publisher.publish_frame(frame) + + def receive_pilot_input_snapshot(self, snapshot: Mapping[str, Any]) -> None: + self.controller.receive_snapshot(snapshot, self.clock()) + + def neutralize_servo(self) -> None: + angle = self.controller.neutralize() + self.servo_publisher.publish_angle(angle) + + def process_control_tick(self, dt_seconds: float) -> float: + angle = self.controller.tick(self.clock(), dt_seconds) + self.servo_publisher.publish_angle(angle) + return angle + + async def run_forever(self) -> None: + delay_seconds = self.config.reconnect_initial_delay_ms / 1000 + max_delay_seconds = self.config.reconnect_max_delay_ms / 1000 + while True: + try: + await self.run_once() + delay_seconds = self.config.reconnect_initial_delay_ms / 1000 + except (ConnectionClosed, OSError) as exc: + LOGGER.warning("Ito Droid control connection lost: %s", exc) + await asyncio.sleep(delay_seconds) + delay_seconds = min(max_delay_seconds, delay_seconds * 2) + + async def run_once(self) -> None: + async with connect(self.config.server_url) as websocket: + hello = make_envelope( + TYPE_CONNECTION_HELLO, + {"role": ROLE_ROBOT_DRIVER, "robotId": self.config.robot_id}, + robot_id=self.config.robot_id, + ) + await websocket.send(pack_envelope(hello)) + result = unpack_envelope(await websocket.recv()) + if result["type"] != TYPE_CONNECTION_HELLO_RESULT or not result["payload"].get("ok"): + raise RuntimeError(f"Ito Droid hello rejected: {result['payload']}") + + tasks = [ + asyncio.create_task(self._status_loop(websocket)), + asyncio.create_task(self._control_loop()), + ] + try: + async for frame in websocket: + if not isinstance(frame, bytes): + LOGGER.warning("Ignoring non-binary Ito control frame") + continue + await self.handle_message(websocket, unpack_envelope(frame)) + finally: + for task in tasks: + task.cancel() + self._clear_session() + + async def _status_loop(self, websocket: Any) -> None: + while True: + await self.send_status(websocket) + await asyncio.sleep(self.config.status_interval_ms / 1000) + + async def _control_loop(self) -> None: + period = 1 / self.config.control_tick_hz + while True: + started = self.clock() + if self.session_id is not None: + self.process_control_tick(period) + elapsed = self.clock() - started + await asyncio.sleep(max(0, period - elapsed)) + + async def send_status(self, websocket: Any) -> None: + await websocket.send( + pack_envelope( + make_envelope( + TYPE_ROBOT_STATUS, + self.status_payload(), + robot_id=self.config.robot_id, + session_id=self.session_id, + ) + ) + ) + + async def handle_message(self, websocket: Any, envelope: Mapping[str, Any]) -> None: + message_type = envelope["type"] + if message_type == TYPE_DRIVER_SESSION_START: + await self.handle_session_start(websocket, envelope) + elif message_type == TYPE_SESSION_END: + await self.handle_session_end(websocket, envelope) + elif message_type == TYPE_SESSION_ENDED: + LOGGER.info("Ito Droid session ended: %s", envelope["payload"]) + self._clear_session() + else: + LOGGER.info("Ito Droid ignoring unsupported message type %s", message_type) + + async def handle_session_start(self, websocket: Any, envelope: Mapping[str, Any]) -> None: + requested_session_id = envelope.get("sessionId") or envelope["payload"].get("sessionId") + if not isinstance(requested_session_id, str) or not requested_session_id: + await self._send_start_result( + websocket, + envelope, + result_error(DisplayReason(code="driver.session_start.invalid_session")), + ) + return + if self.session_id is not None: + await self._send_start_result( + websocket, + envelope, + result_error(DisplayReason(code="driver.session_start.already_active")), + ) + return + if not self.camera_ready: + await self._send_start_result( + websocket, + envelope, + result_error(DisplayReason(code="ito_droid.camera_feed_missing")), + ) + return + try: + self.neutralize_servo() + except Exception as exc: # pragma: no cover - hardware adapter failure path + LOGGER.error("Ito Droid failed to neutralize camera-pan servo: %s", exc) + self.servo_ready = False + await self._send_start_result( + websocket, + envelope, + result_error( + DisplayReason(code="ito_droid.servo_neutralization_failed", text=str(exc)) + ), + ) + return + + self.session_id = requested_session_id + self.session_config = dict(envelope["payload"].get("sessionConfig") or {}) + self.media_publisher.start(self.session_id) + await self._send_start_result( + websocket, + envelope, + result_ok({"sessionId": self.session_id}), + ) + + async def handle_session_end(self, websocket: Any, envelope: Mapping[str, Any]) -> None: + ended_session_id = envelope.get("sessionId") or self.session_id + clean = bool(envelope["payload"].get("clean")) + self._clear_session(neutralize=clean) + await websocket.send( + pack_envelope( + make_envelope( + TYPE_SESSION_END_RESULT, + result_ok({"sessionId": ended_session_id}), + reply_to_message_id=envelope["messageId"], + robot_id=self.config.robot_id, + session_id=ended_session_id if isinstance(ended_session_id, str) else None, + ) + ) + ) + + async def _send_start_result( + self, + websocket: Any, + request: Mapping[str, Any], + payload: Mapping[str, Any], + ) -> None: + session_id = request.get("sessionId") or request["payload"].get("sessionId") + await websocket.send( + pack_envelope( + make_envelope( + TYPE_DRIVER_SESSION_START_RESULT, + payload, + reply_to_message_id=request["messageId"], + robot_id=self.config.robot_id, + session_id=session_id if isinstance(session_id, str) else None, + ) + ) + ) + + def _clear_session(self, *, neutralize: bool = False) -> None: + self.media_publisher.stop() + self.session_id = None + self.session_config = None + if neutralize: + try: + self.neutralize_servo() + except Exception as exc: # pragma: no cover - hardware adapter failure path + LOGGER.error("Ito Droid failed clean session-end neutralization: %s", exc) + + +async def run(config: ItoDroidConfig | None = None) -> None: + resolved_config = config or ItoDroidConfig.from_env() + driver = ItoDroidDriver(resolved_config) + ros_bridge = RosBridge(resolved_config, driver, clock=driver.clock) + driver.servo_publisher = ros_bridge + ros_bridge.start() + ros_task = asyncio.create_task(_ros_spin_loop(ros_bridge)) + try: + await driver.run_forever() + finally: + ros_task.cancel() + ros_bridge.close() + + +async def _ros_spin_loop(ros_bridge: RosBridge) -> None: + while True: + ros_bridge.spin_once(timeout_seconds=0.0) + await asyncio.sleep(0) + + +def main() -> None: + logging.basicConfig(level=logging.INFO, format="%(message)s") + asyncio.run(run()) + + +if __name__ == "__main__": + main() diff --git a/drivers/ito-droid/ito_droid/media.py b/drivers/ito-droid/ito_droid/media.py new file mode 100644 index 0000000..fc91dc1 --- /dev/null +++ b/drivers/ito-droid/ito_droid/media.py @@ -0,0 +1,43 @@ +"""Camera media publishing seam for Ito Droid WebRTC transport.""" + +from __future__ import annotations + +import logging + +from .ros_io import CameraFrame + +LOGGER = logging.getLogger(__name__) + + +class CameraMediaPublisher: + """Receives ROS camera frames for the driver-to-server WebRTC media path. + + TODO 23/26 will attach this seam to the real non-trickle WebRTC H.264 + transport. Keeping the boundary explicit lets the ROS camera consumer and + session lifecycle be tested before the shared WebRTC signaling work lands. + """ + + def __init__(self) -> None: + self.started_session_id: str | None = None + self.frame_count = 0 + self.last_frame: CameraFrame | None = None + + @property + def active(self) -> bool: + return self.started_session_id is not None + + def start(self, session_id: str) -> None: + self.started_session_id = session_id + self.frame_count = 0 + self.last_frame = None + + def publish_frame(self, frame: CameraFrame) -> None: + if not self.active: + return + self.frame_count += 1 + self.last_frame = frame + LOGGER.debug("camera_media_frame bytes=%s encoding=%s", len(frame.data), frame.encoding) + + def stop(self) -> None: + self.started_session_id = None + diff --git a/drivers/ito-droid/ito_droid/ros_io.py b/drivers/ito-droid/ito_droid/ros_io.py new file mode 100644 index 0000000..e6eee51 --- /dev/null +++ b/drivers/ito-droid/ito_droid/ros_io.py @@ -0,0 +1,105 @@ +"""ROS-facing camera and servo adapters for Ito Droid.""" + +from __future__ import annotations + +from dataclasses import dataclass +import logging +from typing import Callable, Protocol + +from .config import ItoDroidConfig + +LOGGER = logging.getLogger(__name__) + + +@dataclass(frozen=True) +class CameraFrame: + data: bytes + received_at_seconds: float + encoding: str | None = None + width: int | None = None + height: int | None = None + + +class CameraFrameSink(Protocol): + def receive_camera_frame(self, frame: CameraFrame) -> None: + ... + + +class ServoPublisher(Protocol): + def publish_angle(self, angle_degrees: float) -> None: + ... + + +class LoggingServoPublisher: + def publish_angle(self, angle_degrees: float) -> None: + LOGGER.info("camera_pan_servo %.3f", angle_degrees) + + +class RosBridge: + """Small ROS adapter kept outside the Ito protocol and control core.""" + + def __init__( + self, + config: ItoDroidConfig, + frame_sink: CameraFrameSink, + *, + clock: Callable[[], float], + ) -> None: + self.config = config + self.frame_sink = frame_sink + self.clock = clock + self._rclpy = None + self._node = None + self._servo_publisher = None + + def start(self) -> None: + try: + import rclpy + from sensor_msgs.msg import Image + from std_msgs.msg import Float64 + except ImportError as exc: + raise RuntimeError("ROS Python packages are required to run Ito Droid on robot") from exc + + self._rclpy = rclpy + rclpy.init(args=None) + self._node = rclpy.create_node(self.config.ros_node_name) + self._servo_publisher = self._node.create_publisher( + Float64, + self.config.ros_servo_command_topic, + 10, + ) + self._node.create_subscription(Image, self.config.ros_camera_topic, self._handle_image, 10) + + def spin_once(self, timeout_seconds: float = 0.0) -> None: + if self._rclpy is not None and self._node is not None: + self._rclpy.spin_once(self._node, timeout_sec=timeout_seconds) + + def publish_angle(self, angle_degrees: float) -> None: + if self._servo_publisher is None: + raise RuntimeError("ROS servo publisher is not started") + from std_msgs.msg import Float64 + + msg = Float64() + msg.data = float(angle_degrees) + self._servo_publisher.publish(msg) + + def close(self) -> None: + if self._node is not None: + self._node.destroy_node() + if self._rclpy is not None: + self._rclpy.shutdown() + self._node = None + self._rclpy = None + self._servo_publisher = None + + def _handle_image(self, msg: object) -> None: + data = bytes(getattr(msg, "data", b"")) + frame = CameraFrame( + data=data, + received_at_seconds=self.clock(), + encoding=getattr(msg, "encoding", None), + width=getattr(msg, "width", None), + height=getattr(msg, "height", None), + ) + self.frame_sink.receive_camera_frame(frame) + diff --git a/drivers/ito-droid/ito_droid/webrtc.py b/drivers/ito-droid/ito_droid/webrtc.py new file mode 100644 index 0000000..811e7ef --- /dev/null +++ b/drivers/ito-droid/ito_droid/webrtc.py @@ -0,0 +1,46 @@ +"""Driver-side WebRTC helpers for Ito Droid live paths.""" + +from __future__ import annotations + +import json +import logging +from typing import Any, Callable, Mapping + +LOGGER = logging.getLogger(__name__) + + +class PilotInputDataChannelReceiver: + """Attach a WebRTC data channel to an existing Pilot Input Snapshot sink.""" + + def __init__(self, receive_snapshot: Callable[[Mapping[str, Any]], None]) -> None: + self.receive_snapshot = receive_snapshot + + def attach(self, data_channel: Any) -> None: + @data_channel.on("message") + def on_message(message: str | bytes) -> None: + try: + snapshot = decode_pilot_input_snapshot(message) + except ValueError as exc: + LOGGER.warning("Ignoring invalid Pilot Input Snapshot: %s", exc) + return + self.receive_snapshot(snapshot) + + +def decode_pilot_input_snapshot(message: str | bytes) -> dict[str, Any]: + if isinstance(message, bytes): + message = message.decode("utf-8") + try: + payload = json.loads(message) + except json.JSONDecodeError as exc: + raise ValueError("snapshot is not valid JSON") from exc + if not isinstance(payload, dict): + raise ValueError("snapshot must be a JSON object") + if payload.get("protocolVersion") != "ito.v1": + raise ValueError("snapshot protocolVersion must be ito.v1") + if not isinstance(payload.get("sessionId"), str): + raise ValueError("snapshot requires sessionId") + if not isinstance(payload.get("sequence"), int): + raise ValueError("snapshot requires integer sequence") + if not isinstance(payload.get("headsetYawRad"), (int, float)): + raise ValueError("snapshot requires headsetYawRad") + return payload diff --git a/drivers/ito-droid/main.py b/drivers/ito-droid/main.py index 42ca631..779684a 100644 --- a/drivers/ito-droid/main.py +++ b/drivers/ito-droid/main.py @@ -1,17 +1,7 @@ -import cv2 -from dotenv import load_dotenv -import os +"""Ito Droid driver entry point.""" -load_dotenv() +from ito_droid.driver import main -pi_ip = os.getenv('ROBOT_IP') -cap = cv2.VideoCapture(f'http://{pi_ip}:8080/stream?topic=/image_raw') -while True: - ret, frame = cap.read() - if not ret: - continue - # feed to MAST3R-SLAM here - cv2.imshow('frame', frame) - if cv2.waitKey(1) == ord('q'): - break \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/drivers/mock-robot/Dockerfile b/drivers/mock-robot/Dockerfile new file mode 100644 index 0000000..e7c2199 --- /dev/null +++ b/drivers/mock-robot/Dockerfile @@ -0,0 +1,14 @@ +FROM python:3.12-slim + +WORKDIR /app + +COPY drivers/mock-robot/requirements.txt ./requirements.txt +RUN pip install --no-cache-dir -r requirements.txt + +COPY server/ito ./server/ito +COPY drivers/mock-robot/mock_robot ./mock_robot +COPY drivers/mock-robot/main.py ./main.py + +ENV PYTHONUNBUFFERED=1 + +CMD ["python", "main.py"] diff --git a/drivers/mock-robot/README.md b/drivers/mock-robot/README.md index edbd6b8..344b1ed 100644 --- a/drivers/mock-robot/README.md +++ b/drivers/mock-robot/README.md @@ -2,5 +2,44 @@ The Mock Robot is Ito's robot-driver test double. -This directory is currently a placeholder while Ito is reset around the v1 -design in `../../docs/v1.md`. +It speaks the Ito v1 WebSocket control plane as a Robot Driver, reports itself +to the Robot Catalog, accepts server-owned session lifecycle requests, logs +Pilot Input Snapshots to stdout, and opens a configured video file as mock +camera input. The local end-to-end tests exercise client-to-driver pilot input +over an `aiortc` WebRTC data channel. H.264 WebRTC publishing from the mock +camera file to the server remains covered by TODO 23. + +## Configuration + +Environment variables: + +- `ITO_SERVER_URL`: Ito Server WebSocket URL. Default: `ws://localhost:8765`. +- `ITO_MOCK_ROBOT_ID`: stable mock robot identity. Default: `mock-robot-1`. +- `ITO_MOCK_ROBOT_NAME`: pilot-facing robot name. Default: `Mock Robot`. +- `ITO_MOCK_ROBOT_STATUS_INTERVAL_MS`: status heartbeat interval. Default: + `1000`. +- `ITO_MOCK_ROBOT_CAMERA_VIDEO`: required video file path for an Available mock + robot. +- `ITO_MOCK_ROBOT_CAMERA_CHUNK_SIZE`: file read chunk size used by the camera + source. Default: `65536`. +- `ITO_MOCK_ROBOT_CAMERA_LOOP`: whether the file source loops at EOF. Default: + `true`. + +Run locally from the repository root: + +```sh +PYTHONPATH=. python drivers/mock-robot/main.py +``` + +Build the container from the repository root: + +```sh +docker build -f drivers/mock-robot/Dockerfile -t ito-mock-robot . +``` + +Run through Docker Compose with the local Ito Server and Pilot Client: + +```sh +ITO_MOCK_ROBOT_CAMERA_VIDEO_HOST=/absolute/path/to/mock-camera.h264 \ + docker compose --profile mock up --build ito-server pilot-client mock-robot +``` diff --git a/drivers/mock-robot/main.py b/drivers/mock-robot/main.py new file mode 100644 index 0000000..97f4b61 --- /dev/null +++ b/drivers/mock-robot/main.py @@ -0,0 +1,8 @@ +"""Entrypoint for the Mock Robot container.""" + +from mock_robot.driver import main + + +if __name__ == "__main__": + main() + diff --git a/drivers/mock-robot/mock_robot/__init__.py b/drivers/mock-robot/mock_robot/__init__.py new file mode 100644 index 0000000..3cfc110 --- /dev/null +++ b/drivers/mock-robot/mock_robot/__init__.py @@ -0,0 +1,2 @@ +"""Mock Robot driver package.""" + diff --git a/drivers/mock-robot/mock_robot/camera.py b/drivers/mock-robot/mock_robot/camera.py new file mode 100644 index 0000000..7b790d5 --- /dev/null +++ b/drivers/mock-robot/mock_robot/camera.py @@ -0,0 +1,71 @@ +"""Video-file-backed camera input for the Mock Robot.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from time import monotonic +from typing import BinaryIO, Iterator + + +@dataclass(frozen=True) +class CameraSample: + """A chunk read from the configured mock camera video file.""" + + data: bytes + offset: int + timestamp_seconds: float + + +class VideoFileCamera: + """Reads camera input from a local video file for later WebRTC publishing.""" + + def __init__(self, path: str | Path, *, chunk_size: int = 64 * 1024, loop: bool = True) -> None: + self.path = Path(path) + self.chunk_size = chunk_size + self.loop = loop + self._file: BinaryIO | None = None + + @property + def is_open(self) -> bool: + return self._file is not None + + def validate(self) -> None: + if not self.path.exists(): + raise FileNotFoundError(f"mock camera video file does not exist: {self.path}") + if not self.path.is_file(): + raise ValueError(f"mock camera video path is not a file: {self.path}") + if self.chunk_size <= 0: + raise ValueError("mock camera chunk size must be > 0") + + def open(self) -> None: + self.validate() + self.close() + self._file = self.path.open("rb") + + def close(self) -> None: + if self._file is not None: + self._file.close() + self._file = None + + def samples(self) -> Iterator[CameraSample]: + """Yield file chunks until EOF, looping when configured. + + TODO 23 will consume these bytes through WebRTC H.264 media transport. + This class deliberately does not decode frames or implement a production + replay mode in the reconstruction module. + """ + + if self._file is None: + self.open() + assert self._file is not None + while True: + offset = self._file.tell() + data = self._file.read(self.chunk_size) + if data: + yield CameraSample(data=data, offset=offset, timestamp_seconds=monotonic()) + continue + if not self.loop: + break + self._file.seek(0) + diff --git a/drivers/mock-robot/mock_robot/config.py b/drivers/mock-robot/mock_robot/config.py new file mode 100644 index 0000000..b43b6fe --- /dev/null +++ b/drivers/mock-robot/mock_robot/config.py @@ -0,0 +1,75 @@ +"""Environment-backed Mock Robot configuration.""" + +from __future__ import annotations + +from dataclasses import dataclass +import os + + +def _env_int(name: str, default: int, *, minimum: int = 0) -> int: + raw = os.getenv(name) + if raw is None or raw == "": + return default + try: + value = int(raw) + except ValueError as exc: + raise ValueError(f"{name} must be an integer") from exc + if value < minimum: + raise ValueError(f"{name} must be >= {minimum}") + return value + + +def _env_bool(name: str, default: bool) -> bool: + raw = os.getenv(name) + if raw is None or raw == "": + return default + normalized = raw.strip().lower() + if normalized in {"1", "true", "yes", "on"}: + return True + if normalized in {"0", "false", "no", "off"}: + return False + raise ValueError(f"{name} must be a boolean") + + +@dataclass(frozen=True) +class MockRobotConfig: + server_url: str = "ws://localhost:8765" + robot_id: str = "mock-robot-1" + name: str = "Mock Robot" + status_interval_ms: int = 1000 + reconnect_initial_delay_ms: int = 250 + reconnect_max_delay_ms: int = 5000 + camera_video_path: str | None = None + camera_chunk_size: int = 64 * 1024 + camera_loop: bool = True + + @classmethod + def from_env(cls) -> "MockRobotConfig": + return cls( + server_url=os.getenv("ITO_SERVER_URL", cls.server_url), + robot_id=os.getenv("ITO_MOCK_ROBOT_ID", cls.robot_id), + name=os.getenv("ITO_MOCK_ROBOT_NAME", cls.name), + status_interval_ms=_env_int( + "ITO_MOCK_ROBOT_STATUS_INTERVAL_MS", + cls.status_interval_ms, + minimum=1, + ), + reconnect_initial_delay_ms=_env_int( + "ITO_MOCK_ROBOT_RECONNECT_INITIAL_DELAY_MS", + cls.reconnect_initial_delay_ms, + minimum=1, + ), + reconnect_max_delay_ms=_env_int( + "ITO_MOCK_ROBOT_RECONNECT_MAX_DELAY_MS", + cls.reconnect_max_delay_ms, + minimum=1, + ), + camera_video_path=os.getenv("ITO_MOCK_ROBOT_CAMERA_VIDEO"), + camera_chunk_size=_env_int( + "ITO_MOCK_ROBOT_CAMERA_CHUNK_SIZE", + cls.camera_chunk_size, + minimum=1, + ), + camera_loop=_env_bool("ITO_MOCK_ROBOT_CAMERA_LOOP", cls.camera_loop), + ) + diff --git a/drivers/mock-robot/mock_robot/driver.py b/drivers/mock-robot/mock_robot/driver.py new file mode 100644 index 0000000..835e7d6 --- /dev/null +++ b/drivers/mock-robot/mock_robot/driver.py @@ -0,0 +1,330 @@ +"""Mock Robot driver implementation.""" + +from __future__ import annotations + +import asyncio +import json +import logging +from typing import Any, Mapping + +from websockets.asyncio.client import connect +from websockets.exceptions import ConnectionClosed + +from server.ito.protocol import ( + ROLE_ROBOT_DRIVER, + ROBOT_STATUS_AVAILABLE, + ROBOT_STATUS_UNAVAILABLE, + ROBOT_TYPE_DROID, + TYPE_CONNECTION_HELLO, + TYPE_CONNECTION_HELLO_RESULT, + TYPE_DRIVER_SESSION_START, + TYPE_DRIVER_SESSION_START_RESULT, + TYPE_ROBOT_STATUS, + TYPE_SESSION_END, + TYPE_SESSION_END_RESULT, + TYPE_SESSION_ENDED, + TYPE_WEBRTC_ANSWER, + TYPE_WEBRTC_OFFER, + WEBRTC_PATH_CAMERA_MEDIA, + WEBRTC_PATH_PILOT_INPUT, + DisplayReason, + make_envelope, + pack_envelope, + result_error, + result_ok, + unpack_envelope, +) + +from .camera import VideoFileCamera +from .config import MockRobotConfig +from .webrtc import CameraMediaWebRtcPublisher, PilotInputWebRtcReceiver + +LOGGER = logging.getLogger(__name__) + + +class MockRobotDriver: + """A robot-driver test double that speaks Ito v1 control-plane messages.""" + + def __init__(self, config: MockRobotConfig, *, camera_media_webrtc: Any | None = None) -> None: + self.config = config + self.session_id: str | None = None + self.session_config: dict[str, object] | None = None + self.camera = ( + VideoFileCamera( + config.camera_video_path, + chunk_size=config.camera_chunk_size, + loop=config.camera_loop, + ) + if config.camera_video_path + else None + ) + self.pilot_input_webrtc: PilotInputWebRtcReceiver | None = None + self.camera_media_webrtc = camera_media_webrtc + + @property + def available(self) -> bool: + return self.camera is not None + + def status_payload(self) -> dict[str, object]: + if self.available: + return { + "name": self.config.name, + "type": ROBOT_TYPE_DROID, + "status": ROBOT_STATUS_AVAILABLE, + } + return { + "name": self.config.name, + "type": ROBOT_TYPE_DROID, + "status": ROBOT_STATUS_UNAVAILABLE, + "availabilityDetail": {"code": "mock_robot.camera_video_required"}, + } + + async def run_forever(self) -> None: + delay_seconds = self.config.reconnect_initial_delay_ms / 1000 + max_delay_seconds = self.config.reconnect_max_delay_ms / 1000 + while True: + try: + await self.run_once() + delay_seconds = self.config.reconnect_initial_delay_ms / 1000 + except (ConnectionClosed, OSError) as exc: + LOGGER.warning("Mock Robot control connection lost: %s", exc) + await asyncio.sleep(delay_seconds) + delay_seconds = min(max_delay_seconds, delay_seconds * 2) + + async def run_once(self) -> None: + if self.camera is not None: + self.camera.validate() + async with connect(self.config.server_url) as websocket: + hello = make_envelope( + TYPE_CONNECTION_HELLO, + {"role": ROLE_ROBOT_DRIVER, "robotId": self.config.robot_id}, + robot_id=self.config.robot_id, + ) + await websocket.send(pack_envelope(hello)) + result = unpack_envelope(await websocket.recv()) + if result["type"] != TYPE_CONNECTION_HELLO_RESULT or not result["payload"].get("ok"): + raise RuntimeError(f"Mock Robot hello rejected: {result['payload']}") + + status_task = asyncio.create_task(self._status_loop(websocket)) + try: + async for frame in websocket: + if not isinstance(frame, bytes): + LOGGER.warning("Ignoring non-binary Ito control frame") + continue + await self.handle_message(websocket, unpack_envelope(frame)) + finally: + status_task.cancel() + await self._clear_session() + + async def _status_loop(self, websocket: Any) -> None: + while True: + await self.send_status(websocket) + await asyncio.sleep(self.config.status_interval_ms / 1000) + + async def send_status(self, websocket: Any) -> None: + await websocket.send( + pack_envelope( + make_envelope( + TYPE_ROBOT_STATUS, + self.status_payload(), + robot_id=self.config.robot_id, + session_id=self.session_id, + ) + ) + ) + + async def handle_message(self, websocket: Any, envelope: Mapping[str, Any]) -> None: + message_type = envelope["type"] + if message_type == TYPE_DRIVER_SESSION_START: + await self.handle_session_start(websocket, envelope) + elif message_type == TYPE_SESSION_END: + await self.handle_session_end(websocket, envelope) + elif message_type == TYPE_SESSION_ENDED: + LOGGER.info("Mock Robot session ended: %s", envelope["payload"]) + await self._clear_session() + elif message_type == TYPE_WEBRTC_OFFER: + await self.handle_webrtc_offer(websocket, envelope) + elif message_type == TYPE_WEBRTC_ANSWER: + await self.handle_webrtc_answer(envelope) + else: + LOGGER.info("Mock Robot ignoring unsupported message type %s", message_type) + + async def handle_session_start(self, websocket: Any, envelope: Mapping[str, Any]) -> None: + requested_session_id = envelope.get("sessionId") or envelope["payload"].get("sessionId") + if not isinstance(requested_session_id, str) or not requested_session_id: + await self._send_start_result( + websocket, + envelope, + result_error(DisplayReason(code="driver.session_start.invalid_session")), + ) + return + if self.session_id is not None: + await self._send_start_result( + websocket, + envelope, + result_error(DisplayReason(code="driver.session_start.already_active")), + ) + return + if self.camera is None: + await self._send_start_result( + websocket, + envelope, + result_error(DisplayReason(code="mock_robot.camera_video_required")), + ) + return + try: + self.camera.open() + except (OSError, ValueError) as exc: + LOGGER.error("Mock Robot camera input failed: %s", exc) + await self._send_start_result( + websocket, + envelope, + result_error( + DisplayReason( + code="mock_robot.camera_video_unavailable", + text=str(exc), + ) + ), + ) + return + + self.session_id = requested_session_id + self.session_config = dict(envelope["payload"].get("sessionConfig") or {}) + LOGGER.info("Mock Robot session started: %s", self.session_id) + await self._send_start_result( + websocket, + envelope, + result_ok({"sessionId": self.session_id}), + ) + await self._start_camera_media(websocket) + + async def handle_session_end(self, websocket: Any, envelope: Mapping[str, Any]) -> None: + ended_session_id = envelope.get("sessionId") or self.session_id + await self._clear_session() + await websocket.send( + pack_envelope( + make_envelope( + TYPE_SESSION_END_RESULT, + result_ok({"sessionId": ended_session_id}), + reply_to_message_id=envelope["messageId"], + robot_id=self.config.robot_id, + session_id=ended_session_id, + ) + ) + ) + + async def handle_webrtc_offer(self, websocket: Any, envelope: Mapping[str, Any]) -> None: + if envelope["payload"].get("path") != WEBRTC_PATH_PILOT_INPUT: + LOGGER.info("Mock Robot ignoring unsupported WebRTC path %s", envelope["payload"].get("path")) + return + session_id = envelope.get("sessionId") or self.session_id + sdp = envelope["payload"].get("sdp") + if not isinstance(session_id, str) or session_id != self.session_id or not isinstance(sdp, str): + LOGGER.warning("Mock Robot ignoring invalid pilot-input WebRTC offer") + return + if self.pilot_input_webrtc is None: + self.pilot_input_webrtc = PilotInputWebRtcReceiver(self.receive_pilot_input_snapshot) + answer_sdp = await self.pilot_input_webrtc.accept_offer(session_id=session_id, sdp=sdp) + await websocket.send( + pack_envelope( + make_envelope( + TYPE_WEBRTC_ANSWER, + {"path": WEBRTC_PATH_PILOT_INPUT, "sdp": answer_sdp}, + reply_to_message_id=envelope["messageId"], + robot_id=self.config.robot_id, + session_id=session_id, + ) + ) + ) + + async def handle_webrtc_answer(self, envelope: Mapping[str, Any]) -> None: + if envelope["payload"].get("path") != WEBRTC_PATH_CAMERA_MEDIA: + LOGGER.info("Mock Robot ignoring unsupported WebRTC answer path %s", envelope["payload"].get("path")) + return + session_id = envelope.get("sessionId") or self.session_id + sdp = envelope["payload"].get("sdp") + if not isinstance(session_id, str) or not isinstance(sdp, str): + LOGGER.warning("Mock Robot ignoring invalid camera-media WebRTC answer") + return + if self.camera_media_webrtc is None: + LOGGER.warning("Mock Robot received camera-media answer without an active publisher") + return + await self.camera_media_webrtc.accept_answer(session_id=session_id, sdp=sdp) + + async def _start_camera_media(self, websocket: Any) -> None: + if self.session_id is None or self.camera is None: + return + try: + if self.camera_media_webrtc is None: + self.camera_media_webrtc = CameraMediaWebRtcPublisher() + sdp = await self.camera_media_webrtc.create_offer( + session_id=self.session_id, + video_path=self.camera.path, + loop=self.camera.loop, + ) + except Exception as exc: + LOGGER.error("Mock Robot failed to start cameraMedia WebRTC: %s", exc) + return + await websocket.send( + pack_envelope( + make_envelope( + TYPE_WEBRTC_OFFER, + {"path": WEBRTC_PATH_CAMERA_MEDIA, "sdp": sdp}, + robot_id=self.config.robot_id, + session_id=self.session_id, + ) + ) + ) + + def receive_pilot_input_snapshot(self, snapshot: Mapping[str, Any]) -> None: + """Receive and log a Pilot Input Snapshot. + + The mock keeps no fake robot pose; stdout logging is the observable + behavior requested for end-to-end session/control testing. + """ + + payload = dict(snapshot) + LOGGER.info("pilot_input_snapshot %s", json.dumps(payload, sort_keys=True)) + + async def _send_start_result( + self, + websocket: Any, + request: Mapping[str, Any], + payload: Mapping[str, Any], + ) -> None: + session_id = request.get("sessionId") or request["payload"].get("sessionId") + await websocket.send( + pack_envelope( + make_envelope( + TYPE_DRIVER_SESSION_START_RESULT, + payload, + reply_to_message_id=request["messageId"], + robot_id=self.config.robot_id, + session_id=session_id if isinstance(session_id, str) else None, + ) + ) + ) + + async def _clear_session(self) -> None: + session_id = self.session_id + if self.camera is not None: + self.camera.close() + if self.pilot_input_webrtc is not None: + await self.pilot_input_webrtc.close_session(session_id) + if self.camera_media_webrtc is not None: + await self.camera_media_webrtc.close_session(session_id) + self.session_id = None + self.session_config = None + + +async def run(config: MockRobotConfig | None = None) -> None: + await MockRobotDriver(config or MockRobotConfig.from_env()).run_forever() + + +def main() -> None: + logging.basicConfig(level=logging.INFO, format="%(message)s") + asyncio.run(run()) + + +if __name__ == "__main__": + main() diff --git a/drivers/mock-robot/mock_robot/webrtc.py b/drivers/mock-robot/mock_robot/webrtc.py new file mode 100644 index 0000000..ae013be --- /dev/null +++ b/drivers/mock-robot/mock_robot/webrtc.py @@ -0,0 +1,170 @@ +"""Mock Robot WebRTC live-path helpers.""" + +from __future__ import annotations + +import asyncio +import json +import logging +from pathlib import Path +from typing import Any, Callable, Mapping + +LOGGER = logging.getLogger(__name__) + + +class PilotInputWebRtcReceiver: + """Accepts pilot-input WebRTC offers and forwards snapshots to a sink.""" + + def __init__(self, receive_snapshot: Callable[[Mapping[str, Any]], None]) -> None: + try: + from aiortc import RTCConfiguration, RTCPeerConnection, RTCSessionDescription + except ImportError as exc: # pragma: no cover - declared runtime dependency + raise RuntimeError("aiortc is required for Mock Robot WebRTC pilot input") from exc + self._configuration_type = RTCConfiguration + self._peer_connection_type = RTCPeerConnection + self._session_description_type = RTCSessionDescription + self._receive_snapshot = receive_snapshot + self._peer_connections: dict[str, Any] = {} + + async def accept_offer(self, *, session_id: str, sdp: str) -> str: + pc = self._peer_connection_type(configuration=self._configuration_type(iceServers=[])) + self._peer_connections[session_id] = pc + + @pc.on("datachannel") + def on_data_channel(channel: Any) -> None: + @channel.on("message") + def on_message(message: str | bytes) -> None: + try: + self._receive_snapshot(decode_pilot_input_snapshot(message)) + except ValueError as exc: + LOGGER.warning("Ignoring invalid Pilot Input Snapshot: %s", exc) + + offer = self._session_description_type(sdp=sdp, type="offer") + await pc.setRemoteDescription(offer) + answer = await pc.createAnswer() + await pc.setLocalDescription(answer) + await _wait_for_ice_gathering_complete(pc) + return pc.localDescription.sdp + + async def close_session(self, session_id: str | None) -> None: + if session_id is None: + return + pc = self._peer_connections.pop(session_id, None) + if pc is not None: + await pc.close() + + async def close_all(self) -> None: + peer_connections = list(self._peer_connections.values()) + self._peer_connections.clear() + await asyncio.gather(*(pc.close() for pc in peer_connections), return_exceptions=True) + + +class CameraMediaWebRtcPublisher: + """Publishes a video file to the server over the `cameraMedia` WebRTC path.""" + + def __init__(self) -> None: + try: + from aiortc import RTCConfiguration, RTCPeerConnection, RTCSessionDescription + from aiortc.contrib.media import MediaPlayer + except ImportError as exc: # pragma: no cover - declared runtime dependency + raise RuntimeError("aiortc is required for Mock Robot camera media") from exc + self._configuration_type = RTCConfiguration + self._peer_connection_type = RTCPeerConnection + self._session_description_type = RTCSessionDescription + self._media_player_type = MediaPlayer + self._peer_connections: dict[str, Any] = {} + self._players: dict[str, Any] = {} + + async def create_offer(self, *, session_id: str, video_path: str | Path, loop: bool) -> str: + pc = self._peer_connection_type(configuration=self._configuration_type(iceServers=[])) + player = self._media_player_type(str(video_path), loop=loop) + if player.video is None: + await pc.close() + raise RuntimeError(f"mock camera video has no video stream: {video_path}") + pc.addTrack(player.video) + self._prefer_h264(pc) + self._peer_connections[session_id] = pc + self._players[session_id] = player + offer = await pc.createOffer() + await pc.setLocalDescription(offer) + await _wait_for_ice_gathering_complete(pc) + return pc.localDescription.sdp + + async def accept_answer(self, *, session_id: str, sdp: str) -> None: + pc = self._peer_connections.get(session_id) + if pc is None: + raise RuntimeError(f"no cameraMedia peer connection for session {session_id}") + await pc.setRemoteDescription(self._session_description_type(sdp=sdp, type="answer")) + + async def close_session(self, session_id: str | None) -> None: + if session_id is None: + return + pc = self._peer_connections.pop(session_id, None) + player = self._players.pop(session_id, None) + if player is not None and player.video is not None: + player.video.stop() + if pc is not None: + await pc.close() + + async def close_all(self) -> None: + peer_connections = list(self._peer_connections.values()) + players = list(self._players.values()) + self._peer_connections.clear() + self._players.clear() + for player in players: + if player.video is not None: + player.video.stop() + await asyncio.gather(*(pc.close() for pc in peer_connections), return_exceptions=True) + + def _prefer_h264(self, pc: Any) -> None: + try: + from aiortc import RTCRtpSender + except ImportError: # pragma: no cover - already imported in __init__ + return + codecs = [ + codec + for codec in RTCRtpSender.getCapabilities("video").codecs + if codec.mimeType.lower() == "video/h264" + ] + if not codecs: + return + for transceiver in pc.getTransceivers(): + if transceiver.kind == "video": + transceiver.setCodecPreferences(codecs) + + +def decode_pilot_input_snapshot(message: str | bytes) -> dict[str, Any]: + if isinstance(message, bytes): + message = message.decode("utf-8") + try: + payload = json.loads(message) + except json.JSONDecodeError as exc: + raise ValueError("snapshot is not valid JSON") from exc + if not isinstance(payload, dict): + raise ValueError("snapshot must be a JSON object") + if payload.get("protocolVersion") != "ito.v1": + raise ValueError("snapshot protocolVersion must be ito.v1") + if not isinstance(payload.get("sessionId"), str): + raise ValueError("snapshot requires sessionId") + if not isinstance(payload.get("sequence"), int): + raise ValueError("snapshot requires integer sequence") + if not isinstance(payload.get("timestampMs"), (int, float)): + raise ValueError("snapshot requires timestampMs") + if not isinstance(payload.get("headsetYawRad"), (int, float)): + raise ValueError("snapshot requires headsetYawRad") + if not isinstance(payload.get("controllers"), dict): + raise ValueError("snapshot requires controllers") + return payload + + +async def _wait_for_ice_gathering_complete(peer_connection: Any) -> None: + if getattr(peer_connection, "iceGatheringState", None) == "complete": + return + + complete = asyncio.Event() + + @peer_connection.on("icegatheringstatechange") + def on_ice_gathering_state_change() -> None: + if peer_connection.iceGatheringState == "complete": + complete.set() + + await complete.wait() diff --git a/drivers/mock-robot/requirements.txt b/drivers/mock-robot/requirements.txt new file mode 100644 index 0000000..0a2cf08 --- /dev/null +++ b/drivers/mock-robot/requirements.txt @@ -0,0 +1,4 @@ +msgpack>=1.1,<2 +websockets>=15,<16 +aiortc>=1.9,<2 +av>=16,<17 diff --git a/server/README.md b/server/README.md index 1760160..631b3f0 100644 --- a/server/README.md +++ b/server/README.md @@ -2,5 +2,22 @@ The Ito Server coordinates Ito's shared server-side state and reconstruction. -This directory is currently a placeholder while Ito is reset around the v1 -design in `../docs/v1.md`. +## Dependencies + +The server uses `msgpack` and `websockets` for the Ito control plane. WebRTC and +H.264 camera decoding use `aiortc` and `av`/PyAV. + +Install local Python dependencies from the repository root: + +```sh +python -m pip install -r server/requirements.txt +``` + +Run tests from the repository root: + +```sh +pytest -q +``` + +Docker Compose commands for running the Ito Server with the static Pilot Client +and optional Mock Robot are documented in `../docs/local-v1.md`. diff --git a/server/ito/app.py b/server/ito/app.py index 8b7ed5c..ef3b37e 100644 --- a/server/ito/app.py +++ b/server/ito/app.py @@ -3,15 +3,17 @@ from __future__ import annotations import asyncio -from dataclasses import dataclass +from dataclasses import dataclass, field import logging from time import monotonic from typing import Any +from uuid import uuid4 from websockets.asyncio.server import ServerConnection, serve from websockets.exceptions import ConnectionClosed from .config import ServerConfig +from .media import AiortcCameraTrackReceiver from .protocol import ( DisplayReason, PROTOCOL_VERSION, @@ -25,22 +27,37 @@ ROBOT_TYPES, TYPE_CATALOG_GET, TYPE_CATALOG_GET_RESULT, + TYPE_DRIVER_SESSION_START, + TYPE_DRIVER_SESSION_START_RESULT, TYPE_SESSION_ACQUIRE, TYPE_SESSION_ACQUIRE_RESULT, TYPE_SESSION_END, TYPE_SESSION_END_RESULT, + TYPE_SESSION_ENDED, + TYPE_WEBRTC_ANSWER, + TYPE_WEBRTC_OFFER, TYPE_CONNECTION_HELLO, TYPE_CONNECTION_HELLO_RESULT, TYPE_ROBOT_STATUS, + WEBRTC_PATH_CAMERA_MEDIA, + WEBRTC_PATH_PILOT_INPUT, + WEBRTC_PATH_SPLAT_BATCHES, make_envelope, pack_envelope, result_error, result_ok, unpack_envelope, ) +from .reconstruction import ReconstructionSessionRuntime +from .webrtc import MissingWebRtcStack, ServerLivePathAcceptor, SplatBatchChannelRegistry +from server.processors.null import NullReconstructionProcessor LOGGER = logging.getLogger(__name__) +SESSION_STATE_STARTING = "starting" +SESSION_STATE_ACTIVE = "active" +SESSION_STATE_ENDED = "ended" + @dataclass(eq=False) class ConnectionState: @@ -88,25 +105,115 @@ def effective_status(self, now: float, watchdog_seconds: float) -> str: return self.driver_status +@dataclass +class SessionRecord: + session_id: str + robot_id: str + pilot_connection: ConnectionState | None + driver_connection: ConnectionState | None + session_config: dict[str, object] + state: str = SESSION_STATE_STARTING + created_at: float = field(default_factory=monotonic) + endpoint_missing_since: float | None = None + ended_reason: dict[str, str] | None = None + ended_by: str | None = None + clean: bool = False + + def note_endpoint_missing(self) -> None: + if self.endpoint_missing_since is None: + self.endpoint_missing_since = monotonic() + + def note_endpoint_present(self) -> None: + if self.pilot_connection is not None and self.driver_connection is not None: + self.endpoint_missing_since = None + + class ItoServer: def __init__(self, config: ServerConfig | None = None) -> None: self.config = config or ServerConfig.from_env() self.connections: set[ConnectionState] = set() self.drivers: dict[str, DriverRecord] = {} + self.sessions: dict[str, SessionRecord] = {} + self._pending_requests: dict[str, asyncio.Future[dict[str, Any]]] = {} + self._pending_webrtc_routes: dict[str, tuple[ConnectionState, str]] = {} + self.splat_channels = SplatBatchChannelRegistry() + self.live_paths: ServerLivePathAcceptor = MissingWebRtcStack() + self.reconstruction_runtimes: dict[str, ReconstructionSessionRuntime] = {} + self._acquisition_lock = asyncio.Lock() self._watchdog_task: asyncio.Task[None] | None = None + self._cleanup_task: asyncio.Task[None] | None = None + self._install_default_live_paths() @property def watchdog_seconds(self) -> float: return self.config.driver_status_watchdog_ms / 1000 + @property + def request_timeout_seconds(self) -> float: + return self.config.request_timeout_ms / 1000 + + @property + def session_cleanup_seconds(self) -> float: + return self.config.session_cleanup_timeout_ms / 1000 + + def _install_default_live_paths(self) -> None: + try: + from .webrtc import AiortcServerLivePaths + except ImportError: # pragma: no cover - module is local + return + try: + self.live_paths = AiortcServerLivePaths( + on_camera_track=self._accept_camera_track, + splat_channels=self.splat_channels, + ) + except RuntimeError: + self.live_paths = MissingWebRtcStack() + + def _accept_camera_track(self, track: object, session_id: str) -> None: + if getattr(track, "kind", None) != "video": + return + runtime = self._reconstruction_runtime(session_id) + receiver = AiortcCameraTrackReceiver(runtime.process_frame) + asyncio.create_task(receiver.consume(track)) + + def _reconstruction_runtime(self, session_id: str) -> ReconstructionSessionRuntime: + runtime = self.reconstruction_runtimes.get(session_id) + if runtime is not None: + return runtime + runtime = ReconstructionSessionRuntime( + session_id, + NullReconstructionProcessor(), + send_splat_batch=lambda payload: self.splat_channels.send(session_id, payload), + fail_session=lambda reason: asyncio.create_task( + self._fail_session_from_reconstruction(session_id, reason) + ), + ) + runtime.start() + self.reconstruction_runtimes[session_id] = runtime + return runtime + + async def _fail_session_from_reconstruction(self, session_id: str, reason: dict[str, str]) -> None: + session = self.sessions.get(session_id) + if session is None or session.state == SESSION_STATE_ENDED: + return + await self._end_session( + session, + reason=reason, + ended_by="server", + clean=False, + request_driver_end=True, + ) + async def serve_forever(self) -> None: LOGGER.info("Starting Ito Server on %s:%s", self.config.host, self.config.port) self._watchdog_task = asyncio.create_task(self._watchdog_loop()) + self._cleanup_task = asyncio.create_task(self._session_cleanup_loop()) try: async with serve(self._handle_connection, self.config.host, self.config.port): await asyncio.Future() finally: self._watchdog_task.cancel() + self._cleanup_task.cancel() async def _handle_connection(self, websocket: ServerConnection) -> None: state = ConnectionState(websocket=websocket) @@ -124,6 +231,7 @@ async def _handle_connection(self, websocket: ServerConnection) -> None: record.connection = None record.driver_status = ROBOT_STATUS_UNAVAILABLE record.availability_detail = {"code": "robot.unavailable.driver_disconnected"} + self._mark_connection_disappeared(state) async def _handle_frame(self, state: ConnectionState, frame: bytes | str) -> None: if not isinstance(frame, bytes): @@ -142,10 +250,20 @@ async def _handle_frame(self, state: ConnectionState, frame: bytes | str) -> Non if envelope["type"] == TYPE_CONNECTION_HELLO: await self._handle_hello(state, envelope) + elif envelope["type"] in {TYPE_DRIVER_SESSION_START_RESULT, TYPE_SESSION_END_RESULT}: + self._handle_response(envelope) elif envelope["type"] == TYPE_ROBOT_STATUS: self._handle_robot_status(state, envelope) elif envelope["type"] == TYPE_CATALOG_GET: await self._handle_catalog_get(state, envelope) + elif envelope["type"] == TYPE_SESSION_ACQUIRE: + await self._handle_session_acquire(state, envelope) + elif envelope["type"] == TYPE_SESSION_END: + await self._handle_session_end(state, envelope) + elif envelope["type"] == TYPE_WEBRTC_OFFER: + await self._handle_webrtc_offer(state, envelope) + elif envelope["type"] == TYPE_WEBRTC_ANSWER: + await self._handle_webrtc_answer(state, envelope) else: result_type = { TYPE_SESSION_ACQUIRE: TYPE_SESSION_ACQUIRE_RESULT, @@ -175,15 +293,57 @@ async def _handle_hello(self, state: ConnectionState, envelope: dict[str, Any]) LOGGER.error("Duplicate robotId reported: %s", robot_id) elif not record.duplicate: record.connection = state + session = self._active_session_for_robot(robot_id) + if session and session.driver_connection is None: + session.driver_connection = state + state.session_id = session.session_id + session.note_endpoint_present() await self._send_result(state, TYPE_CONNECTION_HELLO_RESULT, envelope["messageId"], result_ok({"protocolVersion": PROTOCOL_VERSION, "role": role})) return if role == ROLE_PILOT_CLIENT: + requested_session_id = payload.get("sessionId") + if requested_session_id is not None: + if not isinstance(requested_session_id, str): + await self._send_error(state, TYPE_CONNECTION_HELLO_RESULT, envelope["messageId"], "connection.invalid_session") + return + session = self.sessions.get(requested_session_id) + if session is None or session.state != SESSION_STATE_ACTIVE: + await self._send_error(state, TYPE_CONNECTION_HELLO_RESULT, envelope["messageId"], "session.resume_unavailable") + return + state.role = role + state.session_id = requested_session_id + session.pilot_connection = state + session.note_endpoint_present() + await self._send_result( + state, + TYPE_CONNECTION_HELLO_RESULT, + envelope["messageId"], + result_ok( + { + "protocolVersion": PROTOCOL_VERSION, + "role": role, + "sessionResumed": True, + "sessionConfig": session.session_config, + } + ), + ) + return state.role = role - state.session_id = payload.get("sessionId") - await self._send_result(state, TYPE_CONNECTION_HELLO_RESULT, envelope["messageId"], result_ok({"protocolVersion": PROTOCOL_VERSION, "role": role, "sessionResumed": False} if state.session_id else {"protocolVersion": PROTOCOL_VERSION, "role": role})) + await self._send_result(state, TYPE_CONNECTION_HELLO_RESULT, envelope["messageId"], result_ok({"protocolVersion": PROTOCOL_VERSION, "role": role})) return await self._send_error(state, TYPE_CONNECTION_HELLO_RESULT, envelope["messageId"], "connection.invalid_role") + def _handle_response(self, envelope: dict[str, Any]) -> None: + reply_to = envelope.get("replyToMessageId") + if not isinstance(reply_to, str): + LOGGER.warning("Ignoring response without replyToMessageId: %s", envelope["type"]) + return + pending = self._pending_requests.get(reply_to) + if pending is None or pending.done(): + LOGGER.warning("Ignoring response for unknown request: %s", reply_to) + return + pending.set_result(envelope) + def _handle_robot_status(self, state: ConnectionState, envelope: dict[str, Any]) -> None: if state.role != ROLE_ROBOT_DRIVER or not state.robot_id: LOGGER.warning("Ignoring robot.status from non-driver connection") @@ -214,12 +374,317 @@ async def _handle_catalog_get(self, state: ConnectionState, envelope: dict[str, robots = [r for r in robots if r["status"] != ROBOT_STATUS_UNAVAILABLE] await self._send_result(state, TYPE_CATALOG_GET_RESULT, envelope["messageId"], result_ok({"robots": robots})) + async def _handle_session_acquire(self, state: ConnectionState, envelope: dict[str, Any]) -> None: + if state.role != ROLE_PILOT_CLIENT: + await self._send_error(state, TYPE_SESSION_ACQUIRE_RESULT, envelope["messageId"], "session.acquire.pilot_client_required") + return + robot_id = envelope["payload"].get("robotId") or envelope.get("robotId") + if not isinstance(robot_id, str) or not robot_id: + await self._send_error(state, TYPE_SESSION_ACQUIRE_RESULT, envelope["messageId"], "session.acquire.robot_id_required") + return + + async with self._acquisition_lock: + record = self.drivers.get(robot_id) + now = monotonic() + if record is None or record.effective_status(now, self.watchdog_seconds) != ROBOT_STATUS_AVAILABLE or record.connection is None: + await self._send_error(state, TYPE_SESSION_ACQUIRE_RESULT, envelope["messageId"], "session.acquire.robot_unavailable") + return + + record.occupied = True + session_id = self._make_session_id() + session_config = self.config.session_config_payload() + session = SessionRecord( + session_id=session_id, + robot_id=robot_id, + pilot_connection=state, + driver_connection=record.connection, + session_config=session_config, + ) + self.sessions[session_id] = session + + start_result = await self._request_driver_session_start(record.connection, robot_id, session_id, session_config) + if not start_result["payload"].get("ok"): + self._release_failed_acquisition(session) + await self._send_result(state, TYPE_SESSION_ACQUIRE_RESULT, envelope["messageId"], start_result["payload"]) + return + + value = start_result["payload"].get("value", {}) + if value.get("sessionId") != session_id: + self._release_failed_acquisition(session) + await self._send_error(state, TYPE_SESSION_ACQUIRE_RESULT, envelope["messageId"], "driver.session_start.invalid_session") + return + + session.state = SESSION_STATE_ACTIVE + state.session_id = session_id + record.connection.session_id = session_id + await self._send_result( + state, + TYPE_SESSION_ACQUIRE_RESULT, + envelope["messageId"], + result_ok( + { + "sessionId": session_id, + "robotId": robot_id, + "sessionConfig": session_config, + } + ), + ) + + async def _request_driver_session_start( + self, + driver: ConnectionState, + robot_id: str, + session_id: str, + session_config: dict[str, object], + ) -> dict[str, Any]: + request = make_envelope( + TYPE_DRIVER_SESSION_START, + {"sessionId": session_id, "sessionConfig": session_config}, + robot_id=robot_id, + session_id=session_id, + ) + future: asyncio.Future[dict[str, Any]] = asyncio.get_running_loop().create_future() + self._pending_requests[request["messageId"]] = future + try: + await driver.websocket.send(pack_envelope(request)) + return await asyncio.wait_for(future, timeout=self.request_timeout_seconds) + except TimeoutError: + return make_envelope( + TYPE_DRIVER_SESSION_START_RESULT, + result_error(DisplayReason(code="request.timeout")), + reply_to_message_id=request["messageId"], + robot_id=robot_id, + session_id=session_id, + ) + finally: + self._pending_requests.pop(request["messageId"], None) + + def _release_failed_acquisition(self, session: SessionRecord) -> None: + self.sessions.pop(session.session_id, None) + record = self.drivers.get(session.robot_id) + if record: + record.occupied = False + if session.pilot_connection and session.pilot_connection.session_id == session.session_id: + session.pilot_connection.session_id = None + if session.driver_connection and session.driver_connection.session_id == session.session_id: + session.driver_connection.session_id = None + + async def _handle_session_end(self, state: ConnectionState, envelope: dict[str, Any]) -> None: + session_id = envelope.get("sessionId") or state.session_id + if not isinstance(session_id, str) or session_id not in self.sessions: + await self._send_error(state, TYPE_SESSION_END_RESULT, envelope["messageId"], "session.end.unknown_session") + return + session = self.sessions[session_id] + if session.state == SESSION_STATE_ENDED: + await self._send_result(state, TYPE_SESSION_END_RESULT, envelope["messageId"], result_ok({"sessionId": session_id})) + return + if state not in {session.pilot_connection, session.driver_connection}: + await self._send_error(state, TYPE_SESSION_END_RESULT, envelope["messageId"], "session.end.endpoint_required") + return + + reason = envelope["payload"].get("reason") + if not isinstance(reason, dict): + reason = {"code": "session.ended.requested"} + clean = bool(envelope["payload"].get("clean", False)) + ended_by = state.role or "server" + + await self._send_result(state, TYPE_SESSION_END_RESULT, envelope["messageId"], result_ok({"sessionId": session_id})) + await self._end_session(session, reason=reason, ended_by=ended_by, clean=clean, request_driver_end=state is not session.driver_connection) + + async def _handle_webrtc_offer(self, state: ConnectionState, envelope: dict[str, Any]) -> None: + session = self._session_for_live_path(state, envelope) + if session is None: + LOGGER.warning("Ignoring WebRTC offer for unknown or inactive session") + return + path = envelope["payload"]["path"] + if path == WEBRTC_PATH_PILOT_INPUT: + await self._relay_pilot_input_offer(state, session, envelope) + return + if path not in {WEBRTC_PATH_CAMERA_MEDIA, WEBRTC_PATH_SPLAT_BATCHES}: + LOGGER.warning("Ignoring unsupported WebRTC path %s", path) + return + if (path == WEBRTC_PATH_CAMERA_MEDIA and state is not session.driver_connection) or ( + path == WEBRTC_PATH_SPLAT_BATCHES and state is not session.pilot_connection + ): + LOGGER.warning("Ignoring WebRTC %s offer from wrong endpoint", path) + return + try: + answer_sdp = await self.live_paths.accept_offer( + path=path, + session_id=session.session_id, + sdp=envelope["payload"]["sdp"], + ) + except Exception as exc: + LOGGER.exception("WebRTC %s negotiation failed for session %s", path, session.session_id) + await self._end_session( + session, + reason={"code": "session.ended.reconstruction_failed", "text": str(exc)}, + ended_by="server", + clean=False, + request_driver_end=True, + ) + return + await state.websocket.send( + pack_envelope( + make_envelope( + TYPE_WEBRTC_ANSWER, + {"path": path, "sdp": answer_sdp}, + reply_to_message_id=envelope["messageId"], + robot_id=session.robot_id, + session_id=session.session_id, + ) + ) + ) + + async def _relay_pilot_input_offer( + self, + state: ConnectionState, + session: SessionRecord, + envelope: dict[str, Any], + ) -> None: + if state is not session.pilot_connection or session.driver_connection is None: + LOGGER.warning("Ignoring pilot-input WebRTC offer without pilot and driver endpoints") + return + forwarded = make_envelope( + TYPE_WEBRTC_OFFER, + envelope["payload"], + robot_id=session.robot_id, + session_id=session.session_id, + ) + self._pending_webrtc_routes[forwarded["messageId"]] = (state, envelope["messageId"]) + await session.driver_connection.websocket.send(pack_envelope(forwarded)) + + async def _handle_webrtc_answer(self, state: ConnectionState, envelope: dict[str, Any]) -> None: + reply_to = envelope.get("replyToMessageId") + if not isinstance(reply_to, str): + LOGGER.warning("Ignoring WebRTC answer without replyToMessageId") + return + route = self._pending_webrtc_routes.pop(reply_to, None) + if route is None: + LOGGER.warning("Ignoring WebRTC answer for unknown offer %s", reply_to) + return + destination, original_message_id = route + session = self._session_for_live_path(destination, envelope) + if session is None: + return + await destination.websocket.send( + pack_envelope( + make_envelope( + TYPE_WEBRTC_ANSWER, + envelope["payload"], + reply_to_message_id=original_message_id, + robot_id=session.robot_id, + session_id=session.session_id, + ) + ) + ) + + def _session_for_live_path( + self, state: ConnectionState, envelope: dict[str, Any] + ) -> SessionRecord | None: + session_id = envelope.get("sessionId") or state.session_id + if not isinstance(session_id, str): + return None + session = self.sessions.get(session_id) + if session is None or session.state != SESSION_STATE_ACTIVE: + return None + if state not in {session.pilot_connection, session.driver_connection}: + return None + return session + + async def _end_session( + self, + session: SessionRecord, + *, + reason: dict[str, str], + ended_by: str, + clean: bool, + request_driver_end: bool = True, + ) -> None: + if session.state == SESSION_STATE_ENDED: + return + session.state = SESSION_STATE_ENDED + session.ended_reason = reason + session.ended_by = ended_by + session.clean = clean + record = self.drivers.get(session.robot_id) + if record: + record.occupied = False + if session.pilot_connection and session.pilot_connection.session_id == session.session_id: + session.pilot_connection.session_id = None + if session.driver_connection and session.driver_connection.session_id == session.session_id: + session.driver_connection.session_id = None + runtime = self.reconstruction_runtimes.pop(session.session_id, None) + if runtime is not None: + runtime.close() + close_session = getattr(self.live_paths, "close_session", None) + if close_session is not None: + await close_session(session.session_id) + + if request_driver_end and session.driver_connection is not None: + await self._send_driver_session_end(session, reason, clean) + + ended_payload = {"reason": reason, "endedBy": ended_by, "clean": clean} + await self._send_session_ended(session.pilot_connection, session, ended_payload) + await self._send_session_ended(session.driver_connection, session, ended_payload) + + async def _send_driver_session_end(self, session: SessionRecord, reason: dict[str, str], clean: bool) -> None: + if session.driver_connection is None: + return + await session.driver_connection.websocket.send( + pack_envelope( + make_envelope( + TYPE_SESSION_END, + {"reason": reason, "clean": clean}, + robot_id=session.robot_id, + session_id=session.session_id, + ) + ) + ) + + async def _send_session_ended( + self, state: ConnectionState | None, session: SessionRecord, payload: dict[str, Any] + ) -> None: + if state is None: + return + await state.websocket.send( + pack_envelope( + make_envelope( + TYPE_SESSION_ENDED, + payload, + robot_id=session.robot_id, + session_id=session.session_id, + ) + ) + ) + async def _send_error(self, state: ConnectionState, message_type: str, reply_to: str | None, code: str) -> None: await self._send_result(state, message_type, reply_to, result_error(DisplayReason(code=code))) async def _send_result(self, state: ConnectionState, message_type: str, reply_to: str | None, payload: dict[str, Any]) -> None: await state.websocket.send(pack_envelope(make_envelope(message_type, payload, reply_to_message_id=reply_to, robot_id=state.robot_id, session_id=state.session_id))) + def _make_session_id(self) -> str: + return f"session-{uuid4()}" + + def _active_session_for_robot(self, robot_id: str) -> SessionRecord | None: + for session in self.sessions.values(): + if session.robot_id == robot_id and session.state != SESSION_STATE_ENDED: + return session + return None + + def _mark_connection_disappeared(self, state: ConnectionState) -> None: + for session in self.sessions.values(): + changed = False + if session.pilot_connection is state: + session.pilot_connection = None + changed = True + if session.driver_connection is state: + session.driver_connection = None + changed = True + if changed and session.state != SESSION_STATE_ENDED: + session.note_endpoint_missing() + async def _watchdog_loop(self) -> None: while True: await asyncio.sleep(self.watchdog_seconds / 2) @@ -229,6 +694,26 @@ async def _watchdog_loop(self) -> None: record.driver_status = ROBOT_STATUS_UNAVAILABLE record.availability_detail = {"code": "robot.unavailable.driver_status_timeout"} + async def _session_cleanup_loop(self) -> None: + while True: + await asyncio.sleep(self.session_cleanup_seconds / 2) + await self._cleanup_disappeared_endpoint_sessions() + + async def _cleanup_disappeared_endpoint_sessions(self) -> None: + now = monotonic() + for session in list(self.sessions.values()): + if session.state == SESSION_STATE_ENDED or session.endpoint_missing_since is None: + continue + if now - session.endpoint_missing_since >= self.session_cleanup_seconds: + await self._end_session( + session, + reason={"code": "session.ended.endpoint_disappeared"}, + ended_by="server", + clean=False, + request_driver_end=True, + ) + self.sessions.pop(session.session_id, None) + async def run(config: ServerConfig | None = None) -> None: await ItoServer(config).serve_forever() diff --git a/server/ito/media.py b/server/ito/media.py new file mode 100644 index 0000000..5c6e08c --- /dev/null +++ b/server/ito/media.py @@ -0,0 +1,83 @@ +"""Camera media decoding into reconstruction frames.""" + +from __future__ import annotations + +import asyncio +from collections.abc import Callable +from dataclasses import dataclass +from fractions import Fraction +from typing import Iterable + +from server.processors.base import ReconstructionFrame + + +@dataclass(frozen=True) +class EncodedCameraSample: + data: bytes + timestamp_ms: int + + +class H264CameraDecoder: + """Decode H.264 samples to RGB reconstruction frames using PyAV when present.""" + + def __init__(self) -> None: + try: + import av + except ImportError as exc: # pragma: no cover - depends on optional media stack + raise RuntimeError("PyAV is required for H.264 camera decoding") from exc + self._av = av + self._codec = av.CodecContext.create("h264", "r") + self._sequence = 0 + + def decode(self, sample: EncodedCameraSample) -> Iterable[ReconstructionFrame]: + packet = self._av.Packet(sample.data) + packet.pts = sample.timestamp_ms + packet.time_base = Fraction(1, 1000) + for decoded in self._codec.decode(packet): + rgb = decoded.to_rgb() + self._sequence += 1 + yield ReconstructionFrame( + data=bytes(rgb.planes[0]), + timestamp_ms=sample.timestamp_ms, + width=rgb.width, + height=rgb.height, + pixel_format="rgb24", + sequence=self._sequence, + ) + + +class AiortcCameraTrackReceiver: + """Consumes an aiortc video track into reconstruction frames.""" + + def __init__(self, process_frame: Callable[[ReconstructionFrame], None]) -> None: + self.process_frame = process_frame + self._sequence = 0 + + async def consume(self, track: object) -> None: + try: + from aiortc.mediastreams import MediaStreamError + except ImportError as exc: # pragma: no cover - optional media stack + raise RuntimeError("aiortc is required for camera media receiving") from exc + + while True: + try: + frame = await track.recv() + except MediaStreamError: + return + self.process_frame(self._reconstruction_frame(frame)) + await asyncio.sleep(0) + + def _reconstruction_frame(self, frame: object) -> ReconstructionFrame: + rgb = frame.to_rgb() + self._sequence += 1 + timestamp_ms = 0 + if getattr(frame, "pts", None) is not None and getattr(frame, "time_base", None) is not None: + timestamp_ms = int(float(frame.pts * frame.time_base) * 1000) + return ReconstructionFrame( + data=bytes(rgb.planes[0]), + timestamp_ms=timestamp_ms, + width=rgb.width, + height=rgb.height, + pixel_format="rgb24", + sequence=self._sequence, + ) diff --git a/server/ito/protocol.py b/server/ito/protocol.py index b510557..bdbdf50 100644 --- a/server/ito/protocol.py +++ b/server/ito/protocol.py @@ -25,6 +25,13 @@ TYPE_WEBRTC_OFFER = "webrtc.offer" TYPE_WEBRTC_ANSWER = "webrtc.answer" +WEBRTC_PATH_PILOT_INPUT = "pilotInput" +WEBRTC_PATH_CAMERA_MEDIA = "cameraMedia" +WEBRTC_PATH_SPLAT_BATCHES = "splatBatches" +WEBRTC_PATHS = frozenset( + {WEBRTC_PATH_PILOT_INPUT, WEBRTC_PATH_CAMERA_MEDIA, WEBRTC_PATH_SPLAT_BATCHES} +) + MESSAGE_TYPES = frozenset( { TYPE_CATALOG_GET, @@ -187,6 +194,15 @@ def validate_envelope(envelope: Mapping[str, Any]) -> None: for field in ("robotId", "sessionId"): if envelope.get(field) is not None and not isinstance(envelope.get(field), str): raise ProtocolError(f"{field} must be a string when present") + if envelope.get("type") in {TYPE_WEBRTC_OFFER, TYPE_WEBRTC_ANSWER}: + validate_webrtc_signal_payload(envelope["payload"]) + + +def validate_webrtc_signal_payload(payload: Mapping[str, Any]) -> None: + if payload.get("path") not in WEBRTC_PATHS: + raise ProtocolError(f"unknown WebRTC live path: {payload.get('path')!r}") + if not isinstance(payload.get("sdp"), str) or not payload["sdp"]: + raise ProtocolError("WebRTC signaling payload requires non-empty SDP") def pack_envelope(envelope: Mapping[str, Any]) -> bytes: diff --git a/server/ito/reconstruction.py b/server/ito/reconstruction.py new file mode 100644 index 0000000..5ce1777 --- /dev/null +++ b/server/ito/reconstruction.py @@ -0,0 +1,50 @@ +"""Session-scoped reconstruction runtime and failure isolation.""" + +from __future__ import annotations + +from collections.abc import Callable +import logging + +from server.processors.base import ReconstructionFrame, ReconstructionProcessor + +from .splat import encode_splat_batch + +LOGGER = logging.getLogger(__name__) + + +class ReconstructionSessionRuntime: + """Owns one processor instance for one piloting session.""" + + def __init__( + self, + session_id: str, + processor: ReconstructionProcessor, + *, + send_splat_batch: Callable[[bytes], None], + fail_session: Callable[[dict[str, str]], None], + ) -> None: + self.session_id = session_id + self.processor = processor + self.send_splat_batch = send_splat_batch + self.fail_session = fail_session + self.failed = False + + def start(self) -> None: + self.processor.start(self.session_id) + + def process_frame(self, frame: ReconstructionFrame) -> None: + if self.failed: + return + try: + for batch in self.processor.process_frame(frame): + self.send_splat_batch(encode_splat_batch(batch)) + except Exception: + LOGGER.exception("Reconstruction failed for session %s", self.session_id) + self.failed = True + self.fail_session({"code": "session.ended.reconstruction_failed"}) + + def close(self) -> None: + try: + self.processor.close() + except Exception: + LOGGER.exception("Reconstruction processor close failed for session %s", self.session_id) diff --git a/server/ito/splat.py b/server/ito/splat.py new file mode 100644 index 0000000..b30ae8a --- /dev/null +++ b/server/ito/splat.py @@ -0,0 +1,65 @@ +"""Ito v1 Splat Batch binary encoding.""" + +from __future__ import annotations + +from dataclasses import dataclass +import struct +from typing import Iterable + +from server.processors.base import GaussianSplat, ProcessorSplatBatch + +MAGIC = b"ITOSPLAT" +VERSION = 1 +HEADER = struct.Struct("<8sHHIIH6x") +RECORD = struct.Struct(" bytes: + splats = list(batch.splats) + payload = bytearray( + HEADER.pack(MAGIC, VERSION, flags, batch.sequence, len(splats), RECORD_STRIDE) + ) + for splat in splats: + payload.extend(_pack_splat(splat)) + return bytes(payload) + + +def decode_splat_batch_header(payload: bytes) -> SplatBatchHeader: + magic, version, flags, sequence, splat_count, stride = HEADER.unpack_from(payload) + if magic != MAGIC: + raise ValueError("invalid Ito Splat Batch magic") + if version != VERSION: + raise ValueError(f"unsupported Ito Splat Batch version: {version}") + if stride != RECORD_STRIDE: + raise ValueError(f"unsupported Ito Splat Batch record stride: {stride}") + expected_size = HEADER.size + splat_count * stride + if len(payload) != expected_size: + raise ValueError("Ito Splat Batch payload size does not match header") + return SplatBatchHeader(version, flags, sequence, splat_count, stride) + + +def _pack_splat(splat: GaussianSplat) -> bytes: + x, y, z = splat.position + sx, sy, sz = splat.scale + qx, qy, qz, qw = (_quantize_rotation(value) for value in splat.rotation) + r, g, b, a = (_clamp_u8(value) for value in splat.color) + return RECORD.pack(x, y, z, sx, sy, sz, qx, qy, qz, qw, r, g, b, a) + + +def _quantize_rotation(value: float) -> int: + clamped = max(-1.0, min(1.0, float(value))) + return int(round(clamped * 32767)) + + +def _clamp_u8(value: int) -> int: + return max(0, min(255, int(value))) diff --git a/server/ito/webrtc.py b/server/ito/webrtc.py new file mode 100644 index 0000000..c974b29 --- /dev/null +++ b/server/ito/webrtc.py @@ -0,0 +1,124 @@ +"""Server-side WebRTC live-path seams.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Awaitable, Callable, Protocol + +from .protocol import WEBRTC_PATH_CAMERA_MEDIA, WEBRTC_PATH_SPLAT_BATCHES + + +class ServerLivePathAcceptor(Protocol): + async def accept_offer(self, *, path: str, session_id: str, sdp: str) -> str: + ... + + +class MissingWebRtcStack: + async def accept_offer(self, *, path: str, session_id: str, sdp: str) -> str: + raise RuntimeError("aiortc is required for server-terminated WebRTC live paths") + + +class SplatBatchChannelRegistry: + """Tracks open server-to-client Splat Batch data channels by session.""" + + def __init__(self) -> None: + self.channels: dict[str, object] = {} + + def attach(self, session_id: str, data_channel: object) -> None: + self.channels[session_id] = data_channel + + def detach(self, session_id: str, data_channel: object | None = None) -> None: + if data_channel is None or self.channels.get(session_id) is data_channel: + self.channels.pop(session_id, None) + + def send(self, session_id: str, payload: bytes) -> bool: + channel = self.channels.get(session_id) + if channel is None or getattr(channel, "readyState", None) != "open": + return False + channel.send(payload) + return True + + +@dataclass +class AiortcServerLivePaths: + """Minimal aiortc-backed acceptor for server-terminated WebRTC paths. + + Camera media and Splat Batch transport are owned by the server. Production + reconstruction integration attaches track/data-channel handlers here. + """ + + on_camera_track: Callable[[object, str], Awaitable[None] | None] | None = None + on_splat_channel: Callable[[object, str], Awaitable[None] | None] | None = None + splat_channels: SplatBatchChannelRegistry | None = None + + def __post_init__(self) -> None: + try: + from aiortc import RTCConfiguration, RTCPeerConnection, RTCSessionDescription + except ImportError as exc: # pragma: no cover - optional runtime dependency + raise RuntimeError("aiortc is required for WebRTC live paths") from exc + self._configuration_type = RTCConfiguration + self._peer_connection_type = RTCPeerConnection + self._session_description_type = RTCSessionDescription + self.peer_connections: dict[tuple[str, str], object] = {} + + async def accept_offer(self, *, path: str, session_id: str, sdp: str) -> str: + if path not in {WEBRTC_PATH_CAMERA_MEDIA, WEBRTC_PATH_SPLAT_BATCHES}: + raise ValueError(f"server cannot terminate WebRTC path {path}") + pc = self._peer_connection_type(configuration=self._configuration_type(iceServers=[])) + self.peer_connections[(session_id, path)] = pc + + if path == WEBRTC_PATH_CAMERA_MEDIA and self.on_camera_track is not None: + @pc.on("track") + async def on_track(track: object) -> None: + result = self.on_camera_track(track, session_id) + if result is not None: + await result + + if path == WEBRTC_PATH_SPLAT_BATCHES: + channel = pc.createDataChannel("ito.splatBatches", ordered=True) + if self.splat_channels is not None: + @channel.on("open") + def on_open() -> None: + self.splat_channels.attach(session_id, channel) + + @channel.on("close") + def on_close() -> None: + self.splat_channels.detach(session_id, channel) + if self.on_splat_channel is not None: + result = self.on_splat_channel(channel, session_id) + if result is not None: + await result + + offer = self._session_description_type(sdp=sdp, type="offer") + await pc.setRemoteDescription(offer) + answer = await pc.createAnswer() + await pc.setLocalDescription(answer) + await _wait_for_ice_gathering_complete(pc) + return pc.localDescription.sdp + + async def close_session(self, session_id: str) -> None: + import asyncio + + peers = [ + self.peer_connections.pop(key) + for key in list(self.peer_connections) + if key[0] == session_id + ] + if self.splat_channels is not None: + self.splat_channels.detach(session_id) + await asyncio.gather(*(pc.close() for pc in peers), return_exceptions=True) + + +async def _wait_for_ice_gathering_complete(peer_connection: object) -> None: + if getattr(peer_connection, "iceGatheringState", None) == "complete": + return + import asyncio + + complete = asyncio.Event() + + @peer_connection.on("icegatheringstatechange") + def on_ice_gathering_state_change() -> None: + if peer_connection.iceGatheringState == "complete": + complete.set() + + await complete.wait() diff --git a/server/processors/README.md b/server/processors/README.md index c33629a..18a2d98 100644 --- a/server/processors/README.md +++ b/server/processors/README.md @@ -4,3 +4,8 @@ This directory contains server-internal reconstruction algorithm modules. These modules are part of the Ito Server codebase, not separately deployed Ito programs. + +All processors implement the interface in `base.py`: start a session, accept +decoded `ReconstructionFrame` values, and yield `ProcessorSplatBatch` values. +`null.py` is only an integration seam used before MASt3R-SLAM or MonoGS is +selected; it is not the v1 reconstruction algorithm. diff --git a/server/processors/base.py b/server/processors/base.py new file mode 100644 index 0000000..2d2aaeb --- /dev/null +++ b/server/processors/base.py @@ -0,0 +1,53 @@ +"""Server-internal reconstruction processor interface.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Iterable, Protocol, Sequence + + +CAPTURE_MODALITY_MONOCULAR_RGB = "monocularRgb" + + +@dataclass(frozen=True) +class ReconstructionFrame: + """Decoded camera frame passed from server media ingress to reconstruction.""" + + data: bytes + timestamp_ms: int + width: int + height: int + pixel_format: str = "rgb24" + sequence: int | None = None + + +@dataclass(frozen=True) +class GaussianSplat: + position: tuple[float, float, float] + scale: tuple[float, float, float] + rotation: tuple[float, float, float, float] + color: tuple[int, int, int, int] + + +@dataclass(frozen=True) +class ProcessorSplatBatch: + sequence: int + splats: Sequence[GaussianSplat] + + +class ReconstructionProcessor(Protocol): + """Common interface for algorithms under server/processors/.""" + + capture_modality: str + + def start(self, session_id: str) -> None: + ... + + def process_frame(self, frame: ReconstructionFrame) -> Iterable[ProcessorSplatBatch]: + ... + + def reset(self) -> None: + ... + + def close(self) -> None: + ... diff --git a/server/processors/null.py b/server/processors/null.py new file mode 100644 index 0000000..c174ad3 --- /dev/null +++ b/server/processors/null.py @@ -0,0 +1,40 @@ +"""Null reconstruction processor used until the v1 monocular path is selected.""" + +from __future__ import annotations + +from typing import Iterable + +from .base import ( + CAPTURE_MODALITY_MONOCULAR_RGB, + ProcessorSplatBatch, + ReconstructionFrame, +) + + +class NullReconstructionProcessor: + """Consumes frames and emits no splats. + + This is a local integration seam, not the selected v1 algorithm. It lets the + server exercise media ingress, session failure handling, and Splat Batch + encoding without claiming MASt3R-SLAM or MonoGS have been selected. + """ + + capture_modality = CAPTURE_MODALITY_MONOCULAR_RGB + + def __init__(self) -> None: + self.session_id: str | None = None + self.frame_count = 0 + + def start(self, session_id: str) -> None: + self.session_id = session_id + self.frame_count = 0 + + def process_frame(self, frame: ReconstructionFrame) -> Iterable[ProcessorSplatBatch]: + self.frame_count += 1 + return [] + + def reset(self) -> None: + self.frame_count = 0 + + def close(self) -> None: + self.session_id = None diff --git a/server/requirements.txt b/server/requirements.txt index 01fe653..0a2cf08 100644 --- a/server/requirements.txt +++ b/server/requirements.txt @@ -1,2 +1,4 @@ msgpack>=1.1,<2 websockets>=15,<16 +aiortc>=1.9,<2 +av>=16,<17 diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..0d95272 --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,8 @@ +"""Test import path setup for repository-local packages.""" + +from pathlib import Path +import sys + +ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(ROOT)) + diff --git a/tests/test_ito_droid.py b/tests/test_ito_droid.py new file mode 100644 index 0000000..51a1cd7 --- /dev/null +++ b/tests/test_ito_droid.py @@ -0,0 +1,286 @@ +import asyncio +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +DROID_DRIVER_ROOT = ROOT / "drivers" / "ito-droid" +sys.path.insert(0, str(DROID_DRIVER_ROOT)) + +from ito_droid.config import ItoDroidConfig +from ito_droid.control import CameraPanController +from ito_droid.driver import ItoDroidDriver +from ito_droid.media import CameraMediaPublisher +from ito_droid.ros_io import CameraFrame +from ito_droid.webrtc import PilotInputDataChannelReceiver, decode_pilot_input_snapshot +from server.ito.protocol import ( + TYPE_DRIVER_SESSION_START, + TYPE_DRIVER_SESSION_START_RESULT, + TYPE_SESSION_END, + TYPE_SESSION_END_RESULT, + make_envelope, + unpack_envelope, +) + + +class FakeClock: + def __init__(self, now=0.0): + self.now = now + + def __call__(self): + return self.now + + def advance(self, seconds): + self.now += seconds + + +class RecordingServo: + def __init__(self): + self.angles = [] + + def publish_angle(self, angle_degrees): + self.angles.append(angle_degrees) + + +class FakeWebSocket: + def __init__(self): + self.sent = [] + + async def send(self, frame): + self.sent.append(unpack_envelope(frame)) + + +def test_config_reads_environment(monkeypatch): + monkeypatch.setenv("ITO_SERVER_URL", "ws://server.example/ws") + monkeypatch.setenv("ITO_DROID_ROBOT_ID", "droid-a") + monkeypatch.setenv("ITO_DROID_ROS_CAMERA_TOPIC", "/camera/image") + monkeypatch.setenv("ITO_DROID_CONTROL_TICK_HZ", "30") + monkeypatch.setenv("ITO_DROID_SERVO_MIN_DEGREES", "10") + + config = ItoDroidConfig.from_env() + + assert config.server_url == "ws://server.example/ws" + assert config.robot_id == "droid-a" + assert config.ros_camera_topic == "/camera/image" + assert config.control_tick_hz == 30 + assert config.servo_min_degrees == 10 + + +def test_yaw_to_camera_pan_mapping_clamps_to_servo_limits(): + config = ItoDroidConfig( + servo_neutral_degrees=90, + servo_min_degrees=60, + servo_max_degrees=120, + yaw_to_servo_degrees_per_radian=30, + ) + controller = CameraPanController(config) + + assert controller.target_for_yaw(0) == 90 + assert controller.target_for_yaw(1) == 120 + assert controller.target_for_yaw(-2) == 60 + + +def test_control_tick_uses_newest_snapshot_and_holds_on_timeout(): + config = ItoDroidConfig( + pilot_input_timeout_ms=100, + servo_smoothing=1, + servo_max_velocity_degrees_per_second=1000, + ) + clock = FakeClock() + controller = CameraPanController(config) + + controller.receive_snapshot({"headsetYawRadians": 0.5}, clock()) + angle = controller.tick(clock(), 1 / 60) + assert angle > config.servo_neutral_degrees + + held_angle = angle + clock.advance(0.101) + assert controller.tick(clock(), 1 / 60) == held_angle + + +def test_safe_resumption_ramps_correction_velocity_after_timeout(): + config = ItoDroidConfig( + pilot_input_timeout_ms=100, + servo_smoothing=1, + servo_max_velocity_degrees_per_second=100, + resumption_initial_velocity_degrees_per_second=10, + resumption_ramp_duration_ms=1000, + ) + clock = FakeClock() + controller = CameraPanController(config) + + controller.receive_snapshot({"headsetYawRadians": 0}, clock()) + assert controller.tick(clock(), 0.1) == config.servo_neutral_degrees + + clock.advance(0.101) + assert controller.tick(clock(), 0.1) == config.servo_neutral_degrees + + controller.receive_snapshot({"headsetYawRadians": 1}, clock()) + resumed_angle = controller.tick(clock(), 0.1) + assert resumed_angle == config.servo_neutral_degrees + 1 + + clock.advance(1.0) + controller.receive_snapshot({"headsetYawRadians": 1}, clock()) + later_angle = controller.tick(clock(), 0.1) + assert later_angle > resumed_angle + 1 + + +def test_status_reports_unavailable_until_camera_feed_arrives(): + driver = ItoDroidDriver(ItoDroidConfig()) + + assert driver.status_payload() == { + "name": "Ito Droid", + "type": "Droid", + "status": "Unavailable", + "availabilityDetail": {"code": "ito_droid.camera_feed_missing"}, + } + + driver.receive_camera_frame(CameraFrame(b"rgb", 1.0, encoding="rgb8", width=1, height=1)) + + assert driver.status_payload() == { + "name": "Ito Droid", + "type": "Droid", + "status": "Available", + } + + +def test_ros_camera_frames_flow_to_camera_media_publisher(): + publisher = CameraMediaPublisher() + driver = ItoDroidDriver(ItoDroidConfig(), media_publisher=publisher) + + publisher.start("session-1") + frame = CameraFrame(b"frame", 1.0, encoding="rgb8", width=2, height=2) + driver.receive_camera_frame(frame) + + assert publisher.frame_count == 1 + assert publisher.last_frame == frame + + +def test_session_start_neutralizes_servo_and_starts_media(): + asyncio.run(_session_start_neutralizes_servo_and_starts_media()) + + +async def _session_start_neutralizes_servo_and_starts_media(): + servo = RecordingServo() + media = CameraMediaPublisher() + driver = ItoDroidDriver(ItoDroidConfig(), servo_publisher=servo, media_publisher=media) + driver.camera_ready = True + websocket = FakeWebSocket() + + await driver.handle_session_start( + websocket, + make_envelope( + TYPE_DRIVER_SESSION_START, + {"sessionId": "session-1", "sessionConfig": {"cameraMedia": {"codec": "H264"}}}, + message_id="start-1", + robot_id="ito-droid-1", + session_id="session-1", + ), + ) + + assert driver.session_id == "session-1" + assert servo.angles == [driver.config.servo_neutral_degrees] + assert media.started_session_id == "session-1" + assert websocket.sent[-1]["type"] == TYPE_DRIVER_SESSION_START_RESULT + assert websocket.sent[-1]["replyToMessageId"] == "start-1" + assert websocket.sent[-1]["payload"] == {"ok": True, "value": {"sessionId": "session-1"}} + + +def test_session_start_fails_without_camera_feed(): + asyncio.run(_session_start_fails_without_camera_feed()) + + +async def _session_start_fails_without_camera_feed(): + driver = ItoDroidDriver(ItoDroidConfig()) + websocket = FakeWebSocket() + + await driver.handle_session_start( + websocket, + make_envelope( + TYPE_DRIVER_SESSION_START, + {"sessionId": "session-1", "sessionConfig": {}}, + message_id="start-1", + robot_id="ito-droid-1", + session_id="session-1", + ), + ) + + assert websocket.sent[-1]["type"] == TYPE_DRIVER_SESSION_START_RESULT + assert websocket.sent[-1]["payload"] == { + "ok": False, + "reason": {"code": "ito_droid.camera_feed_missing"}, + } + + +def test_clean_session_end_neutralizes_servo_and_stops_media(): + asyncio.run(_clean_session_end_neutralizes_servo_and_stops_media()) + + +def test_pilot_input_data_channel_receiver_decodes_snapshot_json(): + received = [] + + class FakeDataChannel: + def on(self, event): + assert event == "message" + + def register(callback): + self.callback = callback + return callback + + return register + + channel = FakeDataChannel() + receiver = PilotInputDataChannelReceiver(received.append) + receiver.attach(channel) + channel.callback( + b'{"protocolVersion":"ito.v1","sessionId":"session-1","sequence":1,"headsetYawRad":0.25}' + ) + + assert received == [ + { + "protocolVersion": "ito.v1", + "sessionId": "session-1", + "sequence": 1, + "headsetYawRad": 0.25, + } + ] + assert decode_pilot_input_snapshot( + '{"protocolVersion":"ito.v1","sessionId":"session-1","sequence":2,"headsetYawRad":0}' + )["sequence"] == 2 + + +async def _clean_session_end_neutralizes_servo_and_stops_media(): + servo = RecordingServo() + media = CameraMediaPublisher() + driver = ItoDroidDriver(ItoDroidConfig(), servo_publisher=servo, media_publisher=media) + driver.camera_ready = True + websocket = FakeWebSocket() + + await driver.handle_session_start( + websocket, + make_envelope( + TYPE_DRIVER_SESSION_START, + {"sessionId": "session-1", "sessionConfig": {}}, + message_id="start-1", + robot_id="ito-droid-1", + session_id="session-1", + ), + ) + driver.receive_pilot_input_snapshot({"headsetYawRadians": 0.5}) + driver.process_control_tick(1 / 60) + + await driver.handle_session_end( + websocket, + make_envelope( + TYPE_SESSION_END, + {"reason": {"code": "session.ended.pilot_requested"}, "clean": True}, + message_id="end-1", + robot_id="ito-droid-1", + session_id="session-1", + ), + ) + + assert driver.session_id is None + assert media.started_session_id is None + assert servo.angles[-1] == driver.config.servo_neutral_degrees + assert websocket.sent[-1]["type"] == TYPE_SESSION_END_RESULT + assert websocket.sent[-1]["payload"] == {"ok": True, "value": {"sessionId": "session-1"}} diff --git a/tests/test_mock_robot.py b/tests/test_mock_robot.py new file mode 100644 index 0000000..2c04f8d --- /dev/null +++ b/tests/test_mock_robot.py @@ -0,0 +1,173 @@ +import asyncio +import importlib.util +import logging +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +MOCK_DRIVER_ROOT = ROOT / "drivers" / "mock-robot" +sys.path.insert(0, str(MOCK_DRIVER_ROOT)) + +from mock_robot.camera import VideoFileCamera +from mock_robot.config import MockRobotConfig +from mock_robot.driver import MockRobotDriver +from server.ito.protocol import ( + TYPE_DRIVER_SESSION_START, + TYPE_DRIVER_SESSION_START_RESULT, + TYPE_SESSION_END, + TYPE_SESSION_END_RESULT, + TYPE_WEBRTC_OFFER, + WEBRTC_PATH_CAMERA_MEDIA, + make_envelope, + unpack_envelope, +) + + +class FakeWebSocket: + def __init__(self): + self.sent = [] + + async def send(self, frame): + self.sent.append(unpack_envelope(frame)) + + +class FakeCameraMediaPublisher: + def __init__(self): + self.offers = [] + self.answers = [] + self.closed = [] + + async def create_offer(self, *, session_id, video_path, loop): + self.offers.append({"sessionId": session_id, "videoPath": str(video_path), "loop": loop}) + return "fake-camera-offer" + + async def accept_answer(self, *, session_id, sdp): + self.answers.append({"sessionId": session_id, "sdp": sdp}) + + async def close_session(self, session_id): + self.closed.append(session_id) + + +def test_mock_robot_imports_without_websocket_side_effects(): + spec = importlib.util.spec_from_file_location( + "mock_robot_main", + MOCK_DRIVER_ROOT / "main.py", + ) + assert spec is not None + + +def test_status_requires_camera_video_for_availability(): + driver = MockRobotDriver(MockRobotConfig(camera_video_path=None)) + + assert driver.status_payload() == { + "name": "Mock Robot", + "type": "Droid", + "status": "Unavailable", + "availabilityDetail": {"code": "mock_robot.camera_video_required"}, + } + + +def test_video_file_camera_reads_samples(tmp_path): + video = tmp_path / "camera.h264" + video.write_bytes(b"abcdef") + camera = VideoFileCamera(video, chunk_size=4, loop=False) + + samples = list(camera.samples()) + + assert [sample.data for sample in samples] == [b"abcd", b"ef"] + assert [sample.offset for sample in samples] == [0, 4] + + +def test_session_lifecycle_opens_and_closes_camera(tmp_path): + asyncio.run(_session_lifecycle_opens_and_closes_camera(tmp_path)) + + +async def _session_lifecycle_opens_and_closes_camera(tmp_path): + video = tmp_path / "camera.h264" + video.write_bytes(b"frame-data") + publisher = FakeCameraMediaPublisher() + driver = MockRobotDriver( + MockRobotConfig(camera_video_path=str(video), camera_loop=False), + camera_media_webrtc=publisher, + ) + websocket = FakeWebSocket() + + await driver.handle_session_start( + websocket, + make_envelope( + TYPE_DRIVER_SESSION_START, + {"sessionId": "session-1", "sessionConfig": {"pilotInputDataChannel": {"ordered": False}}}, + message_id="start-1", + robot_id="mock-robot-1", + session_id="session-1", + ), + ) + + assert driver.session_id == "session-1" + assert driver.camera is not None + assert driver.camera.is_open + assert websocket.sent[-2]["type"] == TYPE_DRIVER_SESSION_START_RESULT + assert websocket.sent[-2]["replyToMessageId"] == "start-1" + assert websocket.sent[-2]["payload"] == {"ok": True, "value": {"sessionId": "session-1"}} + assert websocket.sent[-1]["type"] == TYPE_WEBRTC_OFFER + assert websocket.sent[-1]["payload"]["path"] == WEBRTC_PATH_CAMERA_MEDIA + assert websocket.sent[-1]["payload"]["sdp"] == "fake-camera-offer" + assert publisher.offers == [{"sessionId": "session-1", "videoPath": str(video), "loop": False}] + + await driver.handle_session_end( + websocket, + make_envelope( + TYPE_SESSION_END, + {"reason": {"code": "session.ended.pilot_requested"}, "clean": True}, + message_id="end-1", + robot_id="mock-robot-1", + session_id="session-1", + ), + ) + + assert driver.session_id is None + assert not driver.camera.is_open + assert websocket.sent[-1]["type"] == TYPE_SESSION_END_RESULT + assert websocket.sent[-1]["payload"] == {"ok": True, "value": {"sessionId": "session-1"}} + assert publisher.closed == ["session-1"] + + +def test_session_start_fails_without_camera_video(): + asyncio.run(_session_start_fails_without_camera_video()) + + +async def _session_start_fails_without_camera_video(): + driver = MockRobotDriver(MockRobotConfig(camera_video_path=None)) + websocket = FakeWebSocket() + + await driver.handle_session_start( + websocket, + make_envelope( + TYPE_DRIVER_SESSION_START, + {"sessionId": "session-1", "sessionConfig": {}}, + message_id="start-1", + robot_id="mock-robot-1", + session_id="session-1", + ), + ) + + assert websocket.sent[-1]["type"] == TYPE_DRIVER_SESSION_START_RESULT + assert websocket.sent[-1]["payload"] == { + "ok": False, + "reason": {"code": "mock_robot.camera_video_required"}, + } + + +def test_pilot_input_snapshot_is_logged(caplog): + driver = MockRobotDriver(MockRobotConfig(camera_video_path=None)) + + with caplog.at_level(logging.INFO): + driver.receive_pilot_input_snapshot( + { + "headsetYawRadians": 0.25, + "controllers": {"right": {"triggerPressed": True}}, + } + ) + + assert "pilot_input_snapshot" in caplog.text + assert '"headsetYawRadians": 0.25' in caplog.text diff --git a/tests/test_mock_robot_e2e.py b/tests/test_mock_robot_e2e.py new file mode 100644 index 0000000..75f9bb1 --- /dev/null +++ b/tests/test_mock_robot_e2e.py @@ -0,0 +1,246 @@ +import asyncio +import json +import logging +import sys +from contextlib import suppress +from pathlib import Path + +import pytest +from websockets.asyncio.client import connect +from websockets.asyncio.server import serve + +aiortc = pytest.importorskip("aiortc") +av = pytest.importorskip("av") +RTCConfiguration = aiortc.RTCConfiguration +RTCPeerConnection = aiortc.RTCPeerConnection +RTCSessionDescription = aiortc.RTCSessionDescription + +ROOT = Path(__file__).resolve().parents[1] +MOCK_DRIVER_ROOT = ROOT / "drivers" / "mock-robot" +sys.path.insert(0, str(MOCK_DRIVER_ROOT)) + +from mock_robot.config import MockRobotConfig +from mock_robot.driver import MockRobotDriver +from server.ito.app import ItoServer +from server.ito.config import ServerConfig +from server.ito.protocol import ( + ROLE_PILOT_CLIENT, + TYPE_CATALOG_GET, + TYPE_CATALOG_GET_RESULT, + TYPE_CONNECTION_HELLO, + TYPE_CONNECTION_HELLO_RESULT, + TYPE_SESSION_ACQUIRE, + TYPE_SESSION_ACQUIRE_RESULT, + TYPE_SESSION_END, + TYPE_WEBRTC_ANSWER, + TYPE_WEBRTC_OFFER, + WEBRTC_PATH_PILOT_INPUT, + make_envelope, + pack_envelope, + unpack_envelope, +) + + +def test_mock_robot_e2e_acquire_and_pilot_input_over_websocket_and_webrtc(tmp_path, caplog): + asyncio.run(_mock_robot_e2e_acquire_and_pilot_input_over_websocket_and_webrtc(tmp_path, caplog)) + + +async def _mock_robot_e2e_acquire_and_pilot_input_over_websocket_and_webrtc(tmp_path, caplog): + video = tmp_path / "camera.mp4" + _write_h264_sample_video(video) + server = ItoServer( + ServerConfig( + host="127.0.0.1", + port=0, + request_timeout_ms=3000, + driver_status_watchdog_ms=1000, + session_cleanup_timeout_ms=1000, + ) + ) + + async with serve(server._handle_connection, "127.0.0.1", 0) as websocket_server: + port = websocket_server.sockets[0].getsockname()[1] + server_url = f"ws://127.0.0.1:{port}" + driver = MockRobotDriver( + MockRobotConfig( + server_url=server_url, + robot_id="mock-robot-1", + status_interval_ms=50, + camera_video_path=str(video), + camera_loop=False, + ) + ) + driver_task = asyncio.create_task(driver.run_once()) + peer_connection = RTCPeerConnection(configuration=RTCConfiguration(iceServers=[])) + try: + with caplog.at_level(logging.INFO, logger="mock_robot.driver"): + async with connect(server_url) as pilot_ws: + await _send( + pilot_ws, + make_envelope( + TYPE_CONNECTION_HELLO, + {"role": ROLE_PILOT_CLIENT}, + message_id="pilot-hello", + ), + ) + hello = await _recv_type(pilot_ws, TYPE_CONNECTION_HELLO_RESULT, "pilot-hello") + assert hello["payload"]["ok"] is True + + await _wait_for_mock_robot_available(pilot_ws) + await _send( + pilot_ws, + make_envelope( + TYPE_SESSION_ACQUIRE, + {"robotId": "mock-robot-1"}, + message_id="acquire-mock", + robot_id="mock-robot-1", + ), + ) + acquired = await _recv_type(pilot_ws, TYPE_SESSION_ACQUIRE_RESULT, "acquire-mock") + assert acquired["payload"]["ok"] is True + session_id = acquired["payload"]["value"]["sessionId"] + await _wait_for_camera_frame(server, session_id) + + data_channel = peer_connection.createDataChannel( + "ito.pilotInput", + ordered=False, + maxRetransmits=0, + ) + offer = await peer_connection.createOffer() + await peer_connection.setLocalDescription(offer) + await _wait_for_ice_gathering_complete(peer_connection) + await _send( + pilot_ws, + make_envelope( + TYPE_WEBRTC_OFFER, + {"path": WEBRTC_PATH_PILOT_INPUT, "sdp": peer_connection.localDescription.sdp}, + message_id="pilot-input-offer", + robot_id="mock-robot-1", + session_id=session_id, + ), + ) + answer = await _recv_type(pilot_ws, TYPE_WEBRTC_ANSWER, "pilot-input-offer") + assert answer["payload"]["path"] == WEBRTC_PATH_PILOT_INPUT + await peer_connection.setRemoteDescription( + RTCSessionDescription(sdp=answer["payload"]["sdp"], type="answer") + ) + await _wait_for_data_channel_open(data_channel) + + snapshot = { + "protocolVersion": "ito.v1", + "sessionId": session_id, + "sequence": 1, + "timestampMs": 12345, + "headsetYawRad": 0.42, + "controllers": {"left": {}, "right": {"triggerPressed": True}}, + } + data_channel.send(json.dumps(snapshot)) + await _wait_for_log(caplog, '"headsetYawRad": 0.42') + + await _send( + pilot_ws, + make_envelope( + TYPE_SESSION_END, + {"reason": {"code": "session.ended.pilot_requested"}, "clean": True}, + message_id="end-mock", + robot_id="mock-robot-1", + session_id=session_id, + ), + ) + await _recv_type(pilot_ws, "session.end.result", "end-mock") + finally: + await peer_connection.close() + driver_task.cancel() + with suppress(asyncio.CancelledError): + await driver_task + + +async def _wait_for_mock_robot_available(pilot_ws): + for attempt in range(20): + message_id = f"catalog-{attempt}" + await _send( + pilot_ws, + make_envelope(TYPE_CATALOG_GET, {"includeUnavailable": True}, message_id=message_id), + ) + catalog = await _recv_type(pilot_ws, TYPE_CATALOG_GET_RESULT, message_id) + robots = catalog["payload"]["value"]["robots"] + if robots and robots[0]["robotId"] == "mock-robot-1" and robots[0]["status"] == "Available": + return + await asyncio.sleep(0.05) + raise AssertionError("Mock Robot did not become available in the catalog") + + +async def _send(websocket, envelope): + await websocket.send(pack_envelope(envelope)) + + +async def _recv_type(websocket, message_type, reply_to): + for _ in range(20): + envelope = unpack_envelope(await asyncio.wait_for(websocket.recv(), timeout=3)) + if envelope["type"] == message_type and envelope.get("replyToMessageId") == reply_to: + return envelope + raise AssertionError(f"Did not receive {message_type} replying to {reply_to}") + + +async def _wait_for_data_channel_open(data_channel): + if data_channel.readyState == "open": + return + opened = asyncio.Event() + + @data_channel.on("open") + def on_open(): + opened.set() + + await asyncio.wait_for(opened.wait(), timeout=5) + + +async def _wait_for_ice_gathering_complete(peer_connection): + if peer_connection.iceGatheringState == "complete": + return + complete = asyncio.Event() + + @peer_connection.on("icegatheringstatechange") + def on_ice_gathering_state_change(): + if peer_connection.iceGatheringState == "complete": + complete.set() + + await asyncio.wait_for(complete.wait(), timeout=5) + + +async def _wait_for_log(caplog, text): + for _ in range(50): + if text in caplog.text: + return + await asyncio.sleep(0.05) + raise AssertionError(f"Did not find log text: {text}") + + +def _write_h264_sample_video(path): + try: + container = av.open(str(path), mode="w") + stream = container.add_stream("libx264", rate=5) + stream.width = 16 + stream.height = 16 + stream.pix_fmt = "yuv420p" + for index in range(3): + frame = av.VideoFrame(16, 16, "yuv420p") + frame.planes[0].update(bytes([32 + index * 20]) * frame.planes[0].buffer_size) + frame.planes[1].update(bytes([128]) * frame.planes[1].buffer_size) + frame.planes[2].update(bytes([128]) * frame.planes[2].buffer_size) + for packet in stream.encode(frame): + container.mux(packet) + for packet in stream.encode(): + container.mux(packet) + container.close() + except Exception as exc: + pytest.skip(f"local PyAV/FFmpeg cannot create an H.264 sample video: {exc}") + + +async def _wait_for_camera_frame(server, session_id): + for _ in range(80): + runtime = server.reconstruction_runtimes.get(session_id) + processor = getattr(runtime, "processor", None) + if getattr(processor, "frame_count", 0) > 0: + return + await asyncio.sleep(0.05) + raise AssertionError("cameraMedia did not deliver a decoded frame to reconstruction") diff --git a/tests/test_server_app.py b/tests/test_server_app.py index b44be72..6b1b8e5 100644 --- a/tests/test_server_app.py +++ b/tests/test_server_app.py @@ -12,9 +12,18 @@ TYPE_CATALOG_GET_RESULT, TYPE_CONNECTION_HELLO, TYPE_CONNECTION_HELLO_RESULT, + TYPE_DRIVER_SESSION_START, + TYPE_DRIVER_SESSION_START_RESULT, TYPE_ROBOT_STATUS, + TYPE_SESSION_ACQUIRE, + TYPE_SESSION_ACQUIRE_RESULT, + TYPE_SESSION_END, + TYPE_SESSION_END_RESULT, + TYPE_SESSION_ENDED, make_envelope, pack_envelope, + result_error, + result_ok, unpack_envelope, ) @@ -131,3 +140,312 @@ async def _duplicate_robot_id_is_cataloged_unavailable(): robots = pilot.websocket.sent[-1]["payload"]["value"]["robots"] assert robots[0]["robotId"] == "droid-1" assert robots[0]["status"] == ROBOT_STATUS_UNAVAILABLE + + +async def hello_pilot(server, pilot, session_id=None): + payload = {"role": ROLE_PILOT_CLIENT} + if session_id: + payload["sessionId"] = session_id + await server._handle_frame(pilot, pack_envelope(make_envelope(TYPE_CONNECTION_HELLO, payload))) + + +async def hello_available_driver(server, driver, robot_id="droid-1"): + await server._handle_frame( + driver, + pack_envelope( + make_envelope( + TYPE_CONNECTION_HELLO, + {"role": ROLE_ROBOT_DRIVER, "robotId": robot_id}, + robot_id=robot_id, + ) + ), + ) + await server._handle_frame( + driver, + pack_envelope( + make_envelope( + TYPE_ROBOT_STATUS, + {"name": "Dory", "type": ROBOT_TYPE_DROID, "status": ROBOT_STATUS_AVAILABLE}, + robot_id=robot_id, + ) + ), + ) + + +async def acquire_task(server, pilot, robot_id="droid-1", message_id="acquire-1"): + return asyncio.create_task( + server._handle_frame( + pilot, + pack_envelope( + make_envelope( + TYPE_SESSION_ACQUIRE, + {"robotId": robot_id}, + message_id=message_id, + robot_id=robot_id, + ) + ), + ) + ) + + +async def answer_driver_start(server, driver, ok=True): + start = driver.websocket.sent[-1] + assert start["type"] == TYPE_DRIVER_SESSION_START + session_id = start["sessionId"] + payload = result_ok({"sessionId": session_id}) if ok else result_error({"code": "driver.start_failed"}) + await server._handle_frame( + driver, + pack_envelope( + make_envelope( + TYPE_DRIVER_SESSION_START_RESULT, + payload, + reply_to_message_id=start["messageId"], + robot_id="droid-1", + session_id=session_id, + ) + ), + ) + return session_id + + +def test_acquire_reserves_robot_starts_driver_and_allocates_session(): + asyncio.run(_acquire_reserves_robot_starts_driver_and_allocates_session()) + + +async def _acquire_reserves_robot_starts_driver_and_allocates_session(): + server = ItoServer(ServerConfig(request_timeout_ms=1000, driver_status_watchdog_ms=1000)) + driver = state() + pilot = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + + task = await acquire_task(server, pilot) + await asyncio.sleep(0) + + assert server.drivers["droid-1"].occupied is True + session_id = await answer_driver_start(server, driver) + await task + + acquire = pilot.websocket.sent[-1] + assert acquire["type"] == TYPE_SESSION_ACQUIRE_RESULT + assert acquire["replyToMessageId"] == "acquire-1" + assert acquire["payload"]["ok"] is True + assert acquire["payload"]["value"]["sessionId"] == session_id + assert acquire["payload"]["value"]["robotId"] == "droid-1" + assert acquire["payload"]["value"]["sessionConfig"] == server.config.session_config_payload() + assert server.sessions[session_id].state == "active" + assert server.drivers["droid-1"].occupied is True + + +def test_acquisition_reservation_blocks_competing_pilot(): + asyncio.run(_acquisition_reservation_blocks_competing_pilot()) + + +async def _acquisition_reservation_blocks_competing_pilot(): + server = ItoServer(ServerConfig(request_timeout_ms=1000, driver_status_watchdog_ms=1000)) + driver = state() + first = state() + second = state() + await hello_available_driver(server, driver) + await hello_pilot(server, first) + await hello_pilot(server, second) + + first_task = await acquire_task(server, first, message_id="acquire-1") + await asyncio.sleep(0) + second_task = await acquire_task(server, second, message_id="acquire-2") + await asyncio.sleep(0) + + assert len([msg for msg in driver.websocket.sent if msg["type"] == TYPE_DRIVER_SESSION_START]) == 1 + await answer_driver_start(server, driver) + await first_task + await second_task + + assert first.websocket.sent[-1]["payload"]["ok"] is True + assert second.websocket.sent[-1]["type"] == TYPE_SESSION_ACQUIRE_RESULT + assert second.websocket.sent[-1]["payload"] == { + "ok": False, + "reason": {"code": "session.acquire.robot_unavailable"}, + } + + +def test_driver_start_failure_releases_reservation(): + asyncio.run(_driver_start_failure_releases_reservation()) + + +async def _driver_start_failure_releases_reservation(): + server = ItoServer(ServerConfig(request_timeout_ms=1000, driver_status_watchdog_ms=1000)) + driver = state() + pilot = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + + task = await acquire_task(server, pilot) + await asyncio.sleep(0) + session_id = await answer_driver_start(server, driver, ok=False) + await task + + assert session_id not in server.sessions + assert server.drivers["droid-1"].occupied is False + assert pilot.websocket.sent[-1]["payload"] == { + "ok": False, + "reason": {"code": "driver.start_failed"}, + } + + +def test_driver_start_timeout_releases_reservation(): + asyncio.run(_driver_start_timeout_releases_reservation()) + + +async def _driver_start_timeout_releases_reservation(): + server = ItoServer(ServerConfig(request_timeout_ms=1, driver_status_watchdog_ms=1000)) + driver = state() + pilot = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + + task = await acquire_task(server, pilot) + await task + + assert server.sessions == {} + assert server.drivers["droid-1"].occupied is False + assert pilot.websocket.sent[-1]["type"] == TYPE_SESSION_ACQUIRE_RESULT + assert pilot.websocket.sent[-1]["payload"] == { + "ok": False, + "reason": {"code": "request.timeout"}, + } + + +def test_session_end_marks_ended_and_fans_out(): + asyncio.run(_session_end_marks_ended_and_fans_out()) + + +async def _session_end_marks_ended_and_fans_out(): + server = ItoServer(ServerConfig(request_timeout_ms=1000, driver_status_watchdog_ms=1000)) + driver = state() + pilot = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + task = await acquire_task(server, pilot) + await asyncio.sleep(0) + session_id = await answer_driver_start(server, driver) + await task + + await server._handle_frame( + pilot, + pack_envelope( + make_envelope( + TYPE_SESSION_END, + {"reason": {"code": "session.ended.pilot_requested"}, "clean": True}, + message_id="end-1", + session_id=session_id, + ) + ), + ) + + assert pilot.websocket.sent[-2]["type"] == TYPE_SESSION_END_RESULT + assert pilot.websocket.sent[-2]["payload"] == {"ok": True, "value": {"sessionId": session_id}} + assert driver.websocket.sent[-2]["type"] == TYPE_SESSION_END + assert pilot.websocket.sent[-1]["type"] == TYPE_SESSION_ENDED + assert driver.websocket.sent[-1]["type"] == TYPE_SESSION_ENDED + assert pilot.websocket.sent[-1]["payload"] == { + "reason": {"code": "session.ended.pilot_requested"}, + "endedBy": ROLE_PILOT_CLIENT, + "clean": True, + } + driver_sent_count = len(driver.websocket.sent) + await server._handle_frame( + driver, + pack_envelope( + make_envelope( + TYPE_SESSION_END_RESULT, + result_ok({"sessionId": session_id}), + reply_to_message_id=driver.websocket.sent[-2]["messageId"], + session_id=session_id, + ) + ), + ) + assert len(driver.websocket.sent) == driver_sent_count + assert server.sessions[session_id].state == "ended" + assert server.drivers["droid-1"].occupied is False + + +def test_cleanup_ends_session_after_disappeared_endpoint_timeout(): + asyncio.run(_cleanup_ends_session_after_disappeared_endpoint_timeout()) + + +async def _cleanup_ends_session_after_disappeared_endpoint_timeout(): + server = ItoServer( + ServerConfig( + request_timeout_ms=1000, + driver_status_watchdog_ms=1000, + session_cleanup_timeout_ms=1, + ) + ) + driver = state() + pilot = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + task = await acquire_task(server, pilot) + await asyncio.sleep(0) + session_id = await answer_driver_start(server, driver) + await task + + server._mark_connection_disappeared(pilot) + await asyncio.sleep(0.002) + await server._cleanup_disappeared_endpoint_sessions() + + assert session_id not in server.sessions + assert driver.websocket.sent[-2]["type"] == TYPE_SESSION_END + assert driver.websocket.sent[-1]["type"] == TYPE_SESSION_ENDED + assert driver.websocket.sent[-1]["payload"]["reason"] == {"code": "session.ended.endpoint_disappeared"} + + +def test_pilot_reconnect_hello_resumes_active_session(): + asyncio.run(_pilot_reconnect_hello_resumes_active_session()) + + +async def _pilot_reconnect_hello_resumes_active_session(): + server = ItoServer(ServerConfig(request_timeout_ms=1000, driver_status_watchdog_ms=1000)) + driver = state() + pilot = state() + reconnected = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + task = await acquire_task(server, pilot) + await asyncio.sleep(0) + session_id = await answer_driver_start(server, driver) + await task + + server._mark_connection_disappeared(pilot) + await hello_pilot(server, reconnected, session_id=session_id) + + resumed = reconnected.websocket.sent[-1] + assert resumed["type"] == TYPE_CONNECTION_HELLO_RESULT + assert resumed["payload"] == { + "ok": True, + "value": { + "protocolVersion": "ito.v1", + "role": ROLE_PILOT_CLIENT, + "sessionResumed": True, + "sessionConfig": server.config.session_config_payload(), + }, + } + assert server.sessions[session_id].pilot_connection is reconnected + assert server.sessions[session_id].endpoint_missing_since is None + + +def test_pilot_reconnect_hello_rejects_missing_session(): + asyncio.run(_pilot_reconnect_hello_rejects_missing_session()) + + +async def _pilot_reconnect_hello_rejects_missing_session(): + server = ItoServer(ServerConfig()) + pilot = state() + + await hello_pilot(server, pilot, session_id="session-missing") + + assert pilot.websocket.sent[-1]["type"] == TYPE_CONNECTION_HELLO_RESULT + assert pilot.websocket.sent[-1]["payload"] == { + "ok": False, + "reason": {"code": "session.resume_unavailable"}, + } diff --git a/tests/test_webrtc_and_reconstruction.py b/tests/test_webrtc_and_reconstruction.py new file mode 100644 index 0000000..f36182b --- /dev/null +++ b/tests/test_webrtc_and_reconstruction.py @@ -0,0 +1,248 @@ +import asyncio + +from server.ito.app import ItoServer +from server.ito.config import ServerConfig +from server.ito.protocol import ( + ROLE_PILOT_CLIENT, + TYPE_WEBRTC_ANSWER, + TYPE_WEBRTC_OFFER, + WEBRTC_PATH_CAMERA_MEDIA, + WEBRTC_PATH_PILOT_INPUT, + WEBRTC_PATH_SPLAT_BATCHES, + make_envelope, + pack_envelope, + unpack_envelope, +) +from server.ito.reconstruction import ReconstructionSessionRuntime +from server.ito.media import AiortcCameraTrackReceiver +from server.ito.splat import decode_splat_batch_header, encode_splat_batch +from server.ito.webrtc import SplatBatchChannelRegistry +from server.processors.base import GaussianSplat, ProcessorSplatBatch, ReconstructionFrame + +from tests.test_server_app import ( + acquire_task, + answer_driver_start, + hello_available_driver, + hello_pilot, + state, +) + + +class FakeLivePaths: + def __init__(self): + self.offers = [] + + async def accept_offer(self, *, path, session_id, sdp): + self.offers.append({"path": path, "sessionId": session_id, "sdp": sdp}) + return f"answer for {path}" + + +def test_pilot_input_webrtc_offer_is_relayed_and_answer_is_routed_back(): + asyncio.run(_pilot_input_webrtc_offer_is_relayed_and_answer_is_routed_back()) + + +async def _pilot_input_webrtc_offer_is_relayed_and_answer_is_routed_back(): + server = ItoServer(ServerConfig(request_timeout_ms=1000, driver_status_watchdog_ms=1000)) + driver = state() + pilot = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + task = await acquire_task(server, pilot) + await asyncio.sleep(0) + session_id = await answer_driver_start(server, driver) + await task + + await server._handle_frame( + pilot, + pack_envelope( + make_envelope( + TYPE_WEBRTC_OFFER, + {"path": WEBRTC_PATH_PILOT_INPUT, "sdp": "pilot offer"}, + message_id="pilot-offer", + robot_id="droid-1", + session_id=session_id, + ) + ), + ) + + forwarded = driver.websocket.sent[-1] + assert forwarded["type"] == TYPE_WEBRTC_OFFER + assert forwarded["payload"] == {"path": WEBRTC_PATH_PILOT_INPUT, "sdp": "pilot offer"} + + await server._handle_frame( + driver, + pack_envelope( + make_envelope( + TYPE_WEBRTC_ANSWER, + {"path": WEBRTC_PATH_PILOT_INPUT, "sdp": "driver answer"}, + reply_to_message_id=forwarded["messageId"], + robot_id="droid-1", + session_id=session_id, + ) + ), + ) + + answer = pilot.websocket.sent[-1] + assert answer["type"] == TYPE_WEBRTC_ANSWER + assert answer["replyToMessageId"] == "pilot-offer" + assert answer["payload"] == {"path": WEBRTC_PATH_PILOT_INPUT, "sdp": "driver answer"} + + +def test_server_terminated_webrtc_offer_returns_non_trickle_answer(): + asyncio.run(_server_terminated_webrtc_offer_returns_non_trickle_answer()) + + +async def _server_terminated_webrtc_offer_returns_non_trickle_answer(): + server = ItoServer(ServerConfig(request_timeout_ms=1000, driver_status_watchdog_ms=1000)) + server.live_paths = FakeLivePaths() + driver = state() + pilot = state() + await hello_available_driver(server, driver) + await hello_pilot(server, pilot) + task = await acquire_task(server, pilot) + await asyncio.sleep(0) + session_id = await answer_driver_start(server, driver) + await task + + await server._handle_frame( + pilot, + pack_envelope( + make_envelope( + TYPE_WEBRTC_OFFER, + {"path": WEBRTC_PATH_SPLAT_BATCHES, "sdp": "splat offer"}, + message_id="splat-offer", + session_id=session_id, + ) + ), + ) + + answer = pilot.websocket.sent[-1] + assert answer["type"] == TYPE_WEBRTC_ANSWER + assert answer["replyToMessageId"] == "splat-offer" + assert answer["payload"] == {"path": WEBRTC_PATH_SPLAT_BATCHES, "sdp": "answer for splatBatches"} + + await server._handle_frame( + driver, + pack_envelope( + make_envelope( + TYPE_WEBRTC_OFFER, + {"path": WEBRTC_PATH_CAMERA_MEDIA, "sdp": "camera offer"}, + message_id="camera-offer", + robot_id="droid-1", + session_id=session_id, + ) + ), + ) + + assert server.live_paths.offers[-1] == { + "path": WEBRTC_PATH_CAMERA_MEDIA, + "sessionId": session_id, + "sdp": "camera offer", + } + assert driver.websocket.sent[-1]["payload"] == { + "path": WEBRTC_PATH_CAMERA_MEDIA, + "sdp": "answer for cameraMedia", + } + + +def test_splat_batch_encoder_header_and_size(): + batch = ProcessorSplatBatch( + sequence=7, + splats=[ + GaussianSplat( + position=(1.0, 2.0, 3.0), + scale=(0.1, 0.2, 0.3), + rotation=(0.0, 0.0, 0.0, 1.0), + color=(255, 128, 0, 200), + ) + ], + ) + + payload = encode_splat_batch(batch) + header = decode_splat_batch_header(payload) + + assert header.version == 1 + assert header.sequence == 7 + assert header.splat_count == 1 + assert len(payload) == 28 + header.record_stride + + +class FailingProcessor: + capture_modality = "monocularRgb" + + def start(self, session_id): + self.session_id = session_id + + def process_frame(self, frame): + raise RuntimeError("boom") + + def reset(self): + pass + + def close(self): + pass + + +def test_reconstruction_failure_is_reported_without_raising(): + failures = [] + runtime = ReconstructionSessionRuntime( + "session-1", + FailingProcessor(), + send_splat_batch=lambda payload: None, + fail_session=failures.append, + ) + runtime.start() + + runtime.process_frame(ReconstructionFrame(b"rgb", 1, 1, 1)) + runtime.process_frame(ReconstructionFrame(b"rgb", 2, 1, 1)) + + assert failures == [{"code": "session.ended.reconstruction_failed"}] + + +def test_aiortc_camera_track_receiver_converts_video_frames_to_reconstruction_frames(): + class Plane: + def __bytes__(self): + return b"rgb" + + class Frame: + pts = 2 + time_base = 0.5 + width = 1 + height = 1 + planes = [Plane()] + + def to_rgb(self): + return self + + frames = [] + receiver = AiortcCameraTrackReceiver(frames.append) + + frame = receiver._reconstruction_frame(Frame()) + + assert frame.data == b"rgb" + assert frame.timestamp_ms == 1000 + assert frame.width == 1 + assert frame.height == 1 + assert frame.pixel_format == "rgb24" + assert frame.sequence == 1 + + +def test_splat_batch_channel_registry_sends_only_when_open(): + class Channel: + readyState = "open" + + def __init__(self): + self.sent = [] + + def send(self, payload): + self.sent.append(payload) + + registry = SplatBatchChannelRegistry() + channel = Channel() + + assert registry.send("session-1", b"batch") is False + registry.attach("session-1", channel) + assert registry.send("session-1", b"batch") is True + assert channel.sent == [b"batch"] + registry.detach("session-1", channel) + assert registry.send("session-1", b"batch") is False