Add support for basic camera streams [SYNTH-10]

fission/src/mirabuf/MirabufSceneObject.ts

@@ -19,6 +19,7 @@ import {
     defaultFieldPreferences,
     defaultFieldSpawnLocation,
     defaultRobotPreferences,
+    type CameraPreferences,
     type EjectorPreferences,
     type FieldPreferences,
     type IntakePreferences,
@@ -47,6 +48,7 @@ import { SceneOverlayTag } from "@/ui/components/SceneOverlayEvents"
 import { ConfigMode } from "@/ui/panels/configuring/assembly-config/ConfigTypes"
 import ConfigurePanel from "@/ui/panels/configuring/assembly-config/ConfigurePanel"
 import AutoTestPanel from "@/ui/panels/simulation/AutoTestPanel"
+import CameraPreviewPanel from "@/ui/panels/simulation/CameraPreviewPanel"
 import JOLT from "@/util/loading/JoltSyncLoader"
 import {
     convertJoltMat44ToThreeMatrix4,
@@ -61,6 +63,7 @@ import FieldMiraEditor from "./FieldMiraEditor"
 import IntakeSensorSceneObject from "./IntakeSensorSceneObject"
 import MirabufInstance from "./MirabufInstance"
 import MirabufCachingService, { MiraType } from "./MirabufLoader"
+import RobotCameraSceneObject from "./RobotCameraSceneObject"
 import MirabufParser, { ParseErrorSeverity, type RigidNodeId, type RigidNodeReadOnly } from "./MirabufParser"
 import ProtectedZoneSceneObject from "./ProtectedZoneSceneObject"
 import ScoringZoneSceneObject from "./ScoringZoneSceneObject"
@@ -108,6 +111,7 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
 
     private _ejectables: EjectableSceneObject[] = []
     private _intakeSensor?: IntakeSensorSceneObject
+    private _cameras: RobotCameraSceneObject[] = []
     private _scoringZones: ScoringZoneSceneObject[] = []
     private _protectedZones: ProtectedZoneSceneObject[] = []
 
@@ -150,6 +154,17 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         this.robotPreferences.ejector = val
     }
 
+    public get cameraPreferences(): CameraPreferences[] {
+        return this.robotPreferences.cameras
+    }
+    public set cameraPreferences(val: CameraPreferences[]) {
+        this.robotPreferences.cameras = val
+    }
+
+    public get cameras(): Readonly<RobotCameraSceneObject[]> {
+        return this._cameras
+    }
+
     public get multiplayerOwnerName(): string | undefined {
         if (this.multiplayerOwningClientId == null) return undefined
         return World.multiplayerSystem?._clientToInfoMap?.get(this.multiplayerOwningClientId)?.displayName
@@ -304,6 +319,7 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
 
         // Intake
         this.updateIntakeSensor()
+        this.updateCameras()
         this.updateScoringZones()
         this.updateProtectedZones()
 
@@ -447,6 +463,11 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             this._intakeSensor = undefined
         }
 
+        this._cameras.forEach(c => World.sceneRenderer.removeSceneObject(c.id))
+        this._cameras = []
+
+        EventSystem.dispatch("RobotCamerasChangeEvent")
+
         this._scoringZones.forEach(zone => World.sceneRenderer.removeSceneObject(zone.id))
         this._scoringZones.length = 0
 
@@ -625,6 +646,21 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         }
     }
 
+    public updateCameras() {
+        this._cameras.forEach(c => World.sceneRenderer.removeSceneObject(c.id))
+        this._cameras = []
+
+        if (this.miraType !== MiraType.ROBOT) return
+
+        for (const camPref of this.cameraPreferences) {
+            const camera = new RobotCameraSceneObject(this, camPref)
+            this._cameras.push(camera)
+            World.sceneRenderer.registerSceneObject(camera)
+        }
+
+        EventSystem.dispatch("RobotCamerasChangeEvent")
+    }
+
     public setIntakeVisualIndicatorVisible(visible: boolean) {
         if (this._intakeSensor) {
             this._intakeSensor.setVisualIndicatorVisible(visible)
@@ -968,6 +1004,7 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         this.updateScoringZones()
         this.updateProtectedZones()
         this.updateIntakeSensor()
+        this.updateCameras()
     }
 
     public updateSimConfig(config: SimConfigData | undefined) {
@@ -1087,6 +1124,14 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             })
         }
 
