From a2d446bf4949481f4ce78fe5c0ca0490d7aeb5d4 Mon Sep 17 00:00:00 2001 From: DevMiner Date: Sun, 2 Jun 2024 17:43:50 +0200 Subject: [PATCH] feat: emulated trackers --- .changeset/pink-baboons-lick.md | 5 + apps/emulated-tracker-demo/package.json | 31 +++ apps/emulated-tracker-demo/src/index.ts | 99 ++++++++ apps/emulated-tracker-demo/tsconfig.json | 9 + packages/tracker-emulation/.npmignore | 3 + packages/tracker-emulation/package.json | 39 +++ .../tracker-emulation/src/EmulatedSensor.ts | 48 ++++ .../tracker-emulation/src/EmulatedTracker.ts | 239 ++++++++++++++++++ packages/tracker-emulation/src/index.ts | 2 + packages/tracker-emulation/tsconfig.json | 8 + pnpm-lock.yaml | 66 +++++ 11 files changed, 549 insertions(+) create mode 100644 .changeset/pink-baboons-lick.md create mode 100644 apps/emulated-tracker-demo/package.json create mode 100644 apps/emulated-tracker-demo/src/index.ts create mode 100644 apps/emulated-tracker-demo/tsconfig.json create mode 100644 packages/tracker-emulation/.npmignore create mode 100644 packages/tracker-emulation/package.json create mode 100644 packages/tracker-emulation/src/EmulatedSensor.ts create mode 100644 packages/tracker-emulation/src/EmulatedTracker.ts create mode 100644 packages/tracker-emulation/src/index.ts create mode 100644 packages/tracker-emulation/tsconfig.json diff --git a/.changeset/pink-baboons-lick.md b/.changeset/pink-baboons-lick.md new file mode 100644 index 0000000..78c3868 --- /dev/null +++ b/.changeset/pink-baboons-lick.md @@ -0,0 +1,5 @@ +--- +'@slimevr/tracker-emulation': patch +--- + +implement basic emulated trackers diff --git a/apps/emulated-tracker-demo/package.json b/apps/emulated-tracker-demo/package.json new file mode 100644 index 0000000..14dcc28 --- /dev/null +++ b/apps/emulated-tracker-demo/package.json @@ -0,0 +1,31 @@ +{ + "name": "@slimevr/emulated-tracker-demo", + "version": "0.1.6", + "main": "dist/index.js", + "license": "(MIT OR Apache-2.0)", + "private": true, + "author": { + "name": "DevMiner", + "email": "devminer@devminer.xyz" + }, + "dependencies": { + "@slimevr/common": "workspace:*", + "@slimevr/firmware-protocol": "workspace:*", + "@slimevr/tracker-emulation": "workspace:*", + "quaternion": "^1.5.1" + }, + "devDependencies": { + "@slimevr/tsconfig": "workspace:*", + "@types/node": "^18.17.2", + "concurrently": "^8.2.0", + "nodemon": "^3.0.1", + "typescript": "^5.1.6" + }, + "scripts": { + "dev": "concurrently \"tsc -w\" \"nodemon .\"", + "build": "tsc" + }, + "engines": { + "node": ">=16.0.0" + } +} diff --git a/apps/emulated-tracker-demo/src/index.ts b/apps/emulated-tracker-demo/src/index.ts new file mode 100644 index 0000000..802b3c5 --- /dev/null +++ b/apps/emulated-tracker-demo/src/index.ts @@ -0,0 +1,99 @@ +import { MACAddress, Quaternion } from '@slimevr/common'; +import { + BoardType, + FirmwareFeatureFlags, + MCUType, + RotationDataType, + SensorStatus, + SensorType +} from '@slimevr/firmware-protocol'; +import { EmulatedSensor, EmulatedTracker } from '@slimevr/tracker-emulation'; +import BetterQuaternion from 'quaternion'; + +const tracker = new EmulatedTracker( + MACAddress.random(), + '0.0.1', + new FirmwareFeatureFlags(new Map()), + BoardType.CUSTOM, + MCUType.ESP8266 +); +const sensors: EmulatedSensor[] = []; + +const main = async () => { + tracker.on('ready', () => { + console.log('searching for server...'); + }); + + tracker.on('connected-to-server', async (ip, port) => { + console.log('connected to server', ip, port); + }); + + tracker.on('unknown-incoming-packet', (packet) => { + console.log('unknown packet type', packet.type); + }); + + tracker.on('error', (err) => console.error(err)); + + await tracker.init(); + + for (let i = 0; i < 3; i++) { + sensors.push(await tracker.addSensor(SensorType.UNKNOWN, SensorStatus.OK)); + } + + //#region sin wave battery level + { + let i = 0; + setInterval(() => { + tracker.changeBatteryLevel(Math.sin(i) * 0.5 + 3.7, Math.sin(i) * 100); + i += 0.1; + }, 1000).unref(); + } + //#endregion + + //#region sin wave temperature + { + let i = 0; + setInterval(() => { + sensors.forEach((sensor) => sensor.sendTemperature(Math.sin(i) * 10 + 25)); + i += 0.1; + }, 1000).unref(); + } + //#endregion + + //#region sin wave magnetometer accuracy + { + let i = 0; + setInterval(() => { + sensors.forEach((sensor) => sensor.sendMagnetometerAccuracy(Math.sin(i) * 3 + 3)); + i += 0.1; + }, 1000).unref(); + } + //#endregion + + //#region sin wave signal strength + { + let i = 0; + setInterval(() => { + sensors.forEach((sensor) => sensor.sendSignalStrength(Math.sin(i) * 127)); + i += 0.1; + }, 1000).unref(); + } + //#endregion + + //#region + { + let i = 0; + setInterval(() => { + sensors.forEach((sensor, idx) => { + const v = i / 5; + const better = BetterQuaternion.fromEuler(idx == 0 ? v : 0, idx == 1 ? v : 0, idx == 2 ? v : 0); + + sensor.sendRotation(RotationDataType.NORMAL, new Quaternion(better.x, better.y, better.z, better.w), 0); + }); + + i += 0.1; + }, 100).unref(); + } +}; + +main(); diff --git a/apps/emulated-tracker-demo/tsconfig.json b/apps/emulated-tracker-demo/tsconfig.json new file mode 100644 index 0000000..68f8eea --- /dev/null +++ b/apps/emulated-tracker-demo/tsconfig.json @@ -0,0 +1,9 @@ +{ + "extends": "@slimevr/tsconfig/node.json", + "include": ["src/**/*.ts"], + "exclude": ["dist", "node_modules"], + "compilerOptions": { + "target": "ES2022", + "outDir": "./dist" + } +} diff --git a/packages/tracker-emulation/.npmignore b/packages/tracker-emulation/.npmignore new file mode 100644 index 0000000..b7b0bac --- /dev/null +++ b/packages/tracker-emulation/.npmignore @@ -0,0 +1,3 @@ +.turbo +CHANGELOG.md +tsconfig.json diff --git a/packages/tracker-emulation/package.json b/packages/tracker-emulation/package.json new file mode 100644 index 0000000..1377933 --- /dev/null +++ b/packages/tracker-emulation/package.json @@ -0,0 +1,39 @@ +{ + "name": "@slimevr/tracker-emulation", + "version": "0.0.0", + "main": "./dist/index.js", + "module": "./dist/index.mjs", + "types": "./dist/index.d.ts", + "license": "(MIT OR Apache-2.0)", + "author": { + "name": "DevMiner", + "email": "devminer@devminer.xyz" + }, + "repository": { + "type": "git", + "url": "https://github.com/SlimeVR/slimevr-node.git/tree/master/packages/tracker-emulation" + }, + "bugs": { + "url": "https://github.com/SlimeVR/slimevr-node/issues" + }, + "publishConfig": { + "access": "public" + }, + "scripts": { + "build": "tsc", + "prepack": "pnpm build", + "watch": "pnpm build -- --watch", + "clean": "rimraf dist" + }, + "dependencies": { + "@slimevr/common": "workspace:*", + "@slimevr/firmware-protocol": "workspace:*", + "strict-event-emitter-types": "^2.0.0" + }, + "devDependencies": { + "@slimevr/tsconfig": "workspace:*", + "@types/node": "^18.17.2", + "rimraf": "^5.0.1", + "typescript": "^5.4.5" + } +} diff --git a/packages/tracker-emulation/src/EmulatedSensor.ts b/packages/tracker-emulation/src/EmulatedSensor.ts new file mode 100644 index 0000000..1f65ab8 --- /dev/null +++ b/packages/tracker-emulation/src/EmulatedSensor.ts @@ -0,0 +1,48 @@ +import { Quaternion, Vector } from '@slimevr/common'; +import { RotationDataType, SensorStatus, SensorType, ServerBoundSensorInfoPacket } from '@slimevr/firmware-protocol'; +import { EmulatedTracker } from './EmulatedTracker'; + +export class EmulatedSensor { + constructor( + private readonly tracker: EmulatedTracker, + readonly id: number, + readonly type: SensorType, + private _status: SensorStatus + ) {} + + get status() { + return this._status; + } + + async sendSensorInfo() { + await this.tracker.sendPacketToServer(new ServerBoundSensorInfoPacket(this.id, this._status, this.type)); + } + + async sendRotation(dataType: RotationDataType, rotation: Quaternion, accuracyInfo: number) { + await this.tracker.sendRotationData(this.id, dataType, rotation, accuracyInfo); + } + + async sendAcceleration(acceleration: Vector) { + await this.tracker.sendAcceleration(this.id, acceleration); + } + + async sendTemperature(temperature: number) { + await this.tracker.sendTemperature(this.id, temperature); + } + + async sendMagnetometerAccuracy(accuracy: number) { + await this.tracker.sendMagnetometerAccuracy(this.id, accuracy); + } + + /** + * @param signalStrength The signal strength to send to the server, must be between -127 and 127 + */ + async sendSignalStrength(signalStrength: number) { + await this.tracker.sendSignalStrength(this.id, signalStrength); + } + + async changeStatus(status: SensorStatus) { + this._status = status; + await this.sendSensorInfo(); + } +} diff --git a/packages/tracker-emulation/src/EmulatedTracker.ts b/packages/tracker-emulation/src/EmulatedTracker.ts new file mode 100644 index 0000000..ba9e9bc --- /dev/null +++ b/packages/tracker-emulation/src/EmulatedTracker.ts @@ -0,0 +1,239 @@ +import { MACAddress, Quaternion, Vector } from '@slimevr/common'; +import { + BoardType, + DeviceBoundFeatureFlagsPacket, + DeviceBoundHandshakePacket, + DeviceBoundHeartbeatPacket, + DeviceBoundPingPacket, + FirmwareFeatureFlags, + MCUType, + Packet, + parse, + RotationDataType, + SensorStatus, + SensorType, + ServerBoundAccelPacket, + ServerBoundBatteryLevelPacket, + ServerBoundFeatureFlagsPacket, + ServerBoundHandshakePacket, + ServerBoundHeartbeatPacket, + ServerBoundMagnetometerAccuracyPacket, + ServerBoundPongPacket, + ServerBoundRotationDataPacket, + ServerBoundSignalStrengthPacket, + ServerBoundTemperaturePacket, + ServerFeatureFlags +} from '@slimevr/firmware-protocol'; +import { createSocket, RemoteInfo, Socket } from 'dgram'; +import EventEmitter from 'events'; +import { AddressInfo } from 'net'; +import { type StrictEventEmitter } from 'strict-event-emitter-types'; +import { EmulatedSensor } from './EmulatedSensor'; + +type State = + | { status: 'initializing' } + | { status: 'searching-for-server'; discoveryInterval: NodeJS.Timeout } + | { status: 'connected-to-server'; packetNumber: bigint; serverIP: string; serverPort: number }; + +interface EmulatedTrackerEvents { + error: (error: Error) => void; + ready: (address: AddressInfo) => void; + 'connected-to-server': (serverIP: string, serverPort: number) => void; + 'server-feature-flags': (flags: ServerFeatureFlags) => void; + 'unknown-incoming-packet': (packet: Packet) => void; + 'outgoing-packet': (packet: Packet) => void; +} + +const SUPPORTED_FIRMWARE_PROTOCOL_VERSION = 13; + +export class EmulatedTracker extends (EventEmitter as { + new (): StrictEventEmitter; +}) { + // TODO: Implement timing out the server connection if no packets are received for a while + private lastPacket = Date.now(); + private lastPing = { + id: 0, + startTimestamp: 0, + duration: 0 + }; + + private batteryVoltage = 0; + private batteryPercentage = 0; + + private readonly socket: Socket; + private state: State; + + private sensors: EmulatedSensor[] = []; + + constructor( + private readonly mac: MACAddress, + private readonly firmware: string, + private readonly featureFlags: FirmwareFeatureFlags, + private readonly boardType: BoardType = BoardType.UNKNOWN, + private readonly mcuType: MCUType = MCUType.UNKNOWN + ) { + super(); + + this.socket = createSocket('udp4'); + this.socket.on('message', (msg, addr) => this.handle(msg, addr)); + this.socket.on('error', (err) => this.emit('error', err)); + + this.state = { + status: 'initializing' + }; + + this.on('connected-to-server', async () => { + await this.sendPacketToServer(new ServerBoundFeatureFlagsPacket(this.featureFlags)); + await this.sendBatteryLevel(); + await Promise.all(this.sensors.map((sensor) => sensor.sendSensorInfo())); + }); + } + + unref() { + this.socket.unref(); + } + + async addSensor(sensorType: SensorType, sensorStatus: SensorStatus) { + const sensorId = this.sensors.length; + + const sensor = new EmulatedSensor(this, sensorId, sensorType, sensorStatus); + this.sensors.push(sensor); + + await sensor.sendSensorInfo(); + + return sensor; + } + + private async sendDiscovery() { + await this.sendPacket( + new ServerBoundHandshakePacket( + this.boardType, + SensorType.UNKNOWN, + this.mcuType, + SUPPORTED_FIRMWARE_PROTOCOL_VERSION, + this.firmware, + this.mac + ), + 6969, + '255.255.255.255' + ); + } + + async init() { + await new Promise((resolve) => this.socket.bind(0, () => resolve())); + this.socket.setBroadcast(true); + + this.emit('ready', this.socket.address()); + + this.state = { + status: 'searching-for-server', + discoveryInterval: setInterval(() => this.sendDiscovery(), 1000) + }; + } + + private log(msg: string) { + console.log(`[Tracker:${this.mac}] ${msg}`); + } + + async changeBatteryLevel(batteryVoltage: number, batteryPercentage: number) { + this.batteryVoltage = batteryVoltage; + this.batteryPercentage = batteryPercentage; + + await this.sendBatteryLevel(); + } + + async sendBatteryLevel() { + await this.sendPacketToServer(new ServerBoundBatteryLevelPacket(this.batteryVoltage, this.batteryPercentage)); + } + + async sendRotationData(sensorId: number, dataType: RotationDataType, rotation: Quaternion, accuracyInfo: number) { + await this.sendPacketToServer(new ServerBoundRotationDataPacket(sensorId, dataType, rotation, accuracyInfo)); + } + + async sendAcceleration(sensorId: number, acceleration: Vector) { + await this.sendPacketToServer(new ServerBoundAccelPacket(sensorId, acceleration)); + } + + async sendTemperature(sensorId: number, temperature: number) { + await this.sendPacketToServer(new ServerBoundTemperaturePacket(sensorId, temperature)); + } + + async sendMagnetometerAccuracy(sensorId: number, accuracy: number) { + await this.sendPacketToServer(new ServerBoundMagnetometerAccuracyPacket(sensorId, accuracy)); + } + + async sendSignalStrength(sensorId: number, signalStrength: number) { + await this.sendPacketToServer(new ServerBoundSignalStrengthPacket(sensorId, signalStrength)); + } + + async sendPacketToServer(packet: Packet) { + if (this.state.status !== 'connected-to-server') return; + + const port = this.state.serverPort; + const ip = this.state.serverIP; + + await this.sendPacket(packet, port, ip); + } + + private async sendPacket(packet: Packet, port: number, ip: string) { + this.emit('outgoing-packet', packet); + + const encoded = packet.encode(this.state.status === 'connected-to-server' ? this.state.packetNumber++ : 0n); + + await new Promise((res, rej) => + this.socket.send(encoded, 0, encoded.length, port, ip, (err) => (err ? rej(err) : res())) + ); + + this.log(`Sent packet to ${ip}:${port} (${encoded.length} bytes): ${encoded.toString('hex')}`); + } + + private handle(msg: Buffer, addr: RemoteInfo) { + if (this.state.status === 'searching-for-server') { + if (msg.readUint8(0) !== DeviceBoundHandshakePacket.type) return; + + clearInterval(this.state.discoveryInterval); + + this.state = { + status: 'connected-to-server', + packetNumber: 0n, + serverIP: addr.address, + serverPort: addr.port + }; + + this.emit('connected-to-server', addr.address, addr.port); + + return; + } + + const [_num, packet] = parse(msg, true); + if (packet === null) { + this.log(`Received unknown packet (${msg.length} bytes): ${msg.toString('hex')}`); + + return; + } + + this.lastPacket = Date.now(); + + switch (packet.type) { + case DeviceBoundPingPacket.type: { + const pingPacket = packet as DeviceBoundPingPacket; + this.sendPacketToServer(new ServerBoundPongPacket(pingPacket.id)); + break; + } + + case DeviceBoundHeartbeatPacket.type: { + this.sendPacketToServer(new ServerBoundHeartbeatPacket()); + break; + } + + case DeviceBoundFeatureFlagsPacket.type: { + const p = packet as DeviceBoundFeatureFlagsPacket; + this.emit('server-feature-flags', p.flags); + break; + } + + default: + this.emit('unknown-incoming-packet', packet); + } + } +} diff --git a/packages/tracker-emulation/src/index.ts b/packages/tracker-emulation/src/index.ts new file mode 100644 index 0000000..acd72bd --- /dev/null +++ b/packages/tracker-emulation/src/index.ts @@ -0,0 +1,2 @@ +export * from './EmulatedSensor'; +export * from './EmulatedTracker'; diff --git a/packages/tracker-emulation/tsconfig.json b/packages/tracker-emulation/tsconfig.json new file mode 100644 index 0000000..f9fe550 --- /dev/null +++ b/packages/tracker-emulation/tsconfig.json @@ -0,0 +1,8 @@ +{ + "extends": "@slimevr/tsconfig/node.json", + "include": ["src/**/*.ts"], + "exclude": ["dist", "build", "node_modules"], + "compilerOptions": { + "outDir": "dist" + } +} diff --git a/pnpm-lock.yaml b/pnpm-lock.yaml index 973a956..e8e52fe 100644 --- a/pnpm-lock.yaml +++ b/pnpm-lock.yaml @@ -21,6 +21,37 @@ importers: specifier: 1.13.3 version: 1.13.3 + apps/emulated-tracker-demo: + dependencies: + '@slimevr/common': + specifier: workspace:* + version: link:../../packages/common + '@slimevr/firmware-protocol': + specifier: workspace:* + version: link:../../packages/firmware-protocol + '@slimevr/tracker-emulation': + specifier: workspace:* + version: link:../../packages/tracker-emulation + quaternion: + specifier: ^1.5.1 + version: 1.5.1 + devDependencies: + '@slimevr/tsconfig': + specifier: workspace:* + version: link:../../packages/tsconfig + '@types/node': + specifier: ^18.17.2 + version: 18.17.2 + concurrently: + specifier: ^8.2.0 + version: 8.2.0 + nodemon: + specifier: ^3.0.1 + version: 3.0.1 + typescript: + specifier: ^5.1.6 + version: 5.1.6 + apps/firmware-protocol-debugger: dependencies: '@slimevr/common': @@ -267,6 +298,31 @@ importers: specifier: ^7.2.0 version: 7.2.0 + packages/tracker-emulation: + dependencies: + '@slimevr/common': + specifier: workspace:* + version: link:../common + '@slimevr/firmware-protocol': + specifier: workspace:* + version: link:../firmware-protocol + strict-event-emitter-types: + specifier: ^2.0.0 + version: 2.0.0 + devDependencies: + '@slimevr/tsconfig': + specifier: workspace:* + version: link:../tsconfig + '@types/node': + specifier: ^18.17.2 + version: 18.17.2 + rimraf: + specifier: ^5.0.1 + version: 5.0.1 + typescript: + specifier: ^5.4.5 + version: 5.4.5 + packages/tsconfig: {} packages: @@ -4453,6 +4509,10 @@ packages: resolution: {integrity: sha512-rRV+zQD8tVFys26lAGR9WUuS4iUAngJScM+ZRSKtvl5tKeZ2t5bvdNFdNHBW9FWR4guGHlgmsZ1G7BSm2wTbuA==} engines: {node: '>=6'} + /quaternion@1.5.1: + resolution: {integrity: sha512-VwrFuGDZ+MRxytT2Lo8zKwnwUF++sk2xGtAGbD4b/rjqttg81LuCkwgMGPui40WHzdG2Gh1eg3wHD+nmAmbN1Q==} + dev: false + /queue-microtask@1.2.3: resolution: {integrity: sha512-NuaNSa6flKT5JaSYQzJok04JzTL1CA6aGhv5rfLW3PgqA+M2ChpZQnAC8h8i4ZFkBS8X5RqkDBHA7r4hej3K9A==} @@ -5482,6 +5542,12 @@ packages: engines: {node: '>=14.17'} hasBin: true + /typescript@5.4.5: + resolution: {integrity: sha512-vcI4UpRgg81oIRUFwR0WSIHKt11nJ7SAVlYNIu+QpqeyXP+gpQJy/Z4+F0aGxSE4MqwjyXvW/TzgkLAx2AGHwQ==} + engines: {node: '>=14.17'} + hasBin: true + dev: true + /unbox-primitive@1.0.2: resolution: {integrity: sha512-61pPlCD9h51VoreyJ0BReideM3MDKMKnh6+V9L08331ipq6Q8OFXZYiqP6n/tbHx4s5I9uRhcye6BrbkizkBDw==} dependencies: