diff --git a/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt b/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt index 8e92e0615..3e5129590 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/datafeed.kt @@ -167,6 +167,7 @@ fun createDatafeedFrame( class DataFeedInitBehaviour(val server: VRServer, val skeleton: Skeleton) : SolarXRBridgeBehaviour { override fun reduce(state: SolarXRBridgeState, action: SolarXRBridgeActions) = when (action) { is SolarXRBridgeActions.SetConfig -> state.copy(dataFeedConfigs = action.configs) + else -> state } override fun observe(receiver: SolarXRBridge) { diff --git a/server/core/src/main/java/dev/slimevr/solarxr/driver/handshake.kt b/server/core/src/main/java/dev/slimevr/solarxr/driver/handshake.kt new file mode 100644 index 000000000..c889dd0bf --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/solarxr/driver/handshake.kt @@ -0,0 +1,45 @@ +package dev.slimevr.solarxr.driver + +import dev.slimevr.VRServer +import dev.slimevr.solarxr.SolarXRBridge +import dev.slimevr.solarxr.SolarXRBridgeActions +import dev.slimevr.solarxr.SolarXRBridgeBehaviour +import dev.slimevr.solarxr.SolarXRBridgeState +import solarxr_protocol.driver_protocol.DriverHandshakeStatus +import solarxr_protocol.driver_protocol.InboundHandshakeRequest +import solarxr_protocol.driver_protocol.InboundHandshakeResponse + +class DriverHandshakeBehaviour( + private val server: VRServer, +) : SolarXRBridgeBehaviour { + override fun reduce(state: SolarXRBridgeState, action: SolarXRBridgeActions) = when (action) { + is SolarXRBridgeActions.SetDriverName -> state.copy(driverName = action.name) + else -> state + } + + override fun observe(receiver: SolarXRBridge) { + receiver.onDriverMessage { req, replyTo -> + val name = req.driverName + + val duplicate = name != null && + server.context.state.value.solarxr.values.any { + it.id != receiver.id && it.context.state.value.driverName == name + } + + if (duplicate) { + receiver.sendDriverMessage( + InboundHandshakeResponse(status = DriverHandshakeStatus.REJECTED_DUPLICATE), + replyTo = replyTo, + ) + receiver.disconnect() + return@onDriverMessage + } + + if (name != null) receiver.context.dispatch(SolarXRBridgeActions.SetDriverName(name)) + receiver.sendDriverMessage( + InboundHandshakeResponse(status = DriverHandshakeStatus.ACCEPTED), + replyTo = replyTo, + ) + }.launchIn(receiver.context.scope) + } +} diff --git a/server/core/src/main/java/dev/slimevr/solarxr/driver/incoming.kt b/server/core/src/main/java/dev/slimevr/solarxr/driver/incoming.kt new file mode 100644 index 000000000..87d7241b9 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/solarxr/driver/incoming.kt @@ -0,0 +1,117 @@ +package dev.slimevr.solarxr.driver + +import dev.slimevr.VRServer +import dev.slimevr.VRServerActions +import dev.slimevr.device.Device +import dev.slimevr.device.DeviceActions +import dev.slimevr.device.DeviceOrigin +import dev.slimevr.solarxr.SolarXRBridge +import dev.slimevr.solarxr.SolarXRBridgeBehaviour +import dev.slimevr.tracker.Tracker +import dev.slimevr.tracker.TrackerActions +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import solarxr_protocol.datatypes.TrackerStatus +import solarxr_protocol.driver_protocol.AddTrackerStatus +import solarxr_protocol.driver_protocol.InboundAddTrackerRequest +import solarxr_protocol.driver_protocol.InboundAddTrackerResponse +import solarxr_protocol.driver_protocol.InboundBatteryNotification +import solarxr_protocol.driver_protocol.InboundTrackerPositionNotification +import solarxr_protocol.driver_protocol.InboundTrackerStatusNotification + +class DriverIncomingTrackersBehaviour( + private val server: VRServer, +) : SolarXRBridgeBehaviour { + override fun observe(receiver: SolarXRBridge) { + receiver.onDriverMessage { req, replyTo -> + val hardwareId = req.hardwareId + + if (hardwareId == null || receiver.context.state.value.driverName == null) { + receiver.sendDriverMessage(InboundAddTrackerResponse(status = AddTrackerStatus.ERROR), replyTo = replyTo) + return@onDriverMessage + } + + val existing = server.context.state.value.trackers.values + .find { it.context.state.value.hardwareId == hardwareId } + if (existing != null) { + receiver.sendDriverMessage( + InboundAddTrackerResponse( + status = AddTrackerStatus.ALREADY_EXISTS, + trackerId = existing.context.state.value.id.toUShort(), + ), + replyTo = replyTo, + ) + return@onDriverMessage + } + + val scope = server.context.scope + val deviceId = server.nextHandle() + val device = Device.create( + scope = scope, + appContext = receiver.appContext, + id = deviceId, + name = req.displayName ?: "Tracker", + manufacturer = req.manufacturer ?: "SlimeVR", + address = hardwareId, + macAddress = hardwareId, + origin = DeviceOrigin.DRIVER, + protocolVersion = 0, + ) + server.context.dispatch(VRServerActions.NewDevice(deviceId, device)) + + val trackerId = server.nextHandle() + val tracker = Tracker.create( + scope = scope, + id = trackerId, + name = req.displayName ?: "Tracker #$trackerId", + bodyPart = req.bodyPart, + deviceId = deviceId, + hardwareId = hardwareId, + origin = DeviceOrigin.DRIVER, + appContext = receiver.appContext, + ) + server.context.dispatch(VRServerActions.NewTracker(trackerId, tracker)) + tracker.context.dispatch(TrackerActions.SetStatus(TrackerStatus.OK)) + + receiver.sendDriverMessage( + InboundAddTrackerResponse(status = AddTrackerStatus.CREATED, trackerId = trackerId.toUShort()), + replyTo = replyTo, + ) + }.launchIn(receiver.context.scope) + + receiver.driverDispatcher.on { event -> + if (receiver.context.state.value.driverName == null) return@on + val trackerId = event.trackerId ?: return@on + val status = event.status ?: return@on + server.getTracker(trackerId.toInt())?.context?.dispatch(TrackerActions.SetStatus(status)) + }.launchIn(receiver.context.scope) + + receiver.driverDispatcher.on { event -> + if (receiver.context.state.value.driverName == null) return@on + val trackerId = event.trackerId ?: return@on + val batteryLevel = event.batteryLevel ?: return@on + val charging = event.charging ?: false + val tracker = server.getTracker(trackerId.toInt()) ?: return@on + val device = server.getDevice(tracker.context.state.value.deviceId) ?: return@on + device.context.dispatch( + DeviceActions.Update { + copy(batteryLevel = batteryLevel / 100f, batteryVoltage = if (charging) 4.3f else 3.7f) + }, + ) + }.launchIn(receiver.context.scope) + + receiver.driverDispatcher.on { event -> + if (receiver.context.state.value.driverName == null) return@on + val trackerId = event.trackerId ?: return@on + val rotation = event.rotation ?: return@on + + server.getTracker(trackerId.toInt())?.context?.dispatch( + TrackerActions.SetRotation( + rotation = Quaternion(rotation.w, rotation.x, rotation.y, rotation.z), + position = event.position?.let { Vector3(it.x, it.y, it.z) }, + // TODO: send velocity? + ), + ) + }.launchIn(receiver.context.scope) + } +} diff --git a/server/core/src/main/java/dev/slimevr/solarxr/driver/outgoing.kt b/server/core/src/main/java/dev/slimevr/solarxr/driver/outgoing.kt new file mode 100644 index 000000000..8b45bed5f --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/solarxr/driver/outgoing.kt @@ -0,0 +1,138 @@ +package dev.slimevr.solarxr.driver + +import dev.slimevr.AppContextProvider +import dev.slimevr.device.DeviceOrigin +import dev.slimevr.logging.AppLogger +import dev.slimevr.skeleton.BodyPartMap +import dev.slimevr.skeleton.bodyPartMap +import dev.slimevr.solarxr.SolarXRBridge +import dev.slimevr.solarxr.SolarXRBridgeBehaviour +import dev.slimevr.tracker.TrackerState +import kotlinx.coroutines.TimeoutCancellationException +import kotlinx.coroutines.flow.combine +import kotlinx.coroutines.flow.distinctUntilChanged +import kotlinx.coroutines.flow.launchIn +import kotlinx.coroutines.flow.map +import kotlinx.coroutines.flow.onEach +import kotlinx.coroutines.launch +import solarxr_protocol.datatypes.BodyPart +import solarxr_protocol.datatypes.TrackerStatus +import solarxr_protocol.datatypes.math.Quat +import solarxr_protocol.datatypes.math.Vec3f +import solarxr_protocol.driver_protocol.AddTrackerStatus +import solarxr_protocol.driver_protocol.OutboundAddTrackerRequest +import solarxr_protocol.driver_protocol.OutboundAddTrackerResponse +import solarxr_protocol.driver_protocol.OutboundTrackerPositionNotification +import solarxr_protocol.driver_protocol.OutboundTrackerStatusNotification +import solarxr_protocol.rpc.RoutingOutput +import java.util.concurrent.ConcurrentHashMap + +class DriverOutgoingTrackersBehaviour( + private val appContext: AppContextProvider, +) : SolarXRBridgeBehaviour { + // Fallback chain per bone, used to attribute battery and status to the nearest + // physical tracker. Bones without an entry just report no battery. + val bodyPartToNearest: BodyPartMap> = BodyPartMap( + mapOf( + BodyPart.UPPER_CHEST to setOf(BodyPart.UPPER_CHEST, BodyPart.CHEST), + BodyPart.HIP to setOf(BodyPart.HIP, BodyPart.WAIST, BodyPart.CHEST, BodyPart.UPPER_CHEST), + BodyPart.LEFT_UPPER_LEG to setOf(BodyPart.LEFT_UPPER_LEG), + BodyPart.RIGHT_UPPER_LEG to setOf(BodyPart.RIGHT_UPPER_LEG), + BodyPart.LEFT_FOOT to setOf(BodyPart.LEFT_FOOT, BodyPart.LEFT_LOWER_LEG), + BodyPart.RIGHT_FOOT to setOf(BodyPart.RIGHT_FOOT, BodyPart.RIGHT_LOWER_LEG), + BodyPart.LEFT_UPPER_ARM to setOf(BodyPart.LEFT_UPPER_ARM, BodyPart.LEFT_LOWER_ARM), + BodyPart.RIGHT_UPPER_ARM to setOf(BodyPart.RIGHT_UPPER_ARM, BodyPart.RIGHT_LOWER_ARM), + BodyPart.LEFT_HAND to setOf(BodyPart.LEFT_HAND, BodyPart.LEFT_LOWER_ARM), + BodyPart.RIGHT_HAND to setOf(BodyPart.RIGHT_HAND, BodyPart.RIGHT_LOWER_ARM), + ), + ) + + override fun observe(receiver: SolarXRBridge) { + val requestedTrackers = ConcurrentHashMap.newKeySet() + val confirmedTrackers = ConcurrentHashMap.newKeySet() + + // Status and battery are rebuilt every frame but only change every few seconds, so the driver + // was being sent the same values at skeleton rate. Keep the last one per body part and only + // send on a real change, the same way subscribedTrackers already gates TrackerAdded. + val lastStatus = mutableMapOf() + + combine( + appContext.skeleton.computed, + appContext.boneRouting.context.state + .map { state -> state.routes.filterValues { RoutingOutput.DRIVER in it }.keys } + .distinctUntilChanged(), + ::Pair, + ) + .distinctUntilChanged() + .onEach { (computedSkeleton, enabledBodyParts) -> + if (receiver.context.state.value.driverName == null) return@onEach + + val serverState = appContext.server.context.state.value + + // Map the nearest trackers to their body parts + val trackerStateByBodyPart = bodyPartMap() + for (tracker in serverState.trackers.values) { + val trackerState = tracker.context.state.value + if (trackerState.origin == DeviceOrigin.DRIVER) continue + val bodyPart = trackerState.bodyPart ?: continue + trackerStateByBodyPart.putIfAbsent(bodyPart, trackerState) + } + + computedSkeleton.forEach { (part, state) -> + if (enabledBodyParts.contains(part)) { + val closestTracker = bodyPartToNearest[part].orEmpty() + .firstNotNullOfOrNull { fallbackPart -> trackerStateByBodyPart[fallbackPart] } + val closestDevice = serverState.devices[closestTracker?.deviceId]?.context?.state?.value + + if (requestedTrackers.add(part.value)) { + receiver.context.scope.launch { + try { + val response = receiver.requestDriverMessage( + OutboundAddTrackerRequest(trackerId = part.value.toUShort(), bodyPart = part), + ) + if (response.status == AddTrackerStatus.ERROR) { + AppLogger.solarxr.warn("Driver rejected adding tracker for body part $part") + requestedTrackers.remove(part.value) // Should we retry on the next frame? + } else { + confirmedTrackers.add(part.value) + } + } catch (e: TimeoutCancellationException) { + AppLogger.solarxr.warn("Timeout waiting for driver to add tracker for body part $part") + requestedTrackers.remove(part.value) + } + } + } + + if (part.value !in confirmedTrackers) return@forEach + + receiver.sendDriverMessage( + OutboundTrackerPositionNotification( + trackerId = part.value.toUShort(), + rotation = Quat(state.rotation.x, state.rotation.y, state.rotation.z, state.rotation.w), + position = Vec3f(state.tailPosition.x, state.tailPosition.y, state.tailPosition.z), + // TODO add veliocity data + ), + ) + + val status = OutboundTrackerStatusNotification( + trackerId = part.value.toUShort(), + status = closestTracker?.status ?: TrackerStatus.OK, + batteryLevel = closestDevice?.batteryLevel ?: 1f, + charging = closestDevice?.batteryVoltage != null && closestDevice.batteryVoltage >= 4.3f, + ) + if (lastStatus.put(part.value, status) != status) receiver.sendDriverMessage(status) + } else { + if (part.value !in confirmedTrackers) return@forEach + + val status = OutboundTrackerStatusNotification( + trackerId = part.value.toUShort(), + status = TrackerStatus.DISCONNECTED, + batteryLevel = null, + charging = false, + ) + if (lastStatus.put(part.value, status) != status) receiver.sendDriverMessage(status) + } + } + }.launchIn(receiver.context.scope) + } +} diff --git a/server/core/src/main/java/dev/slimevr/solarxr/module.kt b/server/core/src/main/java/dev/slimevr/solarxr/module.kt index e1169e9b7..2bcbe4a3e 100644 --- a/server/core/src/main/java/dev/slimevr/solarxr/module.kt +++ b/server/core/src/main/java/dev/slimevr/solarxr/module.kt @@ -2,25 +2,35 @@ package dev.slimevr.solarxr import dev.slimevr.AppContextProvider import dev.slimevr.EventDispatcher +import dev.slimevr.Subscription import dev.slimevr.VRServerActions import dev.slimevr.context.Behaviour import dev.slimevr.context.Context import dev.slimevr.context.ManagedContext +import dev.slimevr.solarxr.driver.DriverHandshakeBehaviour +import dev.slimevr.solarxr.driver.DriverIncomingTrackersBehaviour +import dev.slimevr.solarxr.driver.DriverOutgoingTrackersBehaviour import kotlinx.coroutines.CoroutineScope import kotlinx.coroutines.Job import solarxr_protocol.MessageBundle import solarxr_protocol.data_feed.DataFeedConfig import solarxr_protocol.data_feed.DataFeedMessage import solarxr_protocol.data_feed.DataFeedMessageHeader +import solarxr_protocol.driver_protocol.DriverMessage +import solarxr_protocol.driver_protocol.DriverMessageHeader import solarxr_protocol.rpc.RpcMessage import solarxr_protocol.rpc.RpcMessageHeader +import kotlin.time.Duration +import kotlin.time.Duration.Companion.seconds data class SolarXRBridgeState( val dataFeedConfigs: List, + val driverName: String? = null, ) sealed interface SolarXRBridgeActions { data class SetConfig(val configs: List) : SolarXRBridgeActions + data class SetDriverName(val name: String) : SolarXRBridgeActions } typealias SolarXRBridgeContext = Context @@ -34,8 +44,25 @@ suspend fun onSolarXRMessage(message: MessageBundle, context: SolarXRBridge) { message.rpcMsgs?.forEach { val msg = it.message ?: return + val replyTo = it.replyTo + if (replyTo != null) { + context.rpcRequests.tryResolve(replyTo, msg) + return@forEach + } + context.rpcReplies.record(msg, it.txId) context.rpcDispatcher.emit(msg) } + + message.driverMsgs?.forEach { + val msg = it.message ?: return + val replyTo = it.replyTo + if (replyTo != null) { + context.driverRequests.tryResolve(replyTo, msg) + return@forEach + } + context.driverReplies.record(msg, it.txId) + context.driverDispatcher.emit(msg) + } } class SolarXRBridge( @@ -44,6 +71,7 @@ class SolarXRBridge( val appContext: AppContextProvider, val dataFeedDispatcher: EventDispatcher, val rpcDispatcher: EventDispatcher, + val driverDispatcher: EventDispatcher = EventDispatcher("SolarXR[$id].driver", context.scope, capacity = 64), val outbound: EventDispatcher = EventDispatcher("SolarXR[$id].outbound", context.scope, capacity = 64), private val managedContext: ManagedContext? = null, ) { @@ -52,7 +80,24 @@ class SolarXRBridge( internal var datafeedTimers: List = emptyList() fun dispose() = managedContext?.dispose() - suspend fun sendRpc(message: RpcMessage) = outbound.emit(MessageBundle(rpcMsgs = listOf(RpcMessageHeader(message = message)))) + internal val rpcReplies = PendingReplies() + internal val driverReplies = PendingReplies() + + @PublishedApi internal val rpcRequests = PendingRequests() + + @PublishedApi internal val driverRequests = PendingRequests() + + fun txIdFor(message: RpcMessage): UInt? = rpcReplies.consume(message) + fun driverTxIdFor(message: DriverMessage): UInt? = driverReplies.consume(message) + + inline fun onRpc(noinline action: suspend (P, UInt?) -> Unit): Subscription = rpcDispatcher.on

{ msg -> action(msg, txIdFor(msg)) } + inline fun onDriverMessage(noinline action: suspend (P, UInt?) -> Unit): Subscription = driverDispatcher.on

{ msg -> action(msg, driverTxIdFor(msg)) } + + suspend fun sendRpc(message: RpcMessage, replyTo: UInt? = null, txId: UInt? = null) = outbound.emit(MessageBundle(rpcMsgs = listOf(RpcMessageHeader(txId = txId, replyTo = replyTo, message = message)))) + suspend fun sendDriverMessage(message: DriverMessage, replyTo: UInt? = null, txId: UInt? = null) = outbound.emit(MessageBundle(driverMsgs = listOf(DriverMessageHeader(txId = txId, replyTo = replyTo, message = message)))) + + suspend inline fun requestRpc(message: RpcMessage, timeout: Duration = 10.seconds): R = rpcRequests.request(timeout, message) { msg, newTxId -> sendRpc(msg, txId = newTxId) } + suspend inline fun requestDriverMessage(message: DriverMessage, timeout: Duration = 10.seconds): R = driverRequests.request(timeout, message) { msg, newTxId -> sendDriverMessage(msg, txId = newTxId) } suspend fun sendDataFeed(frame: DataFeedMessageHeader) = outbound.emit(MessageBundle(dataFeedMsgs = listOf(frame))) @@ -83,6 +128,9 @@ class SolarXRBridge( add(SkeletonProportionsBehaviour(appContext.config.userConfig, appContext.skeleton)) add(TrackingChecklistBehaviour(appContext.trackingChecklist, appContext.config.settings)) add(AssignTrackerBehaviour(appContext.server)) + add(DriverHandshakeBehaviour(appContext.server)) + add(DriverOutgoingTrackersBehaviour(appContext)) + add(DriverIncomingTrackersBehaviour(appContext.server)) add(MagBehaviour(appContext)) add(KnownTrackersBehaviour(appContext.config.settings)) add(BvhBehaviour(appContext.bvhManager)) diff --git a/server/core/src/main/java/dev/slimevr/solarxr/tx-correlation.kt b/server/core/src/main/java/dev/slimevr/solarxr/tx-correlation.kt new file mode 100644 index 000000000..e0d6c643c --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/solarxr/tx-correlation.kt @@ -0,0 +1,47 @@ +package dev.slimevr.solarxr + +import kotlinx.coroutines.CompletableDeferred +import kotlinx.coroutines.withTimeout +import java.util.Collections +import java.util.IdentityHashMap +import java.util.concurrent.ConcurrentHashMap +import java.util.concurrent.atomic.AtomicInteger +import kotlin.time.Duration + +class PendingReplies { + private val pending = Collections.synchronizedMap(IdentityHashMap()) + + fun record(message: T, txId: UInt?) { + if (txId != null) pending[message] = txId + } + + fun consume(message: T): UInt? = pending.remove(message) +} + +class PendingRequests { + @PublishedApi + internal val counter = AtomicInteger(1) + + @PublishedApi + internal val pending = ConcurrentHashMap>() + + suspend inline fun request(timeout: Duration, message: T, send: suspend (T, UInt) -> Unit): R { + val txId = counter.getAndIncrement().toUInt() + val deferred = CompletableDeferred() + pending[txId] = deferred + try { + send(message, txId) + val response = withTimeout(timeout) { deferred.await() } + return response as? R + ?: error("expected ${R::class.simpleName} but got ${response::class.simpleName}") + } finally { + pending.remove(txId) + } + } + + fun tryResolve(txId: UInt, message: T): Boolean { + val waiter = pending.remove(txId) ?: return false + waiter.complete(message) + return true + } +} diff --git a/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt b/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt index 33b3c6eb9..10887ab66 100644 --- a/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt +++ b/server/core/src/main/java/dev/slimevr/trackingchecklist/behaviours.kt @@ -64,12 +64,12 @@ private fun deviceStatesFlow(server: VRServer): Flow> = allCon class HMDCheckBehaviour(private val server: VRServer) : TrackingChecklistBehaviourType { private fun computeStep(trackers: List): TrackingChecklistStep { // FIXME: Most likely incomplete - val hasSteamVR = trackers.any { tracker -> tracker.origin == DeviceOrigin.DRIVER } + val hasDriverHMD = trackers.any { tracker -> tracker.origin == DeviceOrigin.DRIVER } val hmdTracker = trackers.firstOrNull { tracker -> tracker.origin == DeviceOrigin.DRIVER && tracker.position != null } val isAssigned = hmdTracker?.bodyPart == BodyPart.HEAD return TrackingChecklistStep( valid = isAssigned, - enabled = hasSteamVR, + enabled = hasDriverHMD, ignorable = true, visibility = TrackingChecklistStepVisibility.WHEN_INVALID, extraData = if (!isAssigned) { diff --git a/solarxr-protocol b/solarxr-protocol index d86d41a08..fd6e516f0 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit d86d41a08cd7efd4e8ef10321e29174b13963917 +Subproject commit fd6e516f0fd962d425c8168b1ac6ebdf3e6840f6