+        if (this.miraType === MiraType.ROBOT && this._cameras.length > 0) {
+            data.items.push({
+                name: "Camera Preview",
+                screen: CameraPreviewPanel,
+                type: "panel",
+            })
+        }
+
         if (World.sceneRenderer.currentCameraControls.controlsType == "Target") {
             const cameraControls = World.sceneRenderer.currentCameraControls as CustomTargetControls
             if (cameraControls.focusProvider == this) {

fission/src/mirabuf/RobotCameraSceneObject.ts

@@ -0,0 +1,200 @@
+import type Jolt from "@azaleacolburn/jolt-physics"
+import * as THREE from "three"
+import type { CameraPreferences } from "@/systems/preferences/PreferenceTypes"
+import SceneObject from "@/systems/scene/SceneObject"
+import { sendCameraFrame } from "@/systems/simulation/wpilib_brain/CameraFrameSocket"
+import SimCamera from "@/systems/simulation/wpilib_brain/sim/SimCamera"
+import World from "@/systems/World"
+import { convertArrayToThreeMatrix4, convertJoltMat44ToThreeMatrix4 } from "@/util/TypeConversions"
+import type MirabufSceneObject from "./MirabufSceneObject"
+
+const MAX_DIMENSION = 1280
+const MIN_DIMENSION = 16
+const MAX_FPS = 60
+const JPEG_QUALITY = 0.6
+
+// camera looks down local -Z but the gizmo placeholder points +Z; flip so it faces where
+// the gizmo points (away from the robot), not back into it
+const FORWARD_FLIP = new THREE.Matrix4().makeRotationY(Math.PI)
+
+class RobotCameraSceneObject extends SceneObject {
+    private static _previewConsumers = 0
+    public static get previewConsumers(): number {
+        return RobotCameraSceneObject._previewConsumers
+    }
+    public static addPreviewConsumer(): void {
+        RobotCameraSceneObject._previewConsumers++
+    }
+    public static removePreviewConsumer(): void {
+        RobotCameraSceneObject._previewConsumers = Math.max(0, RobotCameraSceneObject._previewConsumers - 1)
+    }
+
+    private _parentAssembly: MirabufSceneObject
+    private _prefs: CameraPreferences
+
+    private _camera: THREE.PerspectiveCamera
+    private _renderTarget?: THREE.WebGLRenderTarget
+    private _parentBodyId?: Jolt.BodyID
+    private _deltaTransformation: THREE.Matrix4
+    private readonly _worldTransform = new THREE.Matrix4()
+
+    private _width = 0
+    private _height = 0
+    private _pixelBuffer?: Uint8Array
+    private _frameCanvas?: HTMLCanvasElement
+    private _frameCtx?: CanvasRenderingContext2D
+    private _imageData?: ImageData
+
+    private _timeSinceCapture = Number.POSITIVE_INFINITY
+
+    public constructor(parentAssembly: MirabufSceneObject, prefs: CameraPreferences) {
+        super()
+        this._parentAssembly = parentAssembly
+        this._prefs = prefs
+        this._camera = new THREE.PerspectiveCamera(prefs.fovDegrees, 1, 0.05, 1000)
+        this._deltaTransformation = convertArrayToThreeMatrix4(prefs.deltaTransformation)
+    }
+
+    /** sim device key, e.g. `"USB Camera 0[0]"` */
+    public get deviceName(): string {
+        return `${this._prefs.name}[${this._prefs.id}]`
+    }
+
+    public get displayName(): string {
+        return `${this._parentAssembly.assemblyName} – ${this._prefs.name}`
+    }
+
+    public get frameCanvas(): HTMLCanvasElement | undefined {
+        return this._frameCanvas
+    }
+
+    public get width(): number {
+        return this._width || this._prefs.resolutionWidth
+    }
+
+    public get height(): number {
+        return this._height || this._prefs.resolutionHeight
+    }
+
+    public setup(): void {
+        this._parentBodyId = this._parentAssembly.mechanism.nodeToBody.get(
+            this._prefs.parentNode ?? this._parentAssembly.rootNodeId
+        )
+        this.resize(this._prefs.resolutionWidth, this._prefs.resolutionHeight)
+    }
+
+    private resize(width: number, height: number): void {
+        const w = Math.max(MIN_DIMENSION, Math.min(MAX_DIMENSION, Math.round(width)))
+        const h = Math.max(MIN_DIMENSION, Math.min(MAX_DIMENSION, Math.round(height)))
+        if (w === this._width && h === this._height && this._renderTarget) return
+
+        this._width = w
+        this._height = h
+
+        this._renderTarget?.dispose()
+        this._renderTarget = new THREE.WebGLRenderTarget(w, h)
+
+        this._pixelBuffer = new Uint8Array(w * h * 4)
+        this._imageData = new ImageData(w, h)
+
+        const canvas = this._frameCanvas ?? document.createElement("canvas")
+        canvas.width = w
+        canvas.height = h
+        this._frameCanvas = canvas
+        this._frameCtx = canvas.getContext("2d") ?? undefined
+
+        this._camera.aspect = w / h
+        this._camera.updateProjectionMatrix()
+    }
+
+    public update(): void {
+        if (!this._parentBodyId || !this._renderTarget || !this._pixelBuffer || !this._imageData) return
+
+        if (World.physicsSystem.isPaused) return
+
+        const device = this.deviceName
+        const streaming = SimCamera.isPresent(device)
+
+        if (!streaming && RobotCameraSceneObject.previewConsumers === 0) return
+
+        let reqWidth = this._prefs.resolutionWidth
+        let reqHeight = this._prefs.resolutionHeight
+        let reqFps = this._prefs.fps
+
+        // lets the camera preview panel work without robot code sim running
+        if (streaming) {
+            reqWidth = SimCamera.getWidth(device, reqWidth)
+            reqHeight = SimCamera.getHeight(device, reqHeight)
+            reqFps = SimCamera.getFps(device, reqFps)
+        }
+        const fps = Math.max(1, Math.min(MAX_FPS, reqFps))
+        this.resize(reqWidth, reqHeight)
+
+        if (this._camera.fov !== this._prefs.fovDegrees) {
+            this._camera.fov = this._prefs.fovDegrees
+            this._camera.updateProjectionMatrix()
+        }
+
+        this._timeSinceCapture += World.currentDeltaT
+        if (this._timeSinceCapture < 1 / fps) return
+        this._timeSinceCapture = 0
+
+        const parentBody = World.physicsSystem.getBody(this._parentBodyId)
+        if (!parentBody) return
+        const worldTransform = this._worldTransform
+            .copy(this._deltaTransformation)
+            .premultiply(convertJoltMat44ToThreeMatrix4(parentBody.GetWorldTransform()))
+            .multiply(FORWARD_FLIP)
+        this._camera.position.setFromMatrixPosition(worldTransform)
+        this._camera.quaternion.setFromRotationMatrix(worldTransform)
+        this._camera.updateMatrixWorld()
+
+        const renderer = World.sceneRenderer.renderer
+        const prevTarget = renderer.getRenderTarget()
+        const prevAutoClear = renderer.autoClear
+        try {
+            renderer.autoClear = true
+            renderer.setRenderTarget(this._renderTarget)
+            renderer.clear()
+            renderer.render(World.sceneRenderer.scene, this._camera)
+            renderer.readRenderTargetPixels(this._renderTarget, 0, 0, this._width, this._height, this._pixelBuffer)
+
+            this.flipInto(this._imageData, this._pixelBuffer)
+            this._frameCtx?.putImageData(this._imageData, 0, 0)
+
+            if (streaming && this._frameCanvas) {
+                const dataUrl = this._frameCanvas.toDataURL("image/jpeg", JPEG_QUALITY)
+                const base64 = atob(dataUrl.slice(dataUrl.indexOf(",") + 1))
+                const bytes = new Uint8Array(base64.length)
+                for (let i = 0; i < base64.length; i++) bytes[i] = base64.charCodeAt(i)
+                sendCameraFrame(device, bytes)
+            }
+        } catch (e) {
+            console.error(`Camera capture failed for '${device}'`, e)
+        } finally {
+            renderer.setRenderTarget(prevTarget)
+            renderer.autoClear = prevAutoClear
+        }
+    }
+
+    // GL pixels are bottom-up; flip rows into the top-down ImageData
+    private flipInto(target: ImageData, source: Uint8Array): void {
+        const w = this._width
+        const h = this._height
+        const rowBytes = w * 4
+        const dst = target.data
+        for (let y = 0; y < h; y++) {
+            const srcStart = (h - 1 - y) * rowBytes
+            dst.set(source.subarray(srcStart, srcStart + rowBytes), y * rowBytes)
+        }
+    }
+
+    public dispose(): void {
+        this._renderTarget?.dispose()
+        this._renderTarget = undefined
+        this._frameCanvas = undefined
+        this._frameCtx = undefined
+    }
+}
+
+export default RobotCameraSceneObject

fission/src/systems/EventSystem.ts

@@ -39,6 +39,7 @@ interface EventDataMap {
 
     // Code Sim
     SimMapUpdateEvent: { internalUpdate: boolean }
+    RobotCamerasChangeEvent: never
 
     // Context Menu
     ContextSupplierEvent: { data: ContextData; mousePosition: [number, number] }

fission/src/systems/preferences/PreferenceTypes.ts

@@ -145,6 +145,31 @@ export type EjectorPreferences = {
     ejectOrder: "FIFO" | "LIFO"
 }
 
+// name/id must match the robot code's UsbCamera args, key `"<name>[<id>]"`
+export type CameraPreferences = {
+    name: string
+    id: number
+    deltaTransformation: number[]
+    parentNode: string | undefined
+    fovDegrees: number
+    resolutionWidth: number
+    resolutionHeight: number
+    fps: number
+}
+
+export function defaultCameraPreferences(id: number): CameraPreferences {
+    return {
+        name: `USB Camera ${id}`,
+        id: id,
+        deltaTransformation: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1],
+        parentNode: undefined,
+        fovDegrees: 60,
+        resolutionWidth: 640,
+        resolutionHeight: 480,
+        fps: 30,
+    }
+}
+
 /** The behavior types that can be sequenced. */
 export type BehaviorType = "Elevator" | "Arm"
 
@@ -171,6 +196,7 @@ export type RobotPreferences = {
     motors: MotorPreferences[]
     intake: IntakePreferences
     ejector: EjectorPreferences
+    cameras: CameraPreferences[]
     driveVelocity: number
     driveAcceleration: number
     unstickForce: number
@@ -259,6 +285,7 @@ export function defaultRobotPreferences(): RobotPreferences {
             parentNode: undefined,
             ejectOrder: "FIFO",
         },
+        cameras: [],
         driveVelocity: 0,
         driveAcceleration: 0,
         unstickForce: 8000,