diff --git a/fission/src/systems/simulation/wpilib_brain/SimCameraRenderer.ts b/fission/src/systems/simulation/wpilib_brain/SimCameraRenderer.ts new file mode 100644 index 0000000000..d6194a7695 --- /dev/null +++ b/fission/src/systems/simulation/wpilib_brain/SimCameraRenderer.ts @@ -0,0 +1,140 @@ +import * as THREE from "three" +import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import World from "@/systems/World" + +export class SimCameraRenderer { + private _camera: THREE.PerspectiveCamera + private _renderTarget: THREE.WebGLRenderTarget + private _canvas: OffscreenCanvas + private _ctx: OffscreenCanvasRenderingContext2D + private _robot: MirabufSceneObject + private _cameraPosition: THREE.Vector3 + private _cameraQuaternion: THREE.Quaternion + + constructor(robot: MirabufSceneObject, width: number = 640, height: number = 480) { + this._robot = robot + + // Create camera for robot perspective + this._camera = new THREE.PerspectiveCamera(75, width / height, 0.1, 1000) + + // Create render target for off-screen rendering + this._renderTarget = new THREE.WebGLRenderTarget(width, height, { + minFilter: THREE.LinearFilter, + magFilter: THREE.LinearFilter, + format: THREE.RGBAFormat, + }) + + // Create canvas for frame capture + this._canvas = new OffscreenCanvas(width, height) + this._ctx = this._canvas.getContext("2d")! + + // Camera position relative to robot + this._cameraPosition = new THREE.Vector3(0, 0.5, 0.2) // Mounted on robot + this._cameraQuaternion = new THREE.Quaternion() + + this.updateCameraTransform() + } + + private updateCameraTransform() { + if (!this._robot.mechanism.rootBody) return + + // Get robot's transform + const robotBody = World.physicsSystem.getBody( + this._robot.mechanism.nodeToBody.get(this._robot.mechanism.rootBody)! + ) + + if (!robotBody) return + + const robotPos = robotBody.GetPosition() + const robotRot = robotBody.GetRotation() + + const robotPosition = new THREE.Vector3(robotPos.GetX(), robotPos.GetY(), robotPos.GetZ()) + const robotQuaternion = new THREE.Quaternion(robotRot.GetX(), robotRot.GetY(), robotRot.GetZ(), robotRot.GetW()) + + const worldCameraPos = this._cameraPosition.clone() + worldCameraPos.applyQuaternion(robotQuaternion) + worldCameraPos.add(robotPosition) + + const cameraRotation = new THREE.Quaternion() + cameraRotation.copy(robotQuaternion) + + const forwardFix = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(0, 1, 0), Math.PI) + const upFix = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(0, 0, 1), Math.PI) + + cameraRotation.multiply(forwardFix).multiply(upFix) + + this._camera.position.copy(worldCameraPos) + this._camera.quaternion.copy(cameraRotation) + this._camera.updateMatrixWorld() + } + + public renderFrame(): ImageData | null { + if (!World.sceneRenderer) return null + + this.updateCameraTransform() + + // Render scene from camera perspective + const renderer = (World.sceneRenderer as any)._renderer as THREE.WebGLRenderer + const scene = (World.sceneRenderer as any)._scene as THREE.Scene + + // Store original render target + const originalTarget = renderer.getRenderTarget() + + // Render to our target + renderer.setRenderTarget(this._renderTarget) + renderer.render(scene, this._camera) + + // Read pixels + const pixels = new Uint8Array(this._renderTarget.width * this._renderTarget.height * 4) + renderer.readRenderTargetPixels( + this._renderTarget, + 0, + 0, + this._renderTarget.width, + this._renderTarget.height, + pixels + ) + + // Restore original target + renderer.setRenderTarget(originalTarget) + + // Convert to ImageData + const imageData = new ImageData( + new Uint8ClampedArray(pixels), + this._renderTarget.width, + this._renderTarget.height + ) + + return imageData + } + + public captureFrameAsJPEG(): Promise { + const imageData = this.renderFrame() + if (!imageData) return Promise.reject("No frame data") + + // Draw to canvas + this._ctx.putImageData(imageData, 0, 0) + + // Convert to JPEG blob + return this._canvas.convertToBlob({ type: "image/jpeg", quality: 0.8 }) + } + + public setResolution(width: number, height: number) { + this._renderTarget.setSize(width, height) + this._canvas.width = width + this._canvas.height = height + this._camera.aspect = width / height + this._camera.updateProjectionMatrix() + } + + public setCameraOffset(position: THREE.Vector3, rotation?: THREE.Quaternion) { + this._cameraPosition.copy(position) + if (rotation) { + this._cameraQuaternion.copy(rotation) + } + } + + public dispose() { + this._renderTarget.dispose() + } +} diff --git a/fission/src/systems/simulation/wpilib_brain/SimCameraVisualization.ts b/fission/src/systems/simulation/wpilib_brain/SimCameraVisualization.ts new file mode 100644 index 0000000000..f1cf9386d5 --- /dev/null +++ b/fission/src/systems/simulation/wpilib_brain/SimCameraVisualization.ts @@ -0,0 +1,94 @@ +import * as THREE from "three" +import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import SceneObject from "@/systems/scene/SceneObject" +import World from "@/systems/World" + +/** + * Visual representation of the WPILib camera in the 3D scene + * Shows where the camera is mounted and its field of view + */ +export class SimCameraVisualization extends SceneObject { + private _robot: MirabufSceneObject + private _cameraGroup: THREE.Group + private _cameraPosition: THREE.Vector3 + private _isVisible: boolean = false + + constructor(robot: MirabufSceneObject) { + super() + this._robot = robot + + this._cameraPosition = new THREE.Vector3(0, 0.5, 0.2) + + this._cameraGroup = new THREE.Group() + } + + public setup(): void { + World.sceneRenderer.addObject(this._cameraGroup) + console.log("[VISUAL] SimCameraVisualization added to scene") + } + + public update(): void { + if (!this._isVisible) return + + this.updateCameraTransform() + } + + private updateCameraTransform() { + if (!this._robot.mechanism.rootBody) return + + const robotBody = World.physicsSystem.getBody( + this._robot.mechanism.nodeToBody.get(this._robot.mechanism.rootBody)! + ) + + if (!robotBody) return + + const robotPos = robotBody.GetPosition() + const robotRot = robotBody.GetRotation() + + const robotPosition = new THREE.Vector3(robotPos.GetX(), robotPos.GetY(), robotPos.GetZ()) + const robotQuaternion = new THREE.Quaternion(robotRot.GetX(), robotRot.GetY(), robotRot.GetZ(), robotRot.GetW()) + + const worldCameraPos = this._cameraPosition.clone() + worldCameraPos.applyQuaternion(robotQuaternion) + worldCameraPos.add(robotPosition) + + const cameraRotation = new THREE.Quaternion() + cameraRotation.copy(robotQuaternion) + const forwardFix = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(0, 1, 0), Math.PI) + const upFix = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(0, 0, 1), Math.PI) + cameraRotation.multiply(forwardFix).multiply(upFix) + + this._cameraGroup.position.copy(worldCameraPos) + this._cameraGroup.quaternion.copy(cameraRotation) + } + + public setVisible(visible: boolean) { + this._isVisible = visible + this._cameraGroup.visible = visible + + // if (visible) { + // console.log("๐Ÿ“น [VISUAL] Camera visualization enabled - you should see a camera model on your robot") + // } else { + // console.log("๐Ÿ“น [VISUAL] Camera visualization disabled") + // } + } + + public dispose(): void { + if (this._cameraGroup.parent) { + this._cameraGroup.parent.remove(this._cameraGroup) + } + + this._cameraGroup.traverse(child => { + if (child instanceof THREE.Mesh) { + child.geometry.dispose() + if (Array.isArray(child.material)) { + child.material.forEach(material => material.dispose()) + } else { + child.material.dispose() + } + } + }) + + console.log("[VISUAL] SimCameraVisualization disposed") + } +} diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 5ddad97617..9aa21f4d32 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,3 +1,15 @@ +import type Jolt from "@azaleacolburn/jolt-physics" +import * as THREE from "three" +import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import type Mechanism from "@/systems/physics/Mechanism" +import World from "@/systems/World" +import JOLT from "@/util/loading/JoltSyncLoader" +import { convertJoltQuatToThreeQuaternion, convertJoltVec3ToThreeVector3 } from "@/util/TypeConversions" +import type EncoderStimulus from "../stimulus/EncoderStimulus" +import { SimCameraRenderer } from "./SimCameraRenderer" +import { SimCameraVisualization } from "./SimCameraVisualization" +import { SimAccel, SimAI, SimCANEncoder, SimCamera, SimDIO, SimGeneric, SimGyro } from "./WPILibBrain" + export abstract class SimInput { constructor(protected _device: string) {} @@ -7,3 +19,359 @@ export abstract class SimInput { return this._device } } + +export class SimEncoderInput extends SimInput { + private _stimulus: EncoderStimulus + + constructor(device: string, stimulus: EncoderStimulus) { + super(device) + this._stimulus = stimulus + } + + public update(_deltaT: number) { + SimCANEncoder.setPosition(this._device, this._stimulus.positionValue) + SimCANEncoder.setVelocity(this._device, this._stimulus.velocityValue) + } +} + +export class SimGyroInput extends SimInput { + private _robot: Mechanism + private _joltID?: Jolt.BodyID + private _joltBody?: Jolt.Body + + private static readonly AXIS_X: Jolt.Vec3 = new JOLT.Vec3(1, 0, 0) + private static readonly AXIS_Y: Jolt.Vec3 = new JOLT.Vec3(0, 1, 0) + private static readonly AXIS_Z: Jolt.Vec3 = new JOLT.Vec3(0, 0, 1) + + constructor(device: string, robot: Mechanism) { + super(device) + this._robot = robot + this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) + + if (this._joltID) this._joltBody = World.physicsSystem.getBody(this._joltID) + } + + private getAxis(axis: Jolt.Vec3): number { + return ((this._joltBody?.GetRotation().GetRotationAngle(axis) ?? 0) * 180) / Math.PI + } + + private getX(): number { + return this.getAxis(SimGyroInput.AXIS_X) + } + + private getY(): number { + return this.getAxis(SimGyroInput.AXIS_Y) + } + + private getZ(): number { + return this.getAxis(SimGyroInput.AXIS_Z) + } + + private getAxisVelocity(axis: "x" | "y" | "z"): number { + const axes = this._joltBody?.GetAngularVelocity() + if (!axes) return 0 + + switch (axis) { + case "x": + return axes.GetX() + case "y": + return axes.GetY() + case "z": + return axes.GetZ() + } + } + + public update(_deltaT: number) { + const x = this.getX() + const y = this.getY() + const z = this.getZ() + + SimGyro.setAngleX(this._device, x) + SimGyro.setAngleY(this._device, y) + SimGyro.setAngleZ(this._device, z) + SimGyro.setRateX(this._device, this.getAxisVelocity("x")) + SimGyro.setRateY(this._device, this.getAxisVelocity("y")) + SimGyro.setRateZ(this._device, this.getAxisVelocity("z")) + } +} + +export class SimAccelInput extends SimInput { + private _robot: Mechanism + private _joltID?: Jolt.BodyID + private _prevVel: THREE.Vector3 + + constructor(device: string, robot: Mechanism) { + super(device) + this._robot = robot + this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) + this._prevVel = new THREE.Vector3(0, 0, 0) + } + + public update(deltaT: number) { + if (!this._joltID) return + const body = World.physicsSystem.getBody(this._joltID) + + const rot = convertJoltQuatToThreeQuaternion(body.GetRotation()) + const mat = new THREE.Matrix4().makeRotationFromQuaternion(rot).transpose() + const newVel = convertJoltVec3ToThreeVector3(body.GetLinearVelocity()).applyMatrix4(mat) + + const x = (newVel.x - this._prevVel.x) / deltaT + const y = (newVel.y - this._prevVel.y) / deltaT + const z = (newVel.z - this._prevVel.z) / deltaT + + SimAccel.setX(this._device, x) + SimAccel.setY(this._device, y) + SimAccel.setZ(this._device, z) + + this._prevVel = newVel + } +} + +export class SimCameraInput extends SimInput { + private _defaultWidth: number = 320 + private _defaultHeight: number = 240 + private _defaultFPS: number = 30 + private _isInitialized: boolean = false + private _cameraRenderer?: SimCameraRenderer + private _cameraVisualization: SimCameraVisualization + private _robot: MirabufSceneObject + private _frameInterval: number = 0 + private _lastFrameTime: number = 0 + + constructor(device: string, robot: MirabufSceneObject, width?: number, height?: number, fps?: number) { + super(device) + this._robot = robot + if (width) this._defaultWidth = width + if (height) this._defaultHeight = height + if (fps) this._defaultFPS = fps + this._frameInterval = 1000 / this._defaultFPS // ms between frames + + // Create camera visualization + this._cameraVisualization = new SimCameraVisualization(robot) + this._cameraVisualization.setup() + + console.log( + `[CONSTRUCTOR] SimCameraInput created for ${device} (${this._defaultWidth}x${this._defaultHeight} @ ${this._defaultFPS}fps, interval=${this._frameInterval}ms)` + ) + } + + public update(deltaT: number) { + // if (Math.random() < 0.01) { + // console.log( + // `๐Ÿ”„ [UPDATE] SimCameraInput.update() called for ${this.device} (initialized: ${this._isInitialized})` + // ) + // } + + if (!this._isInitialized) { + this.initializeCamera() + this._isInitialized = true + } + + this.updateCameraSettings() + this.generateVideoFrame(deltaT) + + // Update camera visualization + this._cameraVisualization.update() + } + + private initializeCamera() { + console.log(`[INIT] Starting camera initialization for ${this.device}`) + + // Initialize metadata + SimCamera.setConnected(this.device, true) + SimCamera.setResolutionWidth(this.device, this._defaultWidth) + SimCamera.setResolutionHeight(this.device, this._defaultHeight) + SimCamera.setFPS(this.device, this._defaultFPS) + SimCamera.setBrightness(this.device, 50) + SimCamera.setExposure(this.device, 50) + SimCamera.setAutoExposure(this.device, true) + + // console.log(`๐ŸŽฅ [INIT] Camera metadata set, creating renderer...`) + + try { + // Initialize video renderer for 3D scene capture + this._cameraRenderer = new SimCameraRenderer(this._robot, this._defaultWidth, this._defaultHeight) + console.log(`[INIT] Camera ${this.device} initialized successfully - 3D frames will be generated`) + + // Show camera visualization in 3D scene + this._cameraVisualization.setVisible(true) + + // Force immediate test frame to verify renderer works + // console.log(`[TEST] Attempting immediate test frame capture...`) + this._cameraRenderer + .captureFrameAsJPEG() + .then(blob => { + // console.log(`๐Ÿงช [TEST] Initial test frame captured: ${blob.size} bytes - renderer is working!`) + this.sendFrameToRobot(blob) + }) + .catch(error => { + console.error(`[TEST] Initial test frame failed:`, error) + }) + } catch (error) { + console.error(`[INIT] Failed to create camera renderer:`, error) + } + } + + private updateCameraSettings() { + const requestedWidth = SimCamera.getRequestedWidth(this._device) + const requestedHeight = SimCamera.getRequestedHeight(this._device) + const requestedFPS = SimCamera.getRequestedFPS(this._device) + + // Check if resolution changed + const currentWidth = SimCamera.getRequestedWidth(this._device) + const currentHeight = SimCamera.getRequestedHeight(this._device) + if (requestedWidth !== currentWidth || requestedHeight !== currentHeight) { + this.updateResolution(requestedWidth, requestedHeight) + } + + // Check if frame rate changed - ensure FPS is valid + if (requestedFPS && !isNaN(requestedFPS) && requestedFPS > 0 && requestedFPS !== this._defaultFPS) { + this.updateFrameRate(requestedFPS) + this._defaultFPS = requestedFPS + } + + SimCamera.setResolutionWidth(this.device, requestedWidth) + SimCamera.setResolutionHeight(this.device, requestedHeight) + SimCamera.setFPS(this.device, requestedFPS) + + const requestedBrightness = SimCamera.getRequestedBrightness(this._device) + const requestedExposure = SimCamera.getRequestedExposure(this._device) + const requestedAutoExposure = SimCamera.getRequestedAutoExposure(this._device) + + SimCamera.setBrightness(this.device, requestedBrightness) + SimCamera.setExposure(this.device, requestedExposure) + SimCamera.setAutoExposure(this.device, requestedAutoExposure) + } + + private generateVideoFrame(deltaT: number) { + if (!this._cameraRenderer) { + // console.warn(`๐Ÿ“น [FRAME] No camera renderer for ${this.device} - skipping frame generation`) + return + } + + this._lastFrameTime += deltaT * 1000 + + // Add timing debug logs occasionally + // if (Math.random() < 0.01) { + // console.log( + // `โฑ[TIMING] ${this.device}: lastFrameTime=${this._lastFrameTime.toFixed(1)}ms, interval=${this._frameInterval}ms, deltaT=${(deltaT * 1000).toFixed(1)}ms` + // ) + // } + + // Generate frame at specified FPS + if (this._lastFrameTime >= this._frameInterval) { + this._lastFrameTime = 0 + + // console.log(`[FRAME] Capturing 3D frame for ${this.device}`) + + // Capture frame from 3D scene (robot perspective) + this._cameraRenderer + .captureFrameAsJPEG() + .then(blob => { + console.log(`[FRAME] Successfully captured ${blob.size} bytes, sending to robot`) + this.sendFrameToRobot(blob) + }) + .catch(error => { + console.error(`[FRAME] Failed to capture camera frame:`, error) + }) + } + } + + private async sendFrameToRobot(frameBlob: Blob) { + try { + const arrayBuffer = await frameBlob.arrayBuffer() + const base64Frame = btoa(String.fromCharCode(...new Uint8Array(arrayBuffer))) + + console.log(`[SEND] Converting frame: ${arrayBuffer.byteLength} bytes โ†’ ${base64Frame.length} chars`) + + // Send frame through WebSocket protocol + const frameMessage = { + type: "CAMERA_FRAME", + device: this.device, + data: { + frame: base64Frame, + width: this._defaultWidth, + height: this._defaultHeight, + timestamp: Date.now(), + }, + } + + // Send through the existing WebSocket worker + const success = SimGeneric.sendCameraFrame(this.device, frameMessage.data) + console.log(`[SEND] WebSocket frame sent for ${this.device}: ${success ? "SUCCESS" : "FAILED"}`) + } catch (error) { + console.error(`[SEND] Failed to send camera frame:`, error) + } + } + + private updateResolution(width: number, height: number) { + if (this._cameraRenderer) { + this._cameraRenderer.setResolution(width, height) + } + } + + private updateFrameRate(fps: number) { + this._frameInterval = 1000 / fps + } + + public reconnect() { + SimCamera.setConnected(this._device, true) + this._isInitialized = false // set false so reinitialize on next update + } + + public disconnect() { + SimCamera.setConnected(this._device, false) + this._isInitialized = false + this._cameraVisualization.setVisible(false) + + if (this._cameraRenderer) { + this._cameraRenderer.dispose() + this._cameraRenderer = undefined + } + } + + public dispose() { + this.disconnect() + this._cameraVisualization.dispose() + } +} + +export class SimDigitalInput extends SimInput { + private _valueSupplier: () => boolean + + /** + * Creates a Simulation Digital Input object. + * + * @param device Device ID + * @param valueSupplier Called each frame and returns what the value should be set to + */ + constructor(device: string, valueSupplier: () => boolean) { + super(device) + this._valueSupplier = valueSupplier + } + + private setValue(value: boolean) { + SimDIO.setValue(this._device, value) + } + + public getValue(): boolean { + return SimDIO.getValue(this._device) + } + + public update(_deltaT: number) { + if (this._valueSupplier) this.setValue(this._valueSupplier()) + } +} + +export class SimAnalogInput extends SimInput { + private _valueSupplier: () => number + + constructor(device: string, valueSupplier: () => number) { + super(device) + this._valueSupplier = valueSupplier + } + + public update(_deltaT: number) { + SimAI.setValue(this._device, this._valueSupplier()) + } +} diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 19de6a1faa..0e09676282 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -1,18 +1,571 @@ -import type MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import PreferencesSystem from "@/systems/preferences/PreferencesSystem" import World from "@/systems/World" import { random } from "@/util/Random" import Brain from "../Brain" +import { type NoraNumber, type NoraNumber2, type NoraNumber3, NoraTypes } from "../Nora" import type { SimulationLayer } from "../SimulationSystem" import SynthesisBrain from "../synthesis_brain/SynthesisBrain" -import { type SimFlow, validate } from "./SimDataFlow" -import type { SimInput } from "./SimInput" +import { type SimFlow, type SimReceiver, type SimSupplier, validate } from "./SimDataFlow" +import { SimAccelInput, SimAnalogInput, SimCameraInput, SimDigitalInput, SimGyroInput, type SimInput } from "./SimInput" import { SimAnalogOutput, SimDigitalOutput, type SimOutput } from "./SimOutput" -import { SimAccelInput } from "./sim/SimAccel" -import { SimAnalogInput } from "./sim/SimAI" -import { SimDigitalInput } from "./sim/SimDIO" -import { SimGyroInput } from "./sim/SimGyro" -import { getSimBrain, getSimMap, setConnected, setSimBrain } from "./WPILibState" -import { type DeviceData, SimMapUpdateEvent, SimType, type WSMessage, worker } from "./WPILibTypes" +import * as WPILibState from "./WPILibState" +import { SimMapUpdateEvent, worker } from "./WPILibTypes" + +const PWM_SPEED = "" ? FieldType.BOTH : FieldType.READ + case ">": + return FieldType.WRITE + default: + return FieldType.UNKNOWN + } +} + +type DeviceName = string +type DeviceData = Map + +type SimMap = Map> +export const simMaps = new Map() + +let simBrain: WPILibBrain | undefined +export function setSimBrain(brain: WPILibBrain | undefined) { + if (brain && !simMaps.has(brain.assemblyName)) { + simMaps.set(brain.assemblyName, new Map()) + } + if (simBrain) worker.getValue().postMessage({ command: "disable" }) + simBrain = brain + if (simBrain) + worker.getValue().postMessage({ + command: "enable", + reconnect: PreferencesSystem.getGlobalPreference("SimAutoReconnect"), + }) +} + +export function hasSimBrain() { + return simBrain != undefined +} + +export function getSimMap(): SimMap | undefined { + if (!simBrain) return undefined + return simMaps.get(simBrain.assemblyName) +} + +export class SimGeneric { + private constructor() {} + + public static getUnsafe(simType: SimType, device: string, field: string): T | undefined + public static getUnsafe(simType: SimType, device: string, field: string, defaultValue: T): T + public static getUnsafe(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined { + const map = getSimMap()?.get(simType) + if (!map) { + // console.warn(`No '${simType}' devices found`) + return undefined + } + + const data = map.get(device) + if (!data) { + // console.warn(`No '${simType}' device '${device}' found`) + return undefined + } + + return (data.get(field) as T | undefined) ?? defaultValue + } + + public static get(simType: SimType, device: string, field: string): T | undefined + public static get(simType: SimType, device: string, field: string, defaultValue: T): T + public static get(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined { + const fieldType = getFieldType(field) + if (fieldType != FieldType.READ && fieldType != FieldType.BOTH) { + console.warn(`Field '${field}' is not a read or both field type`) + return undefined + } + + const map = getSimMap()?.get(simType) + if (!map) { + // console.warn(`No '${simType}' devices found`) + return undefined + } + + const data = map.get(device) + if (!data) { + // console.warn(`No '${simType}' device '${device}' found`) + return undefined + } + + return (data.get(field) as T | undefined) ?? defaultValue + } + + public static set( + simType: SimType, + device: string, + field: string, + value: T + ): boolean { + const fieldType = getFieldType(field) + if (fieldType != FieldType.WRITE && fieldType != FieldType.BOTH) { + console.warn(`Field '${field}' is not a write or both field type`) + return false + } + + const map = getSimMap()?.get(simType) + if (!map) { + // console.warn(`No '${simType}' devices found`) + return false + } + + const data = map.get(device) + if (!data) { + // console.warn(`No '${simType}' device '${device}' found`) + return false + } + + const selectedData: { [key: string]: number | boolean | string } = {} + selectedData[field] = value + data.set(field, value) + + worker.getValue().postMessage({ + command: "update", + data: { + type: simType, + device: device, + data: selectedData, + }, + }) + + window.dispatchEvent(new SimMapUpdateEvent(true)) + return true + } + + public static sendCameraFrame(device: string, frameData: any): boolean { + worker.getValue().postMessage({ + command: "camera_frame", + data: { + device: device, + ...frameData, + }, + }) + return true + } +} + +export class SimDriverStation { + private constructor() {} + + public static setMatchTime(time: number) { + SimGeneric.set(SimType.DRIVERS_STATION, "", ">match_time", time) + } + + public static setGameData(gameData: string) { + SimGeneric.set(SimType.DRIVERS_STATION, "", ">match_time", gameData) + } + + public static isEnabled(): boolean { + return SimGeneric.getUnsafe(SimType.DRIVERS_STATION, "", ">enabled", false) + } + + public static setMode(mode: RobotSimMode) { + SimGeneric.set(SimType.DRIVERS_STATION, "", ">enabled", mode != RobotSimMode.DISABLED) + SimGeneric.set(SimType.DRIVERS_STATION, "", ">autonomous", mode == RobotSimMode.AUTO) + } + + public static setStation(station: AllianceStation) { + SimGeneric.set(SimType.DRIVERS_STATION, "", ">station", station) + } +} + +export class SimPWM { + private constructor() {} + + public static getSpeed(device: string): number | undefined { + return SimDriverStation.isEnabled() ? SimGeneric.get(SimType.PWM, device, PWM_SPEED, 0.0) : 0.0 + } + + public static getPosition(device: string): number | undefined { + return SimGeneric.get(SimType.PWM, device, PWM_POSITION, 0.0) + } + + public static genSupplier(device: string): SimSupplier { + return { + getSupplierType: () => supplierTypeMap[SimType.PWM]!, + getSupplierValue: () => SimPWM.getSpeed(device) ?? 0, + } + } +} + +export class SimCAN { + private constructor() {} + + public static getDeviceWithID(id: number, type: SimType): DeviceData | undefined { + const idExp = /SYN.*\[(\d+)\]/g + const map = getSimMap() + if (!map) return undefined + const entries = [...map.entries()].filter(([simType, _data]) => simType == type) + for (const [_simType, data] of entries) { + for (const key of data.keys()) { + const result = [...key.matchAll(idExp)] + if (result?.length <= 0 || result[0].length <= 1) continue + const parsedId = parseInt(result[0][1]) + if (parsedId != id) continue + return data.get(key) + } + } + return undefined + } +} + +export class SimCANMotor { + private constructor() {} + + public static getPercentOutput(device: string): number | undefined { + return SimDriverStation.isEnabled() + ? SimGeneric.get(SimType.CAN_MOTOR, device, CANMOTOR_PERCENT_OUTPUT, 0.0) + : 0.0 + } + + public static getBrakeMode(device: string): number | undefined { + return SimGeneric.get(SimType.CAN_MOTOR, device, CANMOTOR_BRAKE_MODE, 0.0) + } + + public static getNeutralDeadband(device: string): number | undefined { + return SimGeneric.get(SimType.CAN_MOTOR, device, CANMOTOR_NEUTRAL_DEADBAND, 0.0) + } + + public static setSupplyCurrent(device: string, current: number): boolean { + return SimGeneric.set(SimType.CAN_MOTOR, device, CANMOTOR_SUPPLY_CURRENT, current) + } + + public static setMotorCurrent(device: string, current: number): boolean { + return SimGeneric.set(SimType.CAN_MOTOR, device, CANMOTOR_MOTOR_CURRENT, current) + } + + public static setBusVoltage(device: string, voltage: number): boolean { + return SimGeneric.set(SimType.CAN_MOTOR, device, CANMOTOR_BUS_VOLTAGE, voltage) + } + + public static genSupplier(device: string): SimSupplier { + return { + getSupplierType: () => supplierTypeMap[SimType.CAN_MOTOR]!, + getSupplierValue: () => SimCANMotor.getPercentOutput(device) ?? 0, + } + } +} +export class SimCANEncoder { + private constructor() {} + + public static setVelocity(device: string, velocity: number): boolean { + return SimGeneric.set(SimType.CAN_ENCODER, device, CANENCODER_VELOCITY, velocity) + } + + public static setPosition(device: string, position: number): boolean { + return SimGeneric.set(SimType.CAN_ENCODER, device, CANENCODER_POSITION, position) + } + + public static genReceiver(device: string): SimReceiver { + return { + getReceiverType: () => receiverTypeMap[SimType.CAN_ENCODER]!, + setReceiverValue: ([count, rate]: NoraNumber2) => { + SimCANEncoder.setPosition(device, count) + SimCANEncoder.setVelocity(device, rate) + }, + } + } +} + +export class SimGyro { + private constructor() {} + + public static setAngleX(device: string, angle: number): boolean { + return SimGeneric.set(SimType.GYRO, device, ">angle_x", angle) + } + + public static setAngleY(device: string, angle: number): boolean { + return SimGeneric.set(SimType.GYRO, device, ">angle_y", angle) + } + + public static setAngleZ(device: string, angle: number): boolean { + return SimGeneric.set(SimType.GYRO, device, ">angle_z", angle) + } + + public static setRateX(device: string, rate: number): boolean { + return SimGeneric.set(SimType.GYRO, device, ">rate_x", rate) + } + + public static setRateY(device: string, rate: number): boolean { + return SimGeneric.set(SimType.GYRO, device, ">rate_y", rate) + } + + public static setRateZ(device: string, rate: number): boolean { + return SimGeneric.set(SimType.GYRO, device, ">rate_z", rate) + } +} + +export class SimAccel { + private constructor() {} + + public static setX(device: string, accel: number): boolean { + return SimGeneric.set(SimType.ACCELEROMETER, device, ">x", accel) + } + + public static setY(device: string, accel: number): boolean { + return SimGeneric.set(SimType.ACCELEROMETER, device, ">y", accel) + } + + public static setZ(device: string, accel: number): boolean { + return SimGeneric.set(SimType.ACCELEROMETER, device, ">z", accel) + } + + public static genReceiver(device: string): SimReceiver { + return { + getReceiverType: () => receiverTypeMap[SimType.ACCELEROMETER]!, + setReceiverValue: ([x, y, z]: NoraNumber3) => { + SimAccel.setX(device, x) + SimAccel.setY(device, y) + SimAccel.setZ(device, z) + }, + } + } +} + +export class SimCamera { + private constructor() {} + + public static setConnected(device: string, connected: boolean): boolean { + return SimGeneric.set(SimType.CAMERA, device, ">connected", connected) + } + + public static getConnected(device: string): boolean { + return SimGeneric.get(SimType.CAMERA, device, ">connected", false) + } + + public static setResolutionWidth(device: string, width: number): boolean { + return SimGeneric.set(SimType.CAMERA, device, ">width", width) + } + + public static setResolutionHeight(device: string, height: number): boolean { + return SimGeneric.set(SimType.CAMERA, device, ">height", height) + } + + public static setFPS(device: string, fps: number): boolean { + return SimGeneric.set(SimType.CAMERA, device, ">fps", fps) + } + + public static getRequestedWidth(device: string): number { + return SimGeneric.get(SimType.CAMERA, device, "brightness", brightness) + } + + public static setExposure(device: string, exposure: number): boolean { + return SimGeneric.set(SimType.CAMERA, device, ">exposure", exposure) + } + + public static setAutoExposure(device: string, autoExposure: boolean): boolean { + return SimGeneric.set(SimType.CAMERA, device, ">auto_exposure", autoExposure) + } + + public static getRequestedBrightness(device: string): number { + return SimGeneric.get(SimType.CAMERA, device, "value", value) + } + + public static getValue(device: string): boolean { + return SimGeneric.get(SimType.DIO, device, "<>value", false) + } + + public static genReceiver(device: string): SimReceiver { + return { + getReceiverType: () => receiverTypeMap[SimType.DIO]!, + setReceiverValue: (a: NoraNumber) => { + SimDIO.setValue(device, a > 0.5) + }, + } + } + + public static genSupplier(device: string): SimSupplier { + return { + getSupplierType: () => receiverTypeMap[SimType.DIO]!, + getSupplierValue: () => (SimDIO.getValue(device) ? 1 : 0), + } + } +} + +export class SimAI { + constructor() {} + + public static setValue(device: string, value: number): boolean { + return SimGeneric.set(SimType.AI, device, ">voltage", value) + } + + /** + * The number of averaging bits + */ + public static getAvgBits(device: string) { + return SimGeneric.get(SimType.AI, device, "voltage", voltage) + } + /** + * If the accumulator is initialized in the robot program + */ + public static getAccumInit(device: string) { + return SimGeneric.get(SimType.AI, device, "accum_value", accumValue) + } + /** + * The number of accumulated values + */ + public static setAccumCount(device: string, accumCount: number) { + return SimGeneric.set(SimType.AI, device, ">accum_count", accumCount) + } + /** + * The center value of the accumulator + */ + public static getAccumCenter(device: string) { + return SimGeneric.get(SimType.AI, device, "voltage", 0.0) + } +} + +type WSMessage = { + type: string // might be a SimType + device: string // device name + data: Map +} worker.getValue().addEventListener("message", (eventData: MessageEvent) => { let data: WSMessage | undefined @@ -20,11 +573,11 @@ worker.getValue().addEventListener("message", (eventData: MessageEvent) => { if (eventData.data.status) { switch (eventData.data.status) { case "open": - setConnected(true) + WPILibState.setConnected(true) break case "close": case "error": - setConnected(false) + WPILibState.setConnected(false) break default: return @@ -45,6 +598,8 @@ worker.getValue().addEventListener("message", (eventData: MessageEvent) => { if (!data?.type || !(Object.values(SimType) as string[]).includes(data.type)) return + WPILibState.setConnected(true) + updateSimMap(data.type as SimType, data.device, data.data) }) @@ -83,6 +638,8 @@ class WPILibBrain extends Brain { constructor(assembly: MirabufSceneObject) { super(assembly.mechanism, "wpilib") + console.log(`[WPILIBRAIN] Constructor called for assembly: ${assembly.assemblyName}`) + this._assembly = assembly this._simLayer = World.simulationSystem.getSimulationLayer(this._mechanism)! @@ -92,8 +649,11 @@ class WPILibBrain extends Brain { return } + console.log(`[WPILIBRAIN] SimulationLayer found, setting up devices...`) + this.addSimInput(new SimGyroInput("Test Gyro[1]", this._mechanism)) this.addSimInput(new SimAccelInput("ADXL362[4]", this._mechanism)) + this.addSimInput(new SimCameraInput("USB Camera 0", assembly, 640, 480, 30)) this.addSimInput(new SimDigitalInput("SYN DI[0]", () => random() > 0.5)) this.addSimOutput(new SimDigitalOutput("SYN DO[1]")) this.addSimInput(new SimAnalogInput("SYN AI[0]", () => random() * 12)) @@ -113,7 +673,9 @@ class WPILibBrain extends Brain { } public addSimInput(input: SimInput) { + console.log(`[WPILIBRAIN] Adding SimInput: ${input.constructor.name} for device "${input.device}"`) this._simInputs.push(input) + console.log(`[WPILIBRAIN] Total inputs: ${this._simInputs.length}`) } public addSimFlow(flow: SimFlow): boolean { @@ -148,6 +710,14 @@ class WPILibBrain extends Brain { } public update(deltaT: number): void { + // Add occasional logging to confirm update is being called + if (Math.random() < 0.005) { + // ~0.5% chance per frame + console.log( + `๐Ÿ”„ [WPILIBRAIN] update() called - ${this._simInputs.length} inputs, ${this._simOutputs.length} outputs` + ) + } + this._simOutputs.forEach(d => d.update(deltaT)) this._simInputs.forEach(i => i.update(deltaT)) this._simFlows.forEach(({ supplier, receiver }) => { @@ -156,13 +726,13 @@ class WPILibBrain extends Brain { } public enable(): void { - setSimBrain(this) + WPILibState.setSimBrain(this) // worker.getValue().postMessage({ command: "enable", reconnect: RECONNECT }) } public disable(): void { - if (getSimBrain() == this) { - setSimBrain(undefined) + if (WPILibState.getSimBrain() == this) { + WPILibState.setSimBrain(undefined) } // worker.getValue().postMessage({ command: "disable" }) } diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibState.ts b/fission/src/systems/simulation/wpilib_brain/WPILibState.ts index a8a9e960cd..398faa0e80 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibState.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibState.ts @@ -42,6 +42,21 @@ export function getIsConnected() { return isConnected } +worker.getValue().addEventListener("message", (eventData: MessageEvent) => { + if (!eventData?.data?.status) return + switch (eventData.data.status) { + case "open": + setConnected(true) + break + case "close": + case "error": + setConnected(false) + break + default: + break + } +}) + export const supplierTypeMap: { [k in SimType]: NoraTypes | undefined } = { [SimType.PWM]: NoraTypes.NUMBER, [SimType.SIM_DEVICE]: undefined, diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibWSWorker.ts b/fission/src/systems/simulation/wpilib_brain/WPILibWSWorker.ts index 49dca0465b..2f5162fa30 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibWSWorker.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibWSWorker.ts @@ -39,7 +39,9 @@ async function tryConnect(port?: number): Promise { socket.addEventListener("message", onMessage) }) - .then(() => console.debug("Mutex released")) + .then(() => { + /* console.debug("Mutex released") */ + }) } async function tryDisconnect(): Promise { @@ -51,12 +53,9 @@ async function tryDisconnect(): Promise { }) } -// Posts incoming messages function onMessage(event: MessageEvent) { self.postMessage(event.data) } - -// Sends outgoing messages self.addEventListener("message", e => { switch (e.data.command) { case "enable": { @@ -89,6 +88,17 @@ self.addEventListener("message", e => { } break } + case "camera_frame": { + if (socketOpen()) { + const frameMessage = { + type: "CAMERA_FRAME", + device: e.data.data.device, + data: e.data.data, + } + socket!.send(JSON.stringify(frameMessage)) + } + break + } default: { console.warn(`Unrecognized command '${e.data.command}'`) break diff --git a/fission/src/ui/panels/WsViewPanel.tsx b/fission/src/ui/panels/WsViewPanel.tsx index 12a3e945cf..073cf81c9d 100644 --- a/fission/src/ui/panels/WsViewPanel.tsx +++ b/fission/src/ui/panels/WsViewPanel.tsx @@ -51,6 +51,7 @@ function generateTableBody() { // SimType.CANEncoder, // SimType.Gyro, // SimType.Accel, + // SimType.Camera, // SimType.DIO, // SimType.AI, // SimType.AO, @@ -138,7 +139,7 @@ const WSViewPanel: React.FC> = ({ panel }) => { useEffect(() => { configureScreen(panel!, { title: "WS View Panel" }, {}) - }, []) + }, [configureScreen, panel]) return ( @@ -167,7 +168,7 @@ const WSViewPanel: React.FC> = ({ panel }) => {