Driver through solarxr. Not supporting monado skeleton approach. need packets

This commit is contained in:
loucass003 committed 2026-08-11 22:14:00 +02:00
1 parent 3a605df946
commit c697aabfd7
8 files changed
+400 -4

No files matched your search

@@ -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) {
@@ -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<InboundHandshakeRequest> { 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)
}
}
@@ -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<InboundAddTrackerRequest> { 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<InboundTrackerStatusNotification> { 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<InboundBatteryNotification> { 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<InboundTrackerPositionNotification> { 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)
}
}
@@ -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<Set<BodyPart>> = 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<UByte>()
val confirmedTrackers = ConcurrentHashMap.newKeySet<UByte>()
// 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<UByte, OutboundTrackerStatusNotification>()
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<TrackerState>()
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<OutboundAddTrackerResponse>(
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)
}
}
@@ -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<DataFeedConfig>,
val driverName: String? = null,
)
sealed interface SolarXRBridgeActions {
data class SetConfig(val configs: List<DataFeedConfig>) : SolarXRBridgeActions
data class SetDriverName(val name: String) : SolarXRBridgeActions
}
typealias SolarXRBridgeContext = Context<SolarXRBridgeState, SolarXRBridgeActions>
@@ -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<DataFeedMessage>,
val rpcDispatcher: EventDispatcher<RpcMessage>,
val driverDispatcher: EventDispatcher<DriverMessage> = EventDispatcher("SolarXR[$id].driver", context.scope, capacity = 64),
val outbound: EventDispatcher<MessageBundle> = EventDispatcher("SolarXR[$id].outbound", context.scope, capacity = 64),
private val managedContext: ManagedContext<SolarXRBridgeState, SolarXRBridgeActions>? = null,
) {
@@ -52,7 +80,24 @@ class SolarXRBridge(
internal var datafeedTimers: List<Job> = emptyList()
fun dispose() = managedContext?.dispose()
suspend fun sendRpc(message: RpcMessage) = outbound.emit(MessageBundle(rpcMsgs = listOf(RpcMessageHeader(message = message))))
internal val rpcReplies = PendingReplies<RpcMessage>()
internal val driverReplies = PendingReplies<DriverMessage>()
@PublishedApi internal val rpcRequests = PendingRequests<RpcMessage>()
@PublishedApi internal val driverRequests = PendingRequests<DriverMessage>()
fun txIdFor(message: RpcMessage): UInt? = rpcReplies.consume(message)
fun driverTxIdFor(message: DriverMessage): UInt? = driverReplies.consume(message)
inline fun <reified P : RpcMessage> onRpc(noinline action: suspend (P, UInt?) -> Unit): Subscription<RpcMessage, P> = rpcDispatcher.on<P> { msg -> action(msg, txIdFor(msg)) }
inline fun <reified P : DriverMessage> onDriverMessage(noinline action: suspend (P, UInt?) -> Unit): Subscription<DriverMessage, P> = driverDispatcher.on<P> { 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 <reified R : RpcMessage> requestRpc(message: RpcMessage, timeout: Duration = 10.seconds): R = rpcRequests.request(timeout, message) { msg, newTxId -> sendRpc(msg, txId = newTxId) }
suspend inline fun <reified R : DriverMessage> 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))
@@ -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<T : Any> {
private val pending = Collections.synchronizedMap(IdentityHashMap<T, UInt>())
fun record(message: T, txId: UInt?) {
if (txId != null) pending[message] = txId
}
fun consume(message: T): UInt? = pending.remove(message)
}
class PendingRequests<T : Any> {
@PublishedApi
internal val counter = AtomicInteger(1)
@PublishedApi
internal val pending = ConcurrentHashMap<UInt, CompletableDeferred<T>>()
suspend inline fun <reified R : T> request(timeout: Duration, message: T, send: suspend (T, UInt) -> Unit): R {
val txId = counter.getAndIncrement().toUInt()
val deferred = CompletableDeferred<T>()
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
}
}
@@ -64,12 +64,12 @@ private fun deviceStatesFlow(server: VRServer): Flow<List<DeviceState>> = allCon
class HMDCheckBehaviour(private val server: VRServer) : TrackingChecklistBehaviourType {
private fun computeStep(trackers: List<TrackerState>): 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) {