diff --git a/fission/src/mirabuf/MirabufSceneObject.ts b/fission/src/mirabuf/MirabufSceneObject.ts index 614fdf3df1..595e1cf688 100644 --- a/fission/src/mirabuf/MirabufSceneObject.ts +++ b/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" @@ -115,6 +118,7 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier { private _ejectables: EjectableSceneObject[] = [] private _intakeSensor?: IntakeSensorSceneObject + private _cameras: RobotCameraSceneObject[] = [] private _scoringZones: ScoringZoneSceneObject[] = [] private _protectedZones: ProtectedZoneSceneObject[] = [] @@ -160,6 +164,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 { + return this._cameras + } + public get multiplayerOwnerName(): string | undefined { if (this.multiplayerOwningClientId == null) return undefined return World.multiplayerSystem?._clientToInfoMap?.get(this.multiplayerOwningClientId)?.displayName @@ -312,6 +327,12 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier { simLayer.setBrain(this._brain) } + // Intake + this.updateIntakeSensor() + this.updateCameras() + this.updateScoringZones() + this.updateProtectedZones() + if (this.isOwnObject) { setSpotlightAssembly(this) } @@ -466,6 +487,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 @@ -644,6 +670,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 + + this._cameras = this.cameraPreferences.map(camPref => { + const camera = new RobotCameraSceneObject(this, camPref) + World.sceneRenderer.registerSceneObject(camera) + return camera + }) + + EventSystem.dispatch("RobotCamerasChangeEvent") + } + public setIntakeVisualIndicatorVisible(visible: boolean) { if (this._intakeSensor) { this._intakeSensor.setVisualIndicatorVisible(visible) @@ -1128,6 +1169,7 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier { this.updateScoringZones() this.updateProtectedZones() this.updateIntakeSensor() + this.updateCameras() } public updateSimConfig(config: SimConfigData | undefined) { @@ -1247,6 +1289,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) { diff --git a/fission/src/mirabuf/RobotCameraSceneObject.ts b/fission/src/mirabuf/RobotCameraSceneObject.ts new file mode 100644 index 0000000000..b1ed44f523 --- /dev/null +++ b/fission/src/mirabuf/RobotCameraSceneObject.ts @@ -0,0 +1,199 @@ +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 rowBytes = this._width * 4 + const dst = target.data + for (let y = 0; y < this._height; y++) { + const srcStart = (this._height - 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 diff --git a/fission/src/systems/EventSystem.ts b/fission/src/systems/EventSystem.ts index 5467d1f313..b64482af71 100644 --- a/fission/src/systems/EventSystem.ts +++ b/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] } diff --git a/fission/src/systems/preferences/PreferenceTypes.ts b/fission/src/systems/preferences/PreferenceTypes.ts index a506c84ef4..61cb69e0b8 100644 --- a/fission/src/systems/preferences/PreferenceTypes.ts +++ b/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 `"[]"` +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, diff --git a/fission/src/systems/simulation/wpilib_brain/CameraFrameSocket.ts b/fission/src/systems/simulation/wpilib_brain/CameraFrameSocket.ts new file mode 100644 index 0000000000..a4bea8c0e8 --- /dev/null +++ b/fission/src/systems/simulation/wpilib_brain/CameraFrameSocket.ts @@ -0,0 +1,48 @@ +// frame bytes are too large for the HALSim SimDevice channel (numbers/booleans only), so +// the robot process hosts a WebSocket server (SyntheSimJava's CameraFrameServer) and we +// connect out to stream binary "\n" messages to it + +const PORT = 5808 +const RETRY_MS = 2000 +// drop frames instead of queuing when the socket is backed up, else a slow consumer makes +// frames pile up and arrive in erratic bursts +const MAX_BUFFERED_BYTES = 1_000_000 + +const ENCODER = new TextEncoder() + +let socket: WebSocket | undefined +let lastAttempt = Number.NEGATIVE_INFINITY + +function ensureSocket(): void { + if (socket && (socket.readyState === WebSocket.OPEN || socket.readyState === WebSocket.CONNECTING)) return + + const now = performance.now() + if (now - lastAttempt < RETRY_MS) return + lastAttempt = now + + try { + socket = new WebSocket(`ws://localhost:${PORT}`) + } catch { + socket = undefined + } + + if (socket !== undefined) { + socket.addEventListener("close", () => { + socket = undefined + }) + socket.addEventListener("error", () => { + socket = undefined + }) + } +} + +export function sendCameraFrame(device: string, jpeg: Uint8Array): void { + ensureSocket() + if (!socket || socket.readyState !== WebSocket.OPEN || socket.bufferedAmount >= MAX_BUFFERED_BYTES) return + + const header = ENCODER.encode(`${device}\n`) + const msg = new Uint8Array(header.length + jpeg.length) + msg.set(header, 0) + msg.set(jpeg, header.length) + socket.send(msg) +} diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibState.ts b/fission/src/systems/simulation/wpilib_brain/WPILibState.ts index e11baf8a2f..3328ee78e5 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibState.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibState.ts @@ -54,6 +54,7 @@ export const supplierTypeMap: { [k in SimType]: NoraTypes | undefined } = { [SimType.AI]: undefined, [SimType.AO]: NoraTypes.NUMBER, [SimType.DRIVERS_STATION]: undefined, + [SimType.CAMERA]: undefined, } export const receiverTypeMap: { [k in SimType]: NoraTypes | undefined } = { @@ -68,4 +69,5 @@ export const receiverTypeMap: { [k in SimType]: NoraTypes | undefined } = { [SimType.AI]: NoraTypes.NUMBER, [SimType.AO]: undefined, [SimType.DRIVERS_STATION]: undefined, + [SimType.CAMERA]: undefined, } diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibTypes.ts b/fission/src/systems/simulation/wpilib_brain/WPILibTypes.ts index ffb177284b..dc08230b7d 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibTypes.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibTypes.ts @@ -18,6 +18,7 @@ export enum SimType { AI = "AI", AO = "AO", DRIVERS_STATION = "DriverStation", + CAMERA = "Camera", } export enum FieldType { @@ -55,4 +56,11 @@ export const CANMOTOR_BUS_VOLTAGE = ">busVoltage" export const CANENCODER_POSITION = ">position" export const CANENCODER_VELOCITY = ">velocity" +// USB camera config, set by robot code and read by Synthesis. the frame can't ride HALSim +// (numbers/booleans only) and streams over a side channel (see CameraFrameSocket) +export const CAMERA_WIDTH = " = new Lazy(() => new WPILibWSWorker()) diff --git a/fission/src/systems/simulation/wpilib_brain/sim/SimCamera.ts b/fission/src/systems/simulation/wpilib_brain/sim/SimCamera.ts new file mode 100644 index 0000000000..f15a0984de --- /dev/null +++ b/fission/src/systems/simulation/wpilib_brain/sim/SimCamera.ts @@ -0,0 +1,25 @@ +import { getSimMap } from "../WPILibState" +import { CAMERA_FPS, CAMERA_HEIGHT, CAMERA_WIDTH, SimType } from "../WPILibTypes" +import SimGeneric from "./SimGeneric" + +// HALSim only carries camera config (resolution/fps/connected) and existence; the frame is +// streamed separately (see CameraFrameSocket) +export default class SimCamera { + private constructor() {} + + public static getWidth(device: string, defaultValue: number): number { + return SimGeneric.get(SimType.CAMERA, device, CAMERA_WIDTH, defaultValue) + } + + public static getHeight(device: string, defaultValue: number): number { + return SimGeneric.get(SimType.CAMERA, device, CAMERA_HEIGHT, defaultValue) + } + + public static getFps(device: string, defaultValue: number): number { + return SimGeneric.get(SimType.CAMERA, device, CAMERA_FPS, defaultValue) + } + + public static isPresent(device: string): boolean { + return !!getSimMap()?.get(SimType.CAMERA)?.get(device) + } +} diff --git a/fission/src/test/PreferencesSystem.test.ts b/fission/src/test/PreferencesSystem.test.ts index 7f2b99804d..b62315051e 100644 --- a/fission/src/test/PreferencesSystem.test.ts +++ b/fission/src/test/PreferencesSystem.test.ts @@ -118,6 +118,7 @@ describe("Preference System Robot/Field", () => { parentNode: undefined, ejectOrder: "FIFO", }, + cameras: [], driveVelocity: 3, driveAcceleration: 6, unstickForce: 8000, @@ -139,6 +140,7 @@ describe("Preference System Robot/Field", () => { parentNode: undefined, ejectOrder: "LIFO", }, + cameras: [], driveVelocity: 1.5, driveAcceleration: 8, unstickForce: 10000, diff --git a/fission/src/ui/components/Panel.tsx b/fission/src/ui/components/Panel.tsx index 3b173e9fe8..db4f0cd8b9 100644 --- a/fission/src/ui/components/Panel.tsx +++ b/fission/src/ui/components/Panel.tsx @@ -78,6 +78,8 @@ export const Panel = ({ children, panel, parent }: PanelElementProps backgroundColor: "#2e2e2e", boxShadow: 6, maxHeight: "85vh", + width: props.width, + height: props.height, flexDirection: "column", }} ref={nodeRef} @@ -102,12 +104,13 @@ export const Panel = ({ children, panel, parent }: PanelElementProps sx={{ p: 2, flex: "1 1 auto", + minHeight: 0, overflowY: "auto", "&:last-child": { pb: 2 }, backgroundColor: "inherit", }} > -
+
{React.Children.map(children, child => { if (React.isValidElement(child)) return React.cloneElement(child, { panel, parent }) })} diff --git a/fission/src/ui/helpers/UIProviderHelpers.ts b/fission/src/ui/helpers/UIProviderHelpers.ts index d3759b12f6..1139d45adf 100644 --- a/fission/src/ui/helpers/UIProviderHelpers.ts +++ b/fission/src/ui/helpers/UIProviderHelpers.ts @@ -30,6 +30,8 @@ export interface UIScreenProps

{ acceptText?: string blocking?: boolean // if true, will prevent other panels from opening while this panel is open blockingMessage?: string + width?: number | string + height?: number | string custom: P } diff --git a/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts b/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts index 480cb5d96c..26b4abe34a 100644 --- a/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts +++ b/fission/src/ui/panels/configuring/assembly-config/ConfigTypes.ts @@ -38,4 +38,5 @@ export enum ConfigMode { DRIVETRAIN, ALLIANCE, METADATA, + CAMERA, } diff --git a/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx b/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx index 81a35f60d6..1662489586 100644 --- a/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx @@ -40,6 +40,7 @@ import { globalAddToast, globalOpenPanel } from "@/ui/components/GlobalUIControl import AssemblyExportButton from "@/panels/configuring/assembly-config/configure/AssemblyExport.tsx" import MetadataConfigInterface from "@/panels/configuring/assembly-config/interfaces/MetadataConfigInterface.tsx" import { FaArrowsRotate } from "react-icons/fa6" +import ConfigureCameraInterface from "./interfaces/cameras/ConfigureCameraInterface" // Register command: Configure Assets (module-scope side effect) CommandRegistry.get().registerCommands([ @@ -165,6 +166,8 @@ const ConfigInterface: React.FC case ConfigMode.EJECTOR: return + case ConfigMode.CAMERA: + return case ConfigMode.SUBSYSTEMS: return case ConfigMode.CONTROLS: { @@ -374,6 +377,12 @@ const ConfigurePanel: React.FC> "Configure the robot’s ejector mechanism, which controls the release or expulsion of game pieces." ), + new ConfigModeSelectionOption( + "USB Cameras", + ConfigMode.CAMERA, + "Add USB cameras and configure their position, resolution, and field of view for code simulation." + ), + new ConfigModeSelectionOption( "Configure Joints", ConfigMode.SUBSYSTEMS, diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/cameras/CameraConfigInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/cameras/CameraConfigInterface.tsx new file mode 100644 index 0000000000..8b43d44b61 --- /dev/null +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/cameras/CameraConfigInterface.tsx @@ -0,0 +1,198 @@ +import type { RigidNodeId } from "@/mirabuf/MirabufParser" +import * as THREE from "three" +import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import type { RigidNodeAssociate } from "@/mirabuf/MirabufSceneObject" +import PreferencesSystem from "@/systems/preferences/PreferencesSystem" +import type { CameraPreferences } from "@/systems/preferences/PreferenceTypes" +import type GizmoSceneObject from "@/systems/scene/GizmoSceneObject" +import World from "@/systems/World" +import SelectButton from "@/ui/components/SelectButton" +import StatefulSlider from "@/ui/components/StatefulSlider" +import { + convertArrayToThreeMatrix4, + convertJoltMat44ToThreeMatrix4, + convertReactRgbaColorToThreeColor, + convertThreeMatrix4ToArray, +} from "@/util/TypeConversions" +import { TextField } from "@mui/material" +import type Jolt from "@synthesis.adsk/jolt-physics" +import type React from "react" +import { useCallback, useEffect, useMemo, useRef, useState } from "react" +import TransformGizmoControl from "@/ui/components/TransformGizmoControl" +import EventSystem from "@/systems/EventSystem" + +const MIN_FOV = 10 +const MAX_FOV = 170 +const MIN_RES = 64 +const MAX_RES = 1280 +const MIN_FPS = 1 +const MAX_FPS = 60 + +function parentBodyId(robot: MirabufSceneObject, parentNode: string | undefined): Jolt.BodyID | undefined { + return robot.mechanism.nodeToBody.get(parentNode ?? robot.rootNodeId) +} + +interface CameraConfigInterfaceProps { + selectedRobot: MirabufSceneObject + camera: CameraPreferences +} + +const CameraConfigInterface: React.FC = ({ selectedRobot, camera }) => { + const [selectedNode, setSelectedNode] = useState(undefined) + + const gizmoRef = useRef(undefined) + + const commit = useCallback(() => { + if (camera && gizmoRef.current) { + const nodeBodyId = parentBodyId(selectedRobot, camera.parentNode) + if (nodeBodyId) { + const gizmoWorld = gizmoRef.current.obj.matrixWorld.clone() + const robotWorld = convertJoltMat44ToThreeMatrix4( + World.physicsSystem.getBody(nodeBodyId)!.GetWorldTransform() + ) + const delta = gizmoWorld.premultiply(robotWorld.invert()) + camera.deltaTransformation = convertThreeMatrix4ToArray(delta) + } + } + PreferencesSystem.setRobotPreferences(selectedRobot.assemblyName, selectedRobot.robotPreferences) + PreferencesSystem.savePreferences() + selectedRobot.updateCameras() + }, [camera, selectedRobot]) + + useEffect(() => { + return EventSystem.listen("ConfigurationSavedEvent", commit) + }, [commit]) + + useEffect(() => { + setSelectedNode(camera?.parentNode) + }, [camera]) + + // selectedIndex dep so gizmo respawns for newly selected camera + const placeholderMesh = useMemo(() => { + return new THREE.Mesh( + new THREE.BoxGeometry(0.12, 0.08, 0.16).translate(0, 0, 0.08), + World.sceneRenderer.createToonMaterial(convertReactRgbaColorToThreeColor({ r: 80, g: 180, b: 255, a: 255 })) + ) + }, [camera]) + + const gizmoComponent = useMemo(() => { + if (!camera) { + gizmoRef.current = undefined + return <> + } + + const postGizmoCreation = (gizmo: GizmoSceneObject) => { + const material = (gizmo.obj as THREE.Mesh).material as THREE.Material + material.depthTest = false + + const delta = convertArrayToThreeMatrix4(camera.deltaTransformation) + const nodeBodyId = parentBodyId(selectedRobot, camera.parentNode) + if (!nodeBodyId) return + + const robotWorld = convertJoltMat44ToThreeMatrix4( + World.physicsSystem.getBody(nodeBodyId)!.GetWorldTransform() + ) + const gizmoWorld = delta.premultiply(robotWorld) + gizmo.obj.position.setFromMatrixPosition(gizmoWorld) + gizmo.obj.rotation.setFromRotationMatrix(gizmoWorld) + } + + return ( + + ) + // eslint-disable-next-line react-hooks/exhaustive-deps + }, [placeholderMesh, camera, selectedRobot]) + + const trySetSelectedNode = useCallback( + (body: Jolt.BodyID) => { + const assoc = World.physicsSystem.getBodyAssociation(body) as RigidNodeAssociate + if (!assoc || assoc.sceneObject !== selectedRobot) return false + + setSelectedNode(assoc.rigidNodeId) + if (camera) camera.parentNode = assoc.rigidNodeId + + commit() + + return true + }, + [selectedRobot, camera, commit] + ) + + return ( + <> + { + camera.name = e.target.value + }} + /> + + trySetSelectedNode(body.GetID())} + /> + + { + camera.fovDegrees = v + }} + /> + {/* NOTE: in code sim mode, these are only configurable through robot code */} + {selectedRobot.brain?.brainType === "synthesis" && ( + <> + { + camera.resolutionWidth = v + }} + /> + { + camera.resolutionHeight = v + }} + /> + { + camera.fps = v + }} + /> + + )} + + {gizmoComponent} + + ) +} + +export default CameraConfigInterface diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/cameras/ConfigureCameraInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/cameras/ConfigureCameraInterface.tsx new file mode 100644 index 0000000000..26cdcce7b7 --- /dev/null +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/cameras/ConfigureCameraInterface.tsx @@ -0,0 +1,100 @@ +import { Box, Divider, Stack } from "@mui/material" +import { useCallback, useEffect, useState } from "react" +import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import { PAUSE_REF_ASSEMBLY_CONFIG } from "@/systems/physics/PhysicsTypes" +import { type CameraPreferences, defaultCameraPreferences } from "@/systems/preferences/PreferenceTypes" +import World from "@/systems/World" +import { Button, DeleteButton, EditButton, SynthesisIcons } from "@/ui/components/StyledComponents" +import CameraConfigInterface from "./CameraConfigInterface" +import Label from "@/ui/components/Label" +import { SelectMenuHeader } from "@/ui/components/SelectMenu" +import EventSystem from "@/systems/EventSystem" + +interface ConfigCameraProps { + selectedRobot: MirabufSceneObject +} + +const ConfigureCameraInterface: React.FC = ({ selectedRobot }) => { + const [_version, setVersion] = useState(0) + const [selectedCamera, setSelectedCamera] = useState(null) + + const cameras = selectedRobot.cameraPreferences + + const forceRender = useCallback(() => setVersion(v => v + 1), []) + + useEffect(() => { + World.physicsSystem.holdPause(PAUSE_REF_ASSEMBLY_CONFIG) + return () => { + World.physicsSystem.releasePause(PAUSE_REF_ASSEMBLY_CONFIG) + } + }, []) + + return ( + <> + {selectedCamera !== null ? ( + <> + { + EventSystem.dispatch("ConfigurationSavedEvent") + setSelectedCamera(null) + }} + /> + + + + ) : ( + + {cameras.length > 0 ? ( + cameras.map(cameraPrefs => ( + + + + + { + setSelectedCamera(cameraPrefs) + }} + /> + { + selectedRobot.cameraPreferences = + selectedRobot.cameraPreferences.filter( + cpref => cpref.id !== cameraPrefs.id + ) + setSelectedCamera(null) + selectedRobot.updateCameras() + forceRender() + }} + /> + + + + )) + ) : ( + + )} + + + )} + + ) +} + +export default ConfigureCameraInterface diff --git a/fission/src/ui/panels/simulation/CameraPreviewPanel.tsx b/fission/src/ui/panels/simulation/CameraPreviewPanel.tsx new file mode 100644 index 0000000000..be8672652f --- /dev/null +++ b/fission/src/ui/panels/simulation/CameraPreviewPanel.tsx @@ -0,0 +1,149 @@ +import type React from "react" +import { useEffect, useRef, useState } from "react" +import RobotCameraSceneObject from "@/mirabuf/RobotCameraSceneObject" +import EventSystem from "@/systems/EventSystem" +import World from "@/systems/World" +import Label from "@/ui/components/Label" +import type { PanelImplProps } from "@/ui/components/Panel" +import { Button } from "@/ui/components/StyledComponents" +import { useUIContext } from "@/ui/helpers/UIProviderHelpers" + +function resolveCameras(): RobotCameraSceneObject[] { + return World.sceneRenderer.mirabufSceneObjects.getRobots().flatMap(r => [...r.cameras]) +} + +const BOX_W = 480 +const BOX_H = 360 +const PANEL_WIDTH = BOX_W + 40 +const PANEL_HEIGHT = `min(${BOX_H + 200}px, 85vh)` + +const CameraPreviewPanel: React.FC> = ({ panel }) => { + const { configureScreen } = useUIContext() + const previewRef = useRef(null) + const [cameras, setCameras] = useState(resolveCameras) + const [selectedIndex, setSelectedIndex] = useState(0) + + useEffect(() => { + configureScreen( + panel!, + { + title: "Camera Preview", + hideCancel: true, + acceptText: "Close", + width: PANEL_WIDTH, + height: PANEL_HEIGHT, + }, + {} + ) + }, [configureScreen, panel]) + + useEffect(() => { + // cameras don't render if no consumer + RobotCameraSceneObject.addPreviewConsumer() + return () => RobotCameraSceneObject.removePreviewConsumer() + }, []) + + useEffect(() => EventSystem.listen("RobotCamerasChangeEvent", () => setCameras(resolveCameras())), []) + + const index = Math.min(selectedIndex, Math.max(0, cameras.length - 1)) + const selected = cameras[index] + + useEffect(() => { + const preview = previewRef.current + const canvas = selected?.frameCanvas + if (!preview || !canvas) return + + Object.assign(canvas.style, { + display: "block", + position: "absolute", + top: "50%", + left: "50%", + transform: "translate(-50%, -50%)", + maxWidth: "100%", + maxHeight: "100%", + width: "auto", + height: "auto", + borderRadius: "8px", + background: "#000", + }) + preview.appendChild(canvas) + + return () => canvas.remove() + }, [selected]) + + if (cameras.length === 0) { + return ( + + ) + } + + return ( +

+ {cameras.length > 1 && ( +
+ + + +
+ )} + + {selected && ( +
+ +
+
+ )} +
+ ) +} + +export default CameraPreviewPanel diff --git a/simulation/SyntheSimJava/build.gradle b/simulation/SyntheSimJava/build.gradle index d0b00b1dfa..12893616cf 100644 --- a/simulation/SyntheSimJava/build.gradle +++ b/simulation/SyntheSimJava/build.gradle @@ -41,6 +41,9 @@ def WPI_Version = '2026.2.2' def REV_Version = '2026.0.5' def CTRE_Version = '26.3.0' def STUDICA_Version = '2026.0.0' +// OpenCV still published under the frc2025 group for the 2026 season (no frc2026 group) +def OPENCV_Group = 'edu.wpi.first.thirdparty.frc2025.opencv' +def OPENCV_Version = '4.10.0-3' dependencies { // This dependency is exported to consumers, that is to say found on their compile classpath. @@ -57,6 +60,15 @@ dependencies { implementation "edu.wpi.first.ntcore:ntcore-java:$WPI_Version" implementation "edu.wpi.first.wpiunits:wpiunits-java:$WPI_Version" + // cscore + OpenCV for the simulated USB camera (CameraServer-compatible) + implementation "edu.wpi.first.cscore:cscore-java:$WPI_Version" + implementation "edu.wpi.first.cameraserver:cameraserver-java:$WPI_Version" + implementation "$OPENCV_Group:opencv-java:$OPENCV_Version" + + // WebSocket lib for receiving rendered frames; too large for HALSim's SimDevice + // channel, so they stream over a side channel + implementation "org.java-websocket:Java-WebSocket:1.5.7" + // REVRobotics implementation "com.revrobotics.frc:REVLib-java:$REV_Version" diff --git a/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.jar b/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.jar index e6441136f3..9bbc975c74 100644 Binary files a/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.jar and b/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.jar differ diff --git a/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.properties b/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.properties index a4413138c9..a351597e62 100644 --- a/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.properties +++ b/simulation/SyntheSimJava/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.8-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-9.6.1-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/simulation/SyntheSimJava/gradlew b/simulation/SyntheSimJava/gradlew index b740cf1339..faf93008b7 100755 --- a/simulation/SyntheSimJava/gradlew +++ b/simulation/SyntheSimJava/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -84,7 +86,7 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s\n' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -203,7 +205,7 @@ fi DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' # Collect all arguments for the java command: -# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, # and any embedded shellness will be escaped. # * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be # treated as '${Hostname}' itself on the command line. diff --git a/simulation/SyntheSimJava/gradlew.bat b/simulation/SyntheSimJava/gradlew.bat index 25da30dbde..9d21a21834 100644 --- a/simulation/SyntheSimJava/gradlew.bat +++ b/simulation/SyntheSimJava/gradlew.bat @@ -13,6 +13,8 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem @if "%DEBUG%"=="" @echo off @rem ########################################################################## diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/Camera.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/Camera.java new file mode 100644 index 0000000000..011a657578 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/Camera.java @@ -0,0 +1,114 @@ +package com.autodesk.synthesis.cscore; + +import org.opencv.core.Mat; +import org.opencv.core.MatOfByte; +import org.opencv.imgcodecs.Imgcodecs; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimInt; + +/** + * Sim device for a USB camera. + * Config published over HALSim, frame streamed from Fission to {@link CameraFrameServer} + */ +public class Camera { + + private SimDevice m_device; + + private SimInt m_width; + private SimInt m_height; + private SimInt m_fps; + private SimBoolean m_connected; + + private final int m_defaultWidth; + private final int m_defaultHeight; + private final int m_defaultFps; + + private final String m_deviceName; + private final CameraFrameServer m_frameServer; + + public Camera(String name, int deviceId, int width, int height, int fps) { + m_device = SimDevice.create("Camera:" + name, deviceId); + + // null outside of simulation + if (m_device != null) { + m_width = m_device.createInt("width", Direction.kOutput, width); + m_height = m_device.createInt("height", Direction.kOutput, height); + m_fps = m_device.createInt("fps", Direction.kOutput, fps); + m_connected = m_device.createBoolean("connected", Direction.kOutput, true); + } + + m_defaultWidth = width; + m_defaultHeight = height; + m_defaultFps = fps; + + m_deviceName = name + "[" + deviceId + "]"; + m_frameServer = m_device != null ? CameraFrameServer.getInstance() : null; + } + + /** @return true if running in simulation (frames come from Fission) */ + public boolean isSimulated() { + return m_device != null; + } + + public void setResolution(int width, int height) { + if (m_width != null) { + m_width.set(width); + m_height.set(height); + } + } + + public void setFPS(int fps) { + if (m_fps != null) { + m_fps.set(fps); + } + } + + public void setConnected(boolean connected) { + if (m_connected != null) { + m_connected.set(connected); + } + } + + public int getWidth() { + return m_width != null ? m_width.get() : m_defaultWidth; + } + + public int getHeight() { + return m_height != null ? m_height.get() : m_defaultHeight; + } + + public int getFPS() { + return m_fps != null ? m_fps.get() : m_defaultFps; + } + + /** + * Latest JPEG frame received from Fission, or null. Callers must track the returned + * reference themselves to detect new frames (multiple consumers may grab concurrently). + */ + public byte[] getLatestFrame() { + return m_frameServer != null ? m_frameServer.getFrame(m_deviceName) : null; + } + + /** @return true if {@code bytes} decoded into {@code dst} */ + public static boolean decodeFrame(byte[] bytes, Mat dst) { + if (bytes == null || bytes.length == 0) { + return false; + } + + MatOfByte buffer = new MatOfByte(bytes); + Mat decoded = Imgcodecs.imdecode(buffer, Imgcodecs.IMREAD_COLOR); + buffer.release(); + + if (decoded.empty()) { + decoded.release(); + return false; + } + + decoded.copyTo(dst); + decoded.release(); + return true; + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CameraFrameServer.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CameraFrameServer.java new file mode 100644 index 0000000000..489581e404 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CameraFrameServer.java @@ -0,0 +1,94 @@ +package com.autodesk.synthesis.cscore; + +import java.net.InetSocketAddress; +import java.nio.ByteBuffer; +import java.nio.charset.StandardCharsets; +import java.util.Arrays; +import java.util.Base64; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; + +import org.java_websocket.WebSocket; +import org.java_websocket.handshake.ClientHandshake; +import org.java_websocket.server.WebSocketServer; + +/** + * localhost WebSocket server that receives rendered camera frames from Synthesis. Frames + * can't use HALSim, so Synthesis streams them here as binary {@code "\n"} + * messages; the latest frame per device is kept for {@link Camera#getLatestFrame}. + */ +public class CameraFrameServer extends WebSocketServer { + + public static final int PORT = 5808; + + private static CameraFrameServer instance; + + private final Map m_frames = new ConcurrentHashMap<>(); + + private CameraFrameServer(int port) { + super(new InetSocketAddress("localhost", port)); + setReuseAddr(true); + } + + public static synchronized CameraFrameServer getInstance() { + if (instance == null) { + instance = new CameraFrameServer(PORT); + instance.setDaemon(true); + instance.start(); + } + return instance; + } + + public byte[] getFrame(String device) { + return m_frames.get(device); + } + + @Override + public void onMessage(WebSocket conn, ByteBuffer message) { + byte[] data = new byte[message.remaining()]; + message.get(data); + + int idx = -1; + for (int i = 0; i < data.length; i++) { + if (data[i] == '\n') { + idx = i; + break; + } + } + if (idx < 0) { + return; + } + + String device = new String(data, 0, idx, StandardCharsets.UTF_8); + m_frames.put(device, Arrays.copyOfRange(data, idx + 1, data.length)); + } + + @Override + public void onMessage(WebSocket conn, String message) { + int idx = message.indexOf('\n'); + if (idx < 0) { + return; + } + try { + m_frames.put(message.substring(0, idx), Base64.getDecoder().decode(message.substring(idx + 1))); + } catch (IllegalArgumentException e) { + // ignore malformed frames + } + } + + @Override + public void onOpen(WebSocket conn, ClientHandshake handshake) { + } + + @Override + public void onClose(WebSocket conn, int code, String reason, boolean remote) { + } + + @Override + public void onError(WebSocket conn, Exception ex) { + } + + @Override + public void onStart() { + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CameraServer.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CameraServer.java new file mode 100644 index 0000000000..c56de013a1 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CameraServer.java @@ -0,0 +1,372 @@ +// Copyright (c) 2009-2026 FIRST and other WPILib contributors All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: +// +// Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +// Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. +// Neither the name of FIRST, WPILib, nor the names of other WPILib contributors may be used to endorse or promote products derived from this software without specific prior written permission. +// THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +package com.autodesk.synthesis.cscore; + +import edu.wpi.first.cscore.CameraServerJNI; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.MjpegServer; +import edu.wpi.first.cscore.VideoException; +import edu.wpi.first.cscore.VideoMode; +import edu.wpi.first.cscore.VideoSink; +import edu.wpi.first.cscore.VideoSource; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.PixelFormat; +import java.util.HashMap; +import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import org.opencv.core.Mat; + +/** + * Swap-in for {@code edu.wpi.first.cameraserver.CameraServer}; mirrors its API so user code only + * changes imports. Implemented locally (no delegation to the WPILib class) because both keep + * port counters starting at {@code kBasePort} (collision) and WPILib's global VideoListener + * would publish conflicting NetworkTables entries for our sources. + * + * In simulation, USB camera frames come from Fission over {@link CameraFrameServer}; a pump + * thread republishes them through a CvSource + MjpegServer so dashboards get a real MJPEG + * stream. Stream URLs are published under {@code CameraPublisher//streams} with a + * {@code 127.0.0.1} address, since CSCore's default mDNS hostname usually can't be resolved by + * Shuffleboard/Glass when running in Synthesis. + */ +public final class CameraServer { + public static final int kBasePort = 1181; + + private static final String kPublishName = "CameraPublisher"; + + private static final AtomicInteger m_defaultUsbDevice = new AtomicInteger(); + private static String m_primarySourceName; + private static final Map m_sources = new HashMap<>(); + private static final Map m_sinks = new HashMap<>(); + private static int m_nextPort = kBasePort; + + private CameraServer() {} + + /** @return backing sim Camera if {@code camera} is a simulated UsbCamera, else null */ + private static Camera simCamera(VideoSource camera) { + if (!(camera instanceof UsbCamera)) { + return null; + } + Camera c = ((UsbCamera) camera).getCamera(); + return c.isSimulated() ? c : null; + } + + private static void publishCamera(String name, boolean usb, boolean sim, VideoMode mode, int port) { + String address = sim ? "127.0.0.1" : CameraServerJNI.getHostname() + ".local"; + String modeString = mode.width + "x" + mode.height + " MJPEG " + mode.fps + " fps"; + NetworkTable table = NetworkTableInstance.getDefault().getTable(kPublishName).getSubTable(name); + table.getEntry("source").setString(usb ? "usb:" + name : "cv:"); + table.getEntry("description").setString(name); + table.getEntry("connected").setBoolean(true); + table.getEntry("mode").setString(modeString); + table.getEntry("modes").setStringArray(new String[] { modeString }); + table.getEntry("streams") + .setStringArray(new String[] { "mjpg:http://" + address + ":" + port + "/?action=stream" }); + } + + /** Decode Fission websocket frames and push into a CvSource the MjpegServer can serve. */ + private static CvSource startFramePump(UsbCamera camera) { + Camera simCamera = camera.getCamera(); + CvSource source = new CvSource(camera.getName() + "_synthesis", PixelFormat.kMJPEG, + simCamera.getWidth(), simCamera.getHeight(), simCamera.getFPS()); + + Thread pump = new Thread(() -> { + Mat frame = new Mat(); + byte[] lastFrame = null; + while (!Thread.interrupted()) { + byte[] bytes = simCamera.getLatestFrame(); + if (bytes != null && bytes != lastFrame) { + lastFrame = bytes; + if (Camera.decodeFrame(bytes, frame)) { + source.putFrame(frame); + } + } + try { + Thread.sleep(1000 / Math.max(1, simCamera.getFPS())); + } catch (InterruptedException e) { + return; + } + } + }, "Synthesis camera pump: " + camera.getName()); + pump.setDaemon(true); + pump.start(); + + return source; + } + + /** + * Start automatically capturing images to send to the dashboard. + * + *

The first time this overload is called, it calls {@link #startAutomaticCapture(int)} with + * device 0, creating a camera named "USB Camera 0". Subsequent calls increment the device + * number (e.g. 1, 2, etc). + * + * @return The USB camera capturing images. + */ + public static UsbCamera startAutomaticCapture() { + return startAutomaticCapture(m_defaultUsbDevice.getAndIncrement()); + } + + /** + * Start automatically capturing images to send to the dashboard. + * + * @param dev The device number of the camera interface + * @return The USB camera capturing images. + */ + public static UsbCamera startAutomaticCapture(int dev) { + UsbCamera camera = new UsbCamera("USB Camera " + dev, dev); + startAutomaticCapture(camera); + return camera; + } + + /** + * Start automatically capturing images to send to the dashboard. + * + * @param name The name to give the camera + * @param dev The device number of the camera interface + * @return The USB camera capturing images. + */ + public static UsbCamera startAutomaticCapture(String name, int dev) { + UsbCamera camera = new UsbCamera(name, dev); + startAutomaticCapture(camera); + return camera; + } + + // TODO: startAutomaticCapture(String name, String path) + + /** + * Start automatically capturing images to send to the dashboard from an existing camera. + * + * @param camera Camera + * @return The MJPEG server serving images from the given camera. + */ + public static synchronized MjpegServer startAutomaticCapture(VideoSource camera) { + addCamera(camera); + MjpegServer server = addServer("serve_" + camera.getName()); + + boolean usb = camera instanceof UsbCamera; + boolean sim = simCamera(camera) != null; + VideoMode mode; + if (sim) { + CvSource pumpSource = startFramePump((UsbCamera) camera); + server.setSource(pumpSource); + mode = pumpSource.getVideoMode(); + } else { + server.setSource(camera); + try { + mode = camera.getVideoMode(); + } catch (VideoException e) { + mode = new VideoMode(PixelFormat.kMJPEG, 0, 0, 0); + } + } + + publishCamera(camera.getName(), usb, sim, mode, server.getPort()); + return server; + } + + /** + * Adds a virtual camera for switching between two streams. Calling setSource() on the returned + * object can be used to switch the actual source of the stream. + * + * @param name The name to give the camera + * @return The MJPEG server serving images from the given camera. + */ + public static MjpegServer addSwitchedCamera(String name) { + CvSource source = new CvSource(name, PixelFormat.kMJPEG, 160, 120, 30); + return startAutomaticCapture(source); + } + + /** + * Get OpenCV access to the primary camera feed. This allows you to get images from the camera + * for image processing. + * + *

This is only valid to call after a camera feed has been added with + * startAutomaticCapture() or addServer(). + * + * @return OpenCV sink for the primary camera feed + */ + public static CvSink getVideo() { + VideoSource source; + synchronized (CameraServer.class) { + if (m_primarySourceName == null) { + throw new VideoException("no camera available"); + } + source = m_sources.get(m_primarySourceName); + } + if (source == null) { + throw new VideoException("no camera available"); + } + return getVideo(source); + } + + /** + * Get OpenCV access to the specified camera. This allows you to get images from the camera for + * image processing. + * + * @param camera Camera (e.g. as returned by startAutomaticCapture). + * @return OpenCV sink for the specified camera + */ + public static CvSink getVideo(VideoSource camera) { + String name = "opencv_" + camera.getName(); + + synchronized (CameraServer.class) { + VideoSink sink = m_sinks.get(name); + if (sink != null) { + if (!(sink instanceof CvSink)) { + throw new VideoException("expected OpenCV sink, but got " + sink.getKind()); + } + return (CvSink) sink; + } + } + + CvSink newsink = new CvSink(name, simCamera(camera)); + newsink.setSource(camera); + addServer(newsink); + return newsink; + } + + /** + * Get OpenCV access to the specified camera. This allows you to get images from the camera for + * image processing. + * + * @param name Camera name + * @return OpenCV sink for the specified camera + */ + public static CvSink getVideo(String name) { + VideoSource source; + synchronized (CameraServer.class) { + source = m_sources.get(name); + } + if (source == null) { + throw new VideoException("could not find camera " + name); + } + return getVideo(source); + } + + /** + * Create a MJPEG stream with OpenCV input. This can be called to pass custom annotated images + * to the dashboard. + * + * @param name Name to give the stream + * @param width Width of the image being sent + * @param height Height of the image being sent + * @return OpenCV source for the MJPEG stream + */ + public static CvSource putVideo(String name, int width, int height) { + CvSource source = new CvSource(name, PixelFormat.kMJPEG, width, height, 30); + startAutomaticCapture(source); + return source; + } + + /** + * Adds a MJPEG server at the next available port. + * + * @param name Server name + * @return The MJPEG server + */ + public static MjpegServer addServer(String name) { + int port; + synchronized (CameraServer.class) { + port = m_nextPort; + m_nextPort++; + } + return addServer(name, port); + } + + /** + * Adds a MJPEG server. + * + * @param name Server name + * @param port Server port + * @return The MJPEG server + */ + public static MjpegServer addServer(String name, int port) { + MjpegServer server = new MjpegServer(name, port); + addServer(server); + return server; + } + + /** + * Adds an already created server. + * + * @param server Server + */ + public static void addServer(VideoSink server) { + synchronized (CameraServer.class) { + m_sinks.put(server.getName(), server); + } + } + + /** + * Removes a server by name. + * + * @param name Server name + */ + public static void removeServer(String name) { + synchronized (CameraServer.class) { + m_sinks.remove(name); + } + } + + /** + * Get server for the primary camera feed. + * + *

This is only valid to call after a camera feed has been added with + * startAutomaticCapture() or addServer(). + * + * @return The server for the primary camera feed + */ + public static VideoSink getServer() { + synchronized (CameraServer.class) { + if (m_primarySourceName == null) { + throw new VideoException("no camera available"); + } + return getServer("serve_" + m_primarySourceName); + } + } + + /** + * Gets a server by name. + * + * @param name Server name + * @return The server + */ + public static VideoSink getServer(String name) { + synchronized (CameraServer.class) { + return m_sinks.get(name); + } + } + + /** + * Adds an already created camera. + * + * @param camera Camera + */ + public static void addCamera(VideoSource camera) { + String name = camera.getName(); + synchronized (CameraServer.class) { + if (m_primarySourceName == null) { + m_primarySourceName = name; + } + m_sources.put(name, camera); + } + } + + /** + * Removes a camera by name. + * + * @param name Camera name + */ + public static void removeCamera(String name) { + synchronized (CameraServer.class) { + m_sources.remove(name); + } + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CvSink.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CvSink.java new file mode 100644 index 0000000000..74ce9fe072 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/CvSink.java @@ -0,0 +1,54 @@ +// Copyright (c) 2009-2026 FIRST and other WPILib contributors All rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: +// +// Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +// Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. +// Neither the name of FIRST, WPILib, nor the names of other WPILib contributors may be used to endorse or promote products derived from this software without specific prior written permission. +// THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +package com.autodesk.synthesis.cscore; + +import org.opencv.core.Mat; + +import edu.wpi.first.util.WPIUtilJNI; + +/** + * Swap-in for {@code edu.wpi.first.cscore.CvSink} + */ +public class CvSink extends edu.wpi.first.cscore.CvSink { + + private Camera m_camera; + private byte[] m_lastFrame; + + /** If camera is null, behave as real cscore CvSink */ + public CvSink(String name, Camera camera) { + super(name); + this.m_camera = camera; + } + + @Override + public long grabFrame(Mat image) { + return m_camera == null ? super.grabFrame(image) : grabFrameNoTimeout(image); + } + + @Override + public long grabFrame(Mat image, double timeout) { + return m_camera == null ? super.grabFrame(image, timeout) : grabFrameNoTimeout(image); + } + + @Override + public long grabFrameNoTimeout(Mat image) { + if (m_camera == null) { + return super.grabFrameNoTimeout(image); + } + + byte[] bytes = m_camera.getLatestFrame(); + if (bytes == null || bytes == m_lastFrame || !Camera.decodeFrame(bytes, image)) { + return 0L; + } + m_lastFrame = bytes; + + return WPIUtilJNI.now(); + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/UsbCamera.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/UsbCamera.java new file mode 100644 index 0000000000..11514a1142 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/cscore/UsbCamera.java @@ -0,0 +1,52 @@ +package com.autodesk.synthesis.cscore; + +/** + * Swap-in for {@code edu.wpi.first.cscore.UsbCamera} + * + * 

+ *     var camera = CameraServer.startAutomaticCapture();
+ *     var sink = CameraServer.getVideo();
+ *     Mat frame = new Mat();
+ *     if (sink.grabFrame(frame) != 0) {
+ *         // ... run vision processing on frame ...
+ *     }
+ *
+ */ +public class UsbCamera extends edu.wpi.first.cscore.UsbCamera { + + private Camera m_camera; + + public UsbCamera(String name, int dev) { + this(name, dev, 640, 480, 30); + } + + public UsbCamera(String name, int dev, int width, int height, int fps) { + super(name, dev); + this.m_camera = new Camera(name, dev, width, height, fps); + } + + Camera getCamera() { + return this.m_camera; + } + + public CvSink getVideo() { + return CameraServer.getVideo(this); + } + + @Override + public boolean setResolution(int width, int height) { + this.m_camera.setResolution(width, height); + + if (!this.m_camera.isSimulated()) { + return super.setResolution(width, height); + } + + return true; + } + + @Override + public boolean setFPS(int fps) { + this.m_camera.setFPS(fps); + return super.setFPS(fps); + } +} diff --git a/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java index f5694a80c3..d3c0a7a783 100644 --- a/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java @@ -8,6 +8,11 @@ import com.autodesk.synthesis.io.*; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Scalar; + +import edu.wpi.first.cscore.CvSource; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.ADXL362; @@ -17,6 +22,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.XboxController; +import com.autodesk.synthesis.cscore.CameraServer; +import com.autodesk.synthesis.cscore.CvSink; +import com.autodesk.synthesis.cscore.UsbCamera; import com.autodesk.synthesis.revrobotics.spark.SparkMax; import com.autodesk.synthesis.revrobotics.RelativeEncoder; import com.autodesk.synthesis.revrobotics.SparkAbsoluteEncoder; @@ -51,6 +59,15 @@ public class Robot extends TimedRobot { private double m_initAngle = 0; + // name and device index must match the camera configured in Synthesis (Configure -> + // USB Cameras) + private static final int kCameraWidth = 640; + private static final int kCameraHeight = 480; + private UsbCamera m_camera; + private CvSink m_cvSink; + private CvSource m_outputStream; + private Mat m_frame; + /** * This function is run when the robot is first started up and should be used * for any @@ -66,6 +83,15 @@ public void robotInit() { // 4 inch diameter wheels, default is 1 unit = 1 radian. // Following conversion factor is 1 unit = 1 inch travelled. m_encoder.setPositionConversionFactor(2.0); + + m_camera = CameraServer.startAutomaticCapture("USB Camera 0", 0); + m_camera.setResolution(kCameraWidth, kCameraHeight); + UsbCamera camera1 = CameraServer.startAutomaticCapture("USB Camera 1", 1); + camera1.setResolution(kCameraWidth, kCameraHeight); + + m_cvSink = CameraServer.getVideo(); + m_outputStream = CameraServer.putVideo("Synthesis Camera", kCameraWidth, kCameraHeight); + m_frame = new Mat(); } /** @@ -96,6 +122,27 @@ public void robotPeriodic() { SmartDashboard.putNumber("AHRS/VelX", m_Gyro.getVelocityX()); SmartDashboard.putNumber("AHRS/VelY", m_Gyro.getVelocityY()); SmartDashboard.putNumber("AHRS/VelZ", m_Gyro.getVelocityZ()); + + // Grab the latest frame rendered by Synthesis and report some basic diagnostics so + // the camera can be verified end-to-end. + if (m_cvSink == null) { + return; + } + + long frameTime = m_cvSink.grabFrame(m_frame); + if (frameTime != 0 && !m_frame.empty()) { + Scalar mean = Core.mean(m_frame); + double brightness = (mean.val[0] + mean.val[1] + mean.val[2]) / 3.0; + + SmartDashboard.putBoolean("Camera/Frame Received", true); + SmartDashboard.putNumber("Camera/Width", m_frame.width()); + SmartDashboard.putNumber("Camera/Height", m_frame.height()); + SmartDashboard.putNumber("Camera/Mean Brightness", brightness); + + m_outputStream.putFrame(m_frame); + } else { + SmartDashboard.putBoolean("Camera/Frame Received", false); + } } /**