From 488838752ba7ae7900ee52ff8cfcf120e469b193 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Fri, 1 Oct 2021 12:27:04 +0300 Subject: [PATCH] Implement new named pipe bridge and test it lightly --- .../dev/slimevr/bridge/NamedPipeBridge.java | 154 ++++++++++++++++-- .../dev/slimevr/bridge/NamedPipeVRBridge.java | 1 + src/main/java/dev/slimevr/bridge/Pipe.java | 24 ++- .../java/dev/slimevr/bridge/PipeState.java | 7 - .../dev/slimevr/bridge/ProtobufBridge.java | 52 +++++- .../dev/slimevr/bridge/ProtobufMessages.java | 15 +- .../bridge/SteamVRPipeInputBridge.java | 1 + src/main/java/io/eiren/vr/VRServer.java | 13 +- 8 files changed, 228 insertions(+), 39 deletions(-) delete mode 100644 src/main/java/dev/slimevr/bridge/PipeState.java diff --git a/src/main/java/dev/slimevr/bridge/NamedPipeBridge.java b/src/main/java/dev/slimevr/bridge/NamedPipeBridge.java index 00d701709..0174e65fb 100644 --- a/src/main/java/dev/slimevr/bridge/NamedPipeBridge.java +++ b/src/main/java/dev/slimevr/bridge/NamedPipeBridge.java @@ -1,27 +1,36 @@ package dev.slimevr.bridge; +import java.io.IOException; + +import com.google.protobuf.CodedOutputStream; +import com.sun.jna.platform.win32.Kernel32; +import com.sun.jna.platform.win32.WinBase; +import com.sun.jna.platform.win32.WinError; +import com.sun.jna.ptr.IntByReference; + +import dev.slimevr.bridge.Pipe.PipeState; import dev.slimevr.bridge.ProtobufMessages.ProtobufMessage; import dev.slimevr.bridge.ProtobufMessages.TrackerAdded; import io.eiren.util.ann.VRServerThread; +import io.eiren.util.logging.LogManager; +import io.eiren.vr.Main; +import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.TrackerPosition; import io.eiren.vr.trackers.TrackerRole; import io.eiren.vr.trackers.VRTracker; public class NamedPipeBridge extends ProtobufBridge implements Runnable { + + private final byte[] buffArray = new byte[2048]; + protected Pipe pipe; protected final String pipeName; protected final Thread runnerThread; - public NamedPipeBridge(String pipeName) { + public NamedPipeBridge(HMDTracker hmd, String bridgeName, String pipeName) { + super(bridgeName, hmd); this.pipeName = pipeName; - runnerThread = new Thread(this, "Named pipe thread"); - } - - @Override - @BridgeThread - protected void sendMessageReal(ProtobufMessage message) { - // TODO Auto-generated method stub - + this.runnerThread = new Thread(this, "Named pipe thread"); } @Override @@ -38,8 +47,127 @@ public class NamedPipeBridge extends ProtobufBridge implements Runnab @Override @BridgeThread public void run() { - // TODO Auto-generated method stub - + try { + createPipe(); + while(true) { + boolean pipesUpdated = false; + if(pipe.state == PipeState.CREATED) { + tryOpeningPipe(pipe); + } + if(pipe.state == PipeState.OPEN) { + pipesUpdated = updatePipe(); + updateMessageQueue(); + } + if(pipe.state == PipeState.ERROR) { + resetPipe(); + } + if(!pipesUpdated) { + try { + Thread.sleep(5); // Up to 200Hz + } catch(InterruptedException e) { + e.printStackTrace(); + } + } + } + } catch(IOException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + + @Override + @BridgeThread + protected boolean sendMessageReal(ProtobufMessage message) { + if(pipe.state == PipeState.OPEN) { + try { + int size = message.getSerializedSize(); + CodedOutputStream os = CodedOutputStream.newInstance(buffArray, 4, size); + message.writeTo(os); + size += 4; + buffArray[0] = (byte) (size & 0xFF); + buffArray[1] = (byte) ((size >> 8) & 0xFF); + buffArray[2] = (byte) ((size >> 16) & 0xFF); + buffArray[3] = (byte) ((size >> 24) & 0xFF); + if(Kernel32.INSTANCE.WriteFile(pipe.pipeHandle, buffArray, size, null, null)) { + return true; + } + pipe.state = PipeState.ERROR; + LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Kernel32.INSTANCE.GetLastError()); + } catch(IOException e) { + e.printStackTrace(); + } + } + return false; + } + + private boolean updatePipe() throws IOException { + if(pipe.state == PipeState.OPEN) { + boolean readAnything = false; + IntByReference bytesAvailable = new IntByReference(0); + while(Kernel32.INSTANCE.PeekNamedPipe(pipe.pipeHandle, buffArray, 4, null, bytesAvailable, null)) { + if(bytesAvailable.getValue() >= 4) { // Got size + int messageLength = (buffArray[3] << 24) | (buffArray[2] << 16) | (buffArray[1] << 8) | buffArray[0]; + if(messageLength > 1024) { // Overflow + LogManager.log.severe("[" + bridgeName + "] Pipe overflow. Message length: " + messageLength); + pipe.state = PipeState.ERROR; + return readAnything; + } + if(bytesAvailable.getValue() >= messageLength) { + if(Kernel32.INSTANCE.ReadFile(pipe.pipeHandle, buffArray, messageLength, bytesAvailable, null)) { + ProtobufMessage message = ProtobufMessage.parser().parseFrom(buffArray, 4, messageLength - 4); + messageRecieved(message); + readAnything = true; + } else { + pipe.state = PipeState.ERROR; + LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Kernel32.INSTANCE.GetLastError()); + return readAnything; + } + } else { + return readAnything; // Wait for more data + } + } else { + return readAnything; // Wait for more data + } + } + pipe.state = PipeState.ERROR; + LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Kernel32.INSTANCE.GetLastError()); + } + return false; + } + + private void resetPipe() { + Pipe.safeDisconnect(pipe); + pipe.state = PipeState.CREATED; + } + + private void createPipe() throws IOException { + try { + pipe = new Pipe(Kernel32.INSTANCE.CreateNamedPipe(pipeName, WinBase.PIPE_ACCESS_DUPLEX, // dwOpenMode + WinBase.PIPE_TYPE_BYTE | WinBase.PIPE_READMODE_BYTE | WinBase.PIPE_WAIT, // dwPipeMode + 1, // nMaxInstances, + 1024 * 16, // nOutBufferSize, + 1024 * 16, // nInBufferSize, + 0, // nDefaultTimeOut, + null), pipeName); // lpSecurityAttributes + LogManager.log.info("[" + bridgeName + "] Pipe " + pipe.name + " created"); + if(WinBase.INVALID_HANDLE_VALUE.equals(pipe.pipeHandle)) + throw new IOException("Can't open " + pipeName + " pipe: " + Kernel32.INSTANCE.GetLastError()); + LogManager.log.info("[" + bridgeName + "] Pipes are created"); + } catch(IOException e) { + Pipe.safeDisconnect(pipe); + throw e; + } + } + + private boolean tryOpeningPipe(Pipe pipe) { + if(Kernel32.INSTANCE.ConnectNamedPipe(pipe.pipeHandle, null) || Kernel32.INSTANCE.GetLastError() == WinError.ERROR_PIPE_CONNECTED) { + pipe.state = PipeState.OPEN; + LogManager.log.info("[" + bridgeName + "] Pipe " + pipe.name + " is open"); + Main.vrServer.queueTask(this::reconnected); + return true; + } + LogManager.log.info("[" + bridgeName + "] Error connecting to pipe " + pipe.name + ": " + Kernel32.INSTANCE.GetLastError()); + return false; } @Override @@ -47,8 +175,4 @@ public class NamedPipeBridge extends ProtobufBridge implements Runnab public void startBridge() { runnerThread.start(); } - - - - } diff --git a/src/main/java/dev/slimevr/bridge/NamedPipeVRBridge.java b/src/main/java/dev/slimevr/bridge/NamedPipeVRBridge.java index db6d997ea..7e216c469 100644 --- a/src/main/java/dev/slimevr/bridge/NamedPipeVRBridge.java +++ b/src/main/java/dev/slimevr/bridge/NamedPipeVRBridge.java @@ -12,6 +12,7 @@ import com.sun.jna.platform.win32.WinBase; import com.sun.jna.platform.win32.WinNT.HANDLE; import com.sun.jna.ptr.IntByReference; +import dev.slimevr.bridge.Pipe.PipeState; import io.eiren.util.collections.FastList; import io.eiren.util.logging.LogManager; import io.eiren.vr.VRServer; diff --git a/src/main/java/dev/slimevr/bridge/Pipe.java b/src/main/java/dev/slimevr/bridge/Pipe.java index 50e72792e..8c4d354ae 100644 --- a/src/main/java/dev/slimevr/bridge/Pipe.java +++ b/src/main/java/dev/slimevr/bridge/Pipe.java @@ -1,14 +1,30 @@ package dev.slimevr.bridge; +import com.sun.jna.platform.win32.Kernel32; import com.sun.jna.platform.win32.WinNT.HANDLE; -class Pipe { - final String name; - final HANDLE pipeHandle; - PipeState state = PipeState.CREATED; +public class Pipe { + + public final String name; + public final HANDLE pipeHandle; + public PipeState state = PipeState.CREATED; public Pipe(HANDLE pipeHandle, String name) { this.pipeHandle = pipeHandle; this.name = name; } + + public static void safeDisconnect(Pipe pipe) { + try { + if(pipe != null && pipe.pipeHandle != null) + Kernel32.INSTANCE.DisconnectNamedPipe(pipe.pipeHandle); + } catch(Exception e) { + } + } + + enum PipeState { + CREATED, + OPEN, + ERROR; + } } \ No newline at end of file diff --git a/src/main/java/dev/slimevr/bridge/PipeState.java b/src/main/java/dev/slimevr/bridge/PipeState.java deleted file mode 100644 index daf5db30d..000000000 --- a/src/main/java/dev/slimevr/bridge/PipeState.java +++ /dev/null @@ -1,7 +0,0 @@ -package dev.slimevr.bridge; - -enum PipeState { - CREATED, - OPEN, - ERROR; -} \ No newline at end of file diff --git a/src/main/java/dev/slimevr/bridge/ProtobufBridge.java b/src/main/java/dev/slimevr/bridge/ProtobufBridge.java index 15f9c2940..0dcd205d8 100644 --- a/src/main/java/dev/slimevr/bridge/ProtobufBridge.java +++ b/src/main/java/dev/slimevr/bridge/ProtobufBridge.java @@ -18,8 +18,12 @@ import io.eiren.util.ann.Synchronize; import io.eiren.util.ann.ThreadSafe; import io.eiren.util.ann.VRServerThread; import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; import io.eiren.vr.Main; +import io.eiren.vr.trackers.ComputedTracker; +import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerRole; import io.eiren.vr.trackers.VRTracker; public abstract class ProtobufBridge implements Bridge { @@ -40,11 +44,17 @@ public abstract class ProtobufBridge implements Bridge { private boolean hadNewData = false; - public ProtobufBridge() { + private T hmdTracker; + private final HMDTracker hmd; + protected final String bridgeName; + + public ProtobufBridge(String bridgeName, HMDTracker hmd) { + this.bridgeName = bridgeName; + this.hmd = hmd; } @BridgeThread - protected abstract void sendMessageReal(ProtobufMessage message); + protected abstract boolean sendMessageReal(ProtobufMessage message); @BridgeThread protected void messageRecieved(ProtobufMessage message) { @@ -60,7 +70,8 @@ public abstract class ProtobufBridge implements Bridge { protected void updateMessageQueue() { ProtobufMessage message = null; while((message = outputQueue.poll()) != null) { - sendMessageReal(message); + if(!sendMessageReal(message)) + return; } } @@ -73,6 +84,16 @@ public abstract class ProtobufBridge implements Bridge { processMessageRecieved(message); hadNewData = true; } + if(hadNewData && hmdTracker != null) { + trackerOverrideUpdate(hmdTracker, hmd); + } + } + + protected void trackerOverrideUpdate(T source, ComputedTracker target) { + target.position.set(source.position); + target.rotation.set(source.rotation); + target.setStatus(source.getStatus()); + target.dataTick(); } @VRServerThread @@ -104,6 +125,8 @@ public abstract class ProtobufBridge implements Bridge { @VRServerThread protected void processMessageRecieved(ProtobufMessage message) { + //if(!message.hasPosition()) + // LogManager.log.info("[" + bridgeName + "] MSG: " + message); if(message.hasPosition()) { positionRecieved(message.getPosition()); } else if(message.hasUserAction()) { @@ -131,13 +154,23 @@ public abstract class ProtobufBridge implements Bridge { @VRServerThread protected void trackerAddedRecieved(TrackerAdded trackerAdded) { - T tracker = createNewTracker(trackerAdded); + T tracker = getInternalRemoteTrackerById(trackerAdded.getTrackerId()); + if(tracker != null) { + // TODO reinit? + return; + } + tracker = createNewTracker(trackerAdded); synchronized(remoteTrackersBySerial) { remoteTrackersBySerial.put(tracker.getName(), tracker); } synchronized(remoteTrackersByTrackerId) { remoteTrackersByTrackerId.put(tracker.getTrackerId(), tracker); } + if(trackerAdded.getTrackerRole() == TrackerRole.HMD.id) { + hmdTracker = tracker; + } else { + Main.vrServer.registerTracker(tracker); + } } @VRServerThread @@ -165,9 +198,20 @@ public abstract class ProtobufBridge implements Bridge { } } + @VRServerThread + protected void reconnected() { + for(int i = 0; i < localTrackers.size(); ++i) { + Tracker tracker = localTrackers.get(i); + TrackerAdded.Builder builder = TrackerAdded.newBuilder().setTrackerId(tracker.getTrackerId()).setTrackerName(tracker.getDescriptiveName()).setTrackerSerial(tracker.getName()).setTrackerRole(tracker.getBodyPosition().trackerRole.id); + sendMessage(ProtobufMessage.newBuilder().setTrackerAdded(builder).build()); + } + } + @VRServerThread @Override public void addSharedTracker(Tracker tracker) { + if(localTrackers.contains(tracker)) + return; localTrackers.add(tracker); TrackerAdded.Builder builder = TrackerAdded.newBuilder().setTrackerId(tracker.getTrackerId()).setTrackerName(tracker.getDescriptiveName()).setTrackerSerial(tracker.getName()).setTrackerRole(tracker.getBodyPosition().trackerRole.id); sendMessage(ProtobufMessage.newBuilder().setTrackerAdded(builder).build()); diff --git a/src/main/java/dev/slimevr/bridge/ProtobufMessages.java b/src/main/java/dev/slimevr/bridge/ProtobufMessages.java index 3e7e1b52d..d7f536a45 100644 --- a/src/main/java/dev/slimevr/bridge/ProtobufMessages.java +++ b/src/main/java/dev/slimevr/bridge/ProtobufMessages.java @@ -259,7 +259,7 @@ public final class ProtobufMessages { dev.slimevr.bridge.ProtobufMessages.PingPong.class, dev.slimevr.bridge.ProtobufMessages.PingPong.Builder.class); } - // Construct using messages.ProtobufMessages.PingPong.newBuilder() + // Construct using dev.slimevr.bridge.ProtobufMessages.PingPong.newBuilder() private Builder() { maybeForceBuilderInitialization(); } @@ -1205,7 +1205,7 @@ public final class ProtobufMessages { dev.slimevr.bridge.ProtobufMessages.Position.class, dev.slimevr.bridge.ProtobufMessages.Position.Builder.class); } - // Construct using messages.ProtobufMessages.Position.newBuilder() + // Construct using dev.slimevr.bridge.ProtobufMessages.Position.newBuilder() private Builder() { maybeForceBuilderInitialization(); } @@ -2281,7 +2281,7 @@ public final class ProtobufMessages { dev.slimevr.bridge.ProtobufMessages.UserAction.class, dev.slimevr.bridge.ProtobufMessages.UserAction.Builder.class); } - // Construct using messages.ProtobufMessages.UserAction.newBuilder() + // Construct using dev.slimevr.bridge.ProtobufMessages.UserAction.newBuilder() private Builder() { maybeForceBuilderInitialization(); } @@ -3120,7 +3120,7 @@ public final class ProtobufMessages { dev.slimevr.bridge.ProtobufMessages.TrackerAdded.class, dev.slimevr.bridge.ProtobufMessages.TrackerAdded.Builder.class); } - // Construct using messages.ProtobufMessages.TrackerAdded.newBuilder() + // Construct using dev.slimevr.bridge.ProtobufMessages.TrackerAdded.newBuilder() private Builder() { maybeForceBuilderInitialization(); } @@ -4384,7 +4384,7 @@ public final class ProtobufMessages { dev.slimevr.bridge.ProtobufMessages.TrackerStatus.class, dev.slimevr.bridge.ProtobufMessages.TrackerStatus.Builder.class); } - // Construct using messages.ProtobufMessages.TrackerStatus.newBuilder() + // Construct using dev.slimevr.bridge.ProtobufMessages.TrackerStatus.newBuilder() private Builder() { maybeForceBuilderInitialization(); } @@ -5465,7 +5465,7 @@ public final class ProtobufMessages { dev.slimevr.bridge.ProtobufMessages.ProtobufMessage.class, dev.slimevr.bridge.ProtobufMessages.ProtobufMessage.Builder.class); } - // Construct using messages.ProtobufMessages.ProtobufMessage.newBuilder() + // Construct using dev.slimevr.bridge.ProtobufMessages.ProtobufMessage.newBuilder() private Builder() { maybeForceBuilderInitialization(); } @@ -6348,7 +6348,8 @@ public final class ProtobufMessages { "\030\002 \001(\0132\024.messages.UserActionH\000\022/\n\rtracke" + "r_added\030\003 \001(\0132\026.messages.TrackerAddedH\000\022" + "1\n\016tracker_status\030\004 \001(\0132\027.messages.Track" + - "erStatusH\000B\t\n\007messageb\006proto3" + "erStatusH\000B\t\n\007messageB&\n\022dev.slimevr.bri" + + "dgeB\020ProtobufMessagesb\006proto3" }; descriptor = com.google.protobuf.Descriptors.FileDescriptor .internalBuildGeneratedFileFrom(descriptorData, diff --git a/src/main/java/dev/slimevr/bridge/SteamVRPipeInputBridge.java b/src/main/java/dev/slimevr/bridge/SteamVRPipeInputBridge.java index 7edb3fc1b..cfd90aa68 100644 --- a/src/main/java/dev/slimevr/bridge/SteamVRPipeInputBridge.java +++ b/src/main/java/dev/slimevr/bridge/SteamVRPipeInputBridge.java @@ -15,6 +15,7 @@ import com.sun.jna.platform.win32.Kernel32; import com.sun.jna.platform.win32.WinBase; import com.sun.jna.ptr.IntByReference; +import dev.slimevr.bridge.Pipe.PipeState; import io.eiren.util.collections.FastList; import io.eiren.util.logging.LogManager; import io.eiren.vr.VRServer; diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java index 2f9a43a48..2126460b0 100644 --- a/src/main/java/io/eiren/vr/VRServer.java +++ b/src/main/java/io/eiren/vr/VRServer.java @@ -16,8 +16,7 @@ import java.util.concurrent.LinkedBlockingQueue; import java.util.function.Consumer; import dev.slimevr.bridge.Bridge; -import dev.slimevr.bridge.NamedPipeVRBridge; -import dev.slimevr.bridge.SteamVRPipeInputBridge; +import dev.slimevr.bridge.NamedPipeBridge; import dev.slimevr.bridge.VMCBridge; import dev.slimevr.bridge.WebSocketVRBridge; import io.eiren.util.OperatingSystem; @@ -62,6 +61,7 @@ public class VRServer extends Thread { // OpenVR bridge currently only supports Windows if(OperatingSystem.getCurrentPlatform() == OperatingSystem.WINDOWS) { + /* // Create named pipe bridge for SteamVR driver NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers, this); tasks.add(() -> driverBridge.startBridge()); @@ -70,6 +70,15 @@ public class VRServer extends Thread { SteamVRPipeInputBridge steamVRInput = new SteamVRPipeInputBridge(this); tasks.add(() -> steamVRInput.startBridge()); bridges.add(steamVRInput); + //*/ + NamedPipeBridge driverBridge = new NamedPipeBridge(hmdTracker, "SteamVR Driver Bridge", "\\\\.\\pipe\\SlimeVRDriver"); + tasks.add(() -> driverBridge.startBridge()); + tasks.add(() -> { + // TODO : THIS SHOULD BE HANDLED COMPLETELY DIFFERENT AND FOR EACH BRIDGE TOGETHER + for(int i = 0; i < shareTrackers.size(); ++i) + driverBridge.addSharedTracker(shareTrackers.get(i)); + }); + bridges.add(driverBridge); } // Create WebSocket server