From e18ce338e93adc70b41858d608c8c5908ed42099 Mon Sep 17 00:00:00 2001 From: ColdIce Date: Wed, 16 Feb 2022 01:25:15 -0600 Subject: [PATCH] restart branch --- src/main/java/dev/slimevr/VRServer.java | 19 +- .../java/dev/slimevr/gui/VRServerGUI.java | 65 ++++ .../platform/linux/LinuxNamedPipeBridge.java | 221 +++++++++++++ .../linux/LinuxNamedPipeVRBridge.java | 282 +++++++++++++++++ .../dev/slimevr/platform/linux/LinuxPipe.java | 26 ++ .../linux/LinuxSteamVRPipeInputBridge.java | 291 ++++++++++++++++++ 6 files changed, 903 insertions(+), 1 deletion(-) create mode 100644 src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeBridge.java create mode 100644 src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeVRBridge.java create mode 100644 src/main/java/dev/slimevr/platform/linux/LinuxPipe.java create mode 100644 src/main/java/dev/slimevr/platform/linux/LinuxSteamVRPipeInputBridge.java diff --git a/src/main/java/dev/slimevr/VRServer.java b/src/main/java/dev/slimevr/VRServer.java index 4058a112f..d79d31689 100644 --- a/src/main/java/dev/slimevr/VRServer.java +++ b/src/main/java/dev/slimevr/VRServer.java @@ -16,6 +16,8 @@ import java.util.concurrent.LinkedBlockingQueue; import java.util.function.Consumer; import dev.slimevr.bridge.Bridge; +import dev.slimevr.platform.linux.LinuxNamedPipeBridge; +import dev.slimevr.platform.linux.LinuxSteamVRPipeInputBridge; import dev.slimevr.platform.windows.WindowsNamedPipeBridge; import dev.slimevr.platform.windows.WindowsSteamVRPipeInputBridge; import dev.slimevr.bridge.VMCBridge; @@ -62,7 +64,7 @@ public class VRServer extends Thread { // Start server for SlimeVR trackers trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker); - // OpenVR bridge currently only supports Windows + // OpenVR bridge currently only supports Windows and Linux if(OperatingSystem.getCurrentPlatform() == OperatingSystem.WINDOWS) { /* // Create named pipe bridge for SteamVR driver @@ -78,6 +80,21 @@ public class VRServer extends Thread { WindowsNamedPipeBridge driverBridge = new WindowsNamedPipeBridge(hmdTracker, "steamvr", "SteamVR Driver Bridge", "\\\\.\\pipe\\SlimeVRDriver", shareTrackers); tasks.add(() -> driverBridge.startBridge()); bridges.add(driverBridge); + } else if (OperatingSystem.getCurrentPlatform() == OperatingSystem.LINUX) { + /* + // Create named pipe bridge for SteamVR driver + NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers, this); + tasks.add(() -> driverBridge.startBridge()); + bridges.add(driverBridge); + //*/ + // Create named pipe bridge for SteamVR input + LinuxSteamVRPipeInputBridge steamVRInput = new LinuxSteamVRPipeInputBridge(this); + tasks.add(() -> steamVRInput.startBridge()); + bridges.add(steamVRInput); + //*/ + LinuxNamedPipeBridge driverBridge = new LinuxNamedPipeBridge(hmdTracker, "steamvr", "SteamVR Driver Bridge", "\\\\.\\pipe\\SlimeVRDriver", shareTrackers); + tasks.add(() -> driverBridge.startBridge()); + bridges.add(driverBridge); } // Create WebSocket server diff --git a/src/main/java/dev/slimevr/gui/VRServerGUI.java b/src/main/java/dev/slimevr/gui/VRServerGUI.java index 1721a0848..fbd1d8c4a 100644 --- a/src/main/java/dev/slimevr/gui/VRServerGUI.java +++ b/src/main/java/dev/slimevr/gui/VRServerGUI.java @@ -7,6 +7,7 @@ import javax.swing.event.MouseInputAdapter; import dev.slimevr.Main; import dev.slimevr.VRServer; +import dev.slimevr.platform.linux.LinuxNamedPipeBridge; import dev.slimevr.platform.windows.WindowsNamedPipeBridge; import dev.slimevr.gui.swing.ButtonTimer; import dev.slimevr.gui.swing.EJBagNoStretch; @@ -337,6 +338,70 @@ public class VRServerGUI extends JFrame { add(Box.createVerticalStrut(10)); + } else if(server.hasBridge(LinuxNamedPipeBridge.class)) { // Linux GUI ver + LinuxNamedPipeBridge br = server.getVRBridge(LinuxNamedPipeBridge.class); + add(l = new JLabel("SteamVR Trackers")); + l.setFont(l.getFont().deriveFont(Font.BOLD)); + l.setAlignmentX(0.5f); + add(l = new JLabel("Changes may require restart of SteamVR")); + l.setFont(l.getFont().deriveFont(Font.ITALIC)); + l.setAlignmentX(0.5f); + + add(new EJBagNoStretch(false, true) {{ + JCheckBox waistCb; + add(waistCb = new JCheckBox("Waist"), c(1, 1)); + waistCb.setSelected(br.getShareSetting(TrackerRole.WAIST)); + waistCb.addActionListener(new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + server.queueTask(() -> { + br.changeShareSettings(TrackerRole.WAIST, waistCb.isSelected()); + }); + } + }); + + JCheckBox legsCb; + add(legsCb = new JCheckBox("Legs"), c(2, 1)); + legsCb.setSelected(br.getShareSetting(TrackerRole.LEFT_FOOT) && br.getShareSetting(TrackerRole.RIGHT_FOOT)); + legsCb.addActionListener(new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + server.queueTask(() -> { + br.changeShareSettings(TrackerRole.LEFT_FOOT, legsCb.isSelected()); + br.changeShareSettings(TrackerRole.RIGHT_FOOT, legsCb.isSelected()); + }); + } + }); + + JCheckBox chestCb; + add(chestCb = new JCheckBox("Chest"), c(1, 2)); + chestCb.setSelected(br.getShareSetting(TrackerRole.CHEST)); + chestCb.addActionListener(new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + server.queueTask(() -> { + br.changeShareSettings(TrackerRole.CHEST, chestCb.isSelected()); + }); + } + }); + + JCheckBox kneesCb; + add(kneesCb = new JCheckBox("Knees"), c(2, 2)); + kneesCb.setSelected(br.getShareSetting(TrackerRole.LEFT_KNEE) && br.getShareSetting(TrackerRole.RIGHT_KNEE)); + kneesCb.addActionListener(new ActionListener() { + @Override + public void actionPerformed(ActionEvent e) { + server.queueTask(() -> { + br.changeShareSettings(TrackerRole.LEFT_KNEE, kneesCb.isSelected()); + br.changeShareSettings(TrackerRole.RIGHT_KNEE, kneesCb.isSelected()); + }); + } + }); + + }}); + + + add(Box.createVerticalStrut(10)); } add(new JLabel("Skeleton data")); add(skeletonList); diff --git a/src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeBridge.java b/src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeBridge.java new file mode 100644 index 000000000..df1d171e9 --- /dev/null +++ b/src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeBridge.java @@ -0,0 +1,221 @@ +package dev.slimevr.platform.linux; + +import com.google.protobuf.CodedOutputStream; +import java.nio.*; +import com.sun.jna.Native; +import com.sun.jna.ptr.IntByReference; +import dev.slimevr.Main; +import dev.slimevr.bridge.BridgeThread; +import dev.slimevr.bridge.PipeState; +import dev.slimevr.bridge.ProtobufBridge; +import dev.slimevr.bridge.ProtobufMessages; +import dev.slimevr.util.ann.VRServerThread; +import dev.slimevr.vr.trackers.*; +import io.eiren.util.logging.LogManager; + +import java.io.IOException; +import java.io.RandomAccessFile; +import java.nio.channels.FileChannel; +import java.util.List; + +public class LinuxNamedPipeBridge extends ProtobufBridge implements Runnable { + + private final TrackerRole[] defaultRoles = new TrackerRole[] {TrackerRole.WAIST, TrackerRole.LEFT_FOOT, TrackerRole.RIGHT_FOOT}; + + private final byte[] buffArray = new byte[2048]; + + protected LinuxPipe pipe; + protected final String pipeName; + protected final String bridgeSettingsKey; + protected final Thread runnerThread; + private final List shareableTrackers; + + public LinuxNamedPipeBridge(HMDTracker hmd, String bridgeSettingsKey, String bridgeName, String pipeName, List shareableTrackers) { + super(bridgeName, hmd); + this.pipeName = pipeName; + this.bridgeSettingsKey = bridgeSettingsKey; + this.runnerThread = new Thread(this, "Named pipe thread"); + this.shareableTrackers = shareableTrackers; + } + + @Override + @VRServerThread + public void startBridge() { + for(TrackerRole role : defaultRoles) { + changeShareSettings(role, Main.vrServer.config.getBoolean("bridge." + bridgeSettingsKey + ".trackers." + role.name().toLowerCase(), true)); + } + for(int i = 0; i < shareableTrackers.size(); ++i) { + ShareableTracker tr = shareableTrackers.get(i); + TrackerRole role = tr.getTrackerRole(); + changeShareSettings(role, Main.vrServer.config.getBoolean("bridge." + bridgeSettingsKey + ".trackers." + role.name().toLowerCase(), false)); + } + runnerThread.start(); + } + + @VRServerThread + public boolean getShareSetting(TrackerRole role) { + for(int i = 0; i < shareableTrackers.size(); ++i) { + ShareableTracker tr = shareableTrackers.get(i); + if(tr.getTrackerRole() == role) { + return sharedTrackers.contains(tr); + } + } + return false; + } + + @VRServerThread + public void changeShareSettings(TrackerRole role, boolean share) { + if(role == null) + return; + for(int i = 0; i < shareableTrackers.size(); ++i) { + ShareableTracker tr = shareableTrackers.get(i); + if(tr.getTrackerRole() == role) { + if(share) { + addSharedTracker(tr); + } else { + removeSharedTracker(tr); + } + Main.vrServer.config.setProperty("bridge." + bridgeSettingsKey + ".trackers." + role.name().toLowerCase(), share); + Main.vrServer.saveConfig(); + } + } + } + + @Override + @VRServerThread + protected VRTracker createNewTracker(ProtobufMessages.TrackerAdded trackerAdded) { + VRTracker tracker = new VRTracker(trackerAdded.getTrackerId(), trackerAdded.getTrackerSerial(), trackerAdded.getTrackerName(), true, true); + TrackerRole role = TrackerRole.getById(trackerAdded.getTrackerRole()); + if(role != null) { + tracker.setBodyPosition(TrackerPosition.getByRole(role)); + } + return tracker; + } + + @Override + @BridgeThread + public void run() { + 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(ProtobufMessages.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); + try { + pipe.pipe.write(ByteBuffer.wrap(buffArray)); + return true; + } catch (IOException e) { + pipe.state = PipeState.ERROR; + LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Native.getLastError()); + e.printStackTrace(); + } + } 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(pipe.pipe.isOpen()) { + 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(pipe.pipe.read(ByteBuffer.wrap(buffArray)) == 0) { + ProtobufMessages.ProtobufMessage message = ProtobufMessages.ProtobufMessage.parser().parseFrom(buffArray, 4, messageLength - 4); + messageReceived(message); + readAnything = true; + } else { + pipe.state = PipeState.ERROR; + LogManager.log.severe("[" + bridgeName + "] Pipe error: " + Native.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: " + Native.getLastError()); + } + return false; + } + + private void resetPipe() { + LinuxPipe.safeDisconnect(pipe); + pipe.state = PipeState.CREATED; + Main.vrServer.queueTask(this::disconnected); + } + + private void createPipe() throws IOException { + try { + RandomAccessFile rw = new RandomAccessFile(pipeName, "rw"); + FileChannel fc = rw.getChannel(); + pipe = new LinuxPipe(fc, pipeName); // lpSecurityAttributes + LogManager.log.info("[SteamVRPipeInputBridge] Pipe " + pipe.name + " created"); + //if(pipe.pipe.) + // throw new IOException("Can't open " + pipeName + " pipe: " + Native.getLastError()); + LogManager.log.info("[SteamVRPipeInputBridge] Pipes are open"); + } catch(IOException e) { + LinuxPipe.safeDisconnect(pipe); + throw e; + } + } + + + private boolean tryOpeningPipe(LinuxPipe pipe) { + if(pipe.pipe.isOpen()) { + 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 + ": " + Native.getLastError()); + return false; + } +} diff --git a/src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeVRBridge.java b/src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeVRBridge.java new file mode 100644 index 000000000..2dcf25f5e --- /dev/null +++ b/src/main/java/dev/slimevr/platform/linux/LinuxNamedPipeVRBridge.java @@ -0,0 +1,282 @@ +package dev.slimevr.platform.linux; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; +import com.sun.jna.Native; +import com.sun.jna.ptr.IntByReference; +import dev.slimevr.VRServer; +import dev.slimevr.bridge.Bridge; +import dev.slimevr.bridge.PipeState; +import dev.slimevr.vr.trackers.*; +import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; + +import java.io.IOException; +import java.io.RandomAccessFile; +import java.nio.ByteBuffer; +import java.nio.channels.FileChannel; +import java.nio.charset.Charset; +import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; + +public class LinuxNamedPipeVRBridge extends Thread implements Bridge { + + private static final int MAX_COMMAND_LENGTH = 2048; + public static final String HMDPipeName = "\\\\.\\pipe\\HMDPipe"; + public static final String TrackersPipeName = "\\\\.\\pipe\\TrackPipe"; + public static final Charset ASCII = Charset.forName("ASCII"); + + private final byte[] buffArray = new byte[1024]; + private final StringBuilder commandBuilder = new StringBuilder(1024); + private final StringBuilder sbBuffer = new StringBuilder(1024); + private final Vector3f vBuffer = new Vector3f(); + private final Vector3f vBuffer2 = new Vector3f(); + private final Quaternion qBuffer = new Quaternion(); + private final Quaternion qBuffer2 = new Quaternion(); + + private LinuxPipe hmdPipe; + private final HMDTracker hmd; + private final List trackerPipes; + private final List shareTrackers; + private final List internalTrackers; + + private final HMDTracker internalHMDTracker = new HMDTracker("internal://HMD"); + private final AtomicBoolean newHMDData = new AtomicBoolean(false); + + public LinuxNamedPipeVRBridge(HMDTracker hmd, List shareTrackers, VRServer server) { + super("Named Pipe VR Bridge"); + this.hmd = hmd; + this.shareTrackers = new FastList<>(shareTrackers); + this.trackerPipes = new FastList<>(shareTrackers.size()); + this.internalTrackers = new FastList<>(shareTrackers.size()); + for(int i = 0; i < shareTrackers.size(); ++i) { + Tracker t = shareTrackers.get(i); + ComputedTracker ct = new ComputedTracker(t.getTrackerId(), "internal://" + t.getName(), true, true); + ct.setStatus(TrackerStatus.OK); + this.internalTrackers.add(ct); + } + } + + @Override + public void run() { + try { + createPipes(); + while(true) { + waitForPipesToOpen(); + if(areAllPipesOpen()) { + boolean hmdUpdated = updateHMD(); // Update at HMDs frequency + for(int i = 0; i < trackerPipes.size(); ++i) { + updateTracker(i, hmdUpdated); + } + if(!hmdUpdated) { + Thread.sleep(5); // Up to 200Hz + } + } + } + } catch(Exception e) { + e.printStackTrace(); + } + } + + @Override + public void dataRead() { + if(newHMDData.compareAndSet(true, false)) { + hmd.position.set(internalHMDTracker.position); + hmd.rotation.set(internalHMDTracker.rotation); + hmd.dataTick(); + } + } + + @Override + public void dataWrite() { + for(int i = 0; i < shareTrackers.size(); ++i) { + Tracker t = shareTrackers.get(i); + ComputedTracker it = this.internalTrackers.get(i); + if(t.getPosition(vBuffer2)) + it.position.set(vBuffer2); + if(t.getRotation(qBuffer2)) + it.rotation.set(qBuffer2); + } + } + + private void waitForPipesToOpen() { + if(hmdPipe.state == PipeState.CREATED) { + if(tryOpeningPipe(hmdPipe)) + initHMDPipe(hmdPipe); + } + for(int i = 0; i < trackerPipes.size(); ++i) { + LinuxPipe trackerPipe = trackerPipes.get(i); + if(trackerPipe.state == PipeState.CREATED) { + if(tryOpeningPipe(trackerPipe)) + initTrackerPipe(trackerPipe, i); + } + } + } + + public boolean updateHMD() throws IOException { + if(hmdPipe.state == PipeState.OPEN) { + IntByReference bytesAvailable = new IntByReference(0); + if(bytesAvailable.getValue() > 0) { + while(hmdPipe.pipe.read(ByteBuffer.wrap(buffArray)) != 0) { + int bytesRead = bytesAvailable.getValue(); + for(int i = 0; i < bytesRead; ++i) { + char c = (char) buffArray[i]; + if(c == '\n') { + executeHMDInput(); + commandBuilder.setLength(0); + } else { + commandBuilder.append(c); + if(commandBuilder.length() >= MAX_COMMAND_LENGTH) { + LogManager.log.severe("[VRBridge] Command from the pipe is too long, flushing buffer"); + commandBuilder.setLength(0); + } + } + } + if(bytesRead < buffArray.length) + break; // Don't repeat, we read all available bytes + } + return true; + } + } + return false; + } + + private void executeHMDInput() throws IOException { + String[] split = commandBuilder.toString().split(" "); + if(split.length < 7) { + LogManager.log.severe("[VRBridge] Short HMD data received: " + commandBuilder.toString()); + return; + } + try { + double x = Double.parseDouble(split[0]); + double y = Double.parseDouble(split[1]); + double z = Double.parseDouble(split[2]); + double qw = Double.parseDouble(split[3]); + double qx = Double.parseDouble(split[4]); + double qy = Double.parseDouble(split[5]); + double qz = Double.parseDouble(split[6]); + + internalHMDTracker.position.set((float) x, (float) y, (float) z); + internalHMDTracker.rotation.set((float) qx, (float) qy, (float) qz, (float) qw); + internalHMDTracker.dataTick(); + newHMDData.set(true); + } catch(NumberFormatException e) { + e.printStackTrace(); + } + } + + public void updateTracker(int trackerId, boolean hmdUpdated) { + Tracker sensor = internalTrackers.get(trackerId); + if(sensor.getStatus().sendData) { + LinuxPipe trackerPipe = trackerPipes.get(trackerId); + if(hmdUpdated && trackerPipe.state == PipeState.OPEN) { + sbBuffer.setLength(0); + sensor.getPosition(vBuffer); + sensor.getRotation(qBuffer); + sbBuffer.append(vBuffer.x).append(' ').append(vBuffer.y).append(' ').append(vBuffer.z).append(' '); + sbBuffer.append(qBuffer.getW()).append(' ').append(qBuffer.getX()).append(' ').append(qBuffer.getY()).append(' ').append(qBuffer.getZ()).append('\n'); + String str = sbBuffer.toString(); + System.arraycopy(str.getBytes(ASCII), 0, buffArray, 0, str.length()); + buffArray[str.length()] = '\0'; + IntByReference lpNumberOfBytesWritten = new IntByReference(0); + try { + trackerPipe.pipe.write(ByteBuffer.wrap(buffArray)); + } catch(IOException e) { + + } + } + } + } + + private void initHMDPipe(LinuxPipe pipe) { + hmd.setStatus(TrackerStatus.OK); + } + + private void initTrackerPipe(LinuxPipe pipe, int trackerId) { + String trackerHello = this.shareTrackers.size() + " 0"; + System.arraycopy(trackerHello.getBytes(ASCII), 0, buffArray, 0, trackerHello.length()); + buffArray[trackerHello.length()] = '\0'; + IntByReference lpNumberOfBytesWritten = new IntByReference(0); + try { + pipe.pipe.write(ByteBuffer.wrap(buffArray)); + } catch(IOException e) { + + } + } + + private boolean tryOpeningPipe(LinuxPipe pipe) { + if(pipe.pipe.isOpen()) { + pipe.state = PipeState.OPEN; + LogManager.log.info("[VRBridge] Pipe " + pipe.name + " is open"); + return true; + } + + LogManager.log.info("[VRBridge] Error connecting to pipe " + pipe.name + ": " + Native.getLastError()); + return false; + } + + private boolean areAllPipesOpen() { + if(hmdPipe == null || hmdPipe.state == PipeState.CREATED) { + return false; + } + for(int i = 0; i < trackerPipes.size(); ++i) { + if(trackerPipes.get(i).state == PipeState.CREATED) + return false; + } + return true; + } + + private void createPipes() throws IOException { + try { + RandomAccessFile rw = new RandomAccessFile(HMDPipeName, "rw"); + FileChannel fc = rw.getChannel(); + hmdPipe = new LinuxPipe(fc, HMDPipeName); // lpSecurityAttributes + LogManager.log.info("[VRBridge] Pipe " + hmdPipe.name + " created"); + //if(WinBase.INVALID_HANDLE_VALUE.equals(hmdPipe.pipeHandle)) + // throw new IOException("Can't open " + HMDPipeName + " pipe: " + Native.getLastError()); + for(int i = 0; i < this.shareTrackers.size(); ++i) { + String pipeName = TrackersPipeName + i; + RandomAccessFile Trw = new RandomAccessFile(pipeName, "rw"); + FileChannel Tfc = Trw.getChannel(); + LinuxPipe pipeHandle = new LinuxPipe(Tfc, pipeName+i); // lpSecurityAttributes + //if(WinBase.INVALID_HANDLE_VALUE.equals(pipeHandle)) + // throw new IOException("Can't open " + pipeName + " pipe: " + Native.getLastError()); + LogManager.log.info("[VRBridge] Pipe " + pipeName + " created"); + trackerPipes.add(new LinuxPipe(pipeHandle.pipe, pipeName)); + } + LogManager.log.info("[VRBridge] Pipes are open"); + } catch(IOException e) { + safeDisconnect(hmdPipe); + for(int i = 0; i < trackerPipes.size(); ++i) + safeDisconnect(trackerPipes.get(i)); + trackerPipes.clear(); + throw e; + } + } + + public static void safeDisconnect(LinuxPipe pipe) { + try { + if(pipe != null && pipe.pipe != null) { + pipe.pipe.close(); + } + } catch (Exception e) { + } + } + + @Override + public void addSharedTracker(ShareableTracker tracker) { + // TODO Auto-generated method stub + + } + + @Override + public void removeSharedTracker(ShareableTracker tracker) { + // TODO Auto-generated method stub + + } + + @Override + public void startBridge() { + start(); + } +} diff --git a/src/main/java/dev/slimevr/platform/linux/LinuxPipe.java b/src/main/java/dev/slimevr/platform/linux/LinuxPipe.java new file mode 100644 index 000000000..1485c27a5 --- /dev/null +++ b/src/main/java/dev/slimevr/platform/linux/LinuxPipe.java @@ -0,0 +1,26 @@ +package dev.slimevr.platform.linux; + +import dev.slimevr.bridge.PipeState; + +import java.nio.channels.FileChannel; + +public class LinuxPipe { + + public final String name; + + public FileChannel pipe; + public PipeState state = PipeState.CREATED; + + public LinuxPipe(FileChannel pipe, String name) { + this.pipe = pipe; + this.name = name; + } + public static void safeDisconnect(LinuxPipe pipe) { + try { + if(pipe != null && pipe.pipe != null) { + pipe.pipe.close(); + } + } catch (Exception e) { + } + } +} diff --git a/src/main/java/dev/slimevr/platform/linux/LinuxSteamVRPipeInputBridge.java b/src/main/java/dev/slimevr/platform/linux/LinuxSteamVRPipeInputBridge.java new file mode 100644 index 000000000..b2d6dc3cb --- /dev/null +++ b/src/main/java/dev/slimevr/platform/linux/LinuxSteamVRPipeInputBridge.java @@ -0,0 +1,291 @@ +package dev.slimevr.platform.linux; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; +import com.sun.jna.Native; +import com.sun.jna.platform.linux.Fcntl; +import com.sun.jna.platform.linux.ErrNo; +import com.sun.jna.ptr.IntByReference; +import dev.slimevr.VRServer; +import dev.slimevr.bridge.Bridge; +import dev.slimevr.bridge.PipeState; +import dev.slimevr.vr.trackers.ShareableTracker; +import dev.slimevr.vr.trackers.TrackerPosition; +import dev.slimevr.vr.trackers.TrackerStatus; +import dev.slimevr.vr.trackers.VRTracker; +import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; +import org.apache.commons.lang3.StringUtils; + +import java.io.IOException; +import java.io.RandomAccessFile; +import java.nio.ByteBuffer; +import java.nio.channels.FileChannel; +import java.util.HashMap; +import java.util.Iterator; +import java.util.List; +import java.util.Map; +import java.util.concurrent.atomic.AtomicBoolean; + +public class LinuxSteamVRPipeInputBridge extends Thread implements Bridge { + + private static final int MAX_COMMAND_LENGTH = 2048; + public static final String PipeName = "\\\\.\\pipe\\SlimeVRInput"; + + private final byte[] buffArray = new byte[1024]; + private final VRServer server; + private final StringBuilder commandBuilder = new StringBuilder(1024); + private final List trackers = new FastList<>(); + private final Map trackersInternal = new HashMap<>(); + private AtomicBoolean newData = new AtomicBoolean(false); + private final Vector3f vBuffer = new Vector3f(); + private final Quaternion qBuffer = new Quaternion(); + private LinuxPipe pipe; + + public LinuxSteamVRPipeInputBridge(VRServer server) { + this.server = server; + } + + @Override + public void run() { + try { + createPipes(); + while(true) { + boolean pipesUpdated = false; + if(pipe.state == PipeState.CREATED) { + tryOpeningPipe(pipe); + } + if(pipe.state == PipeState.OPEN) { + pipesUpdated = updatePipes(); + } + if(pipe.state == PipeState.ERROR) { + resetPipe(); + } + if(!pipesUpdated) { + try { + Thread.sleep(5); // Up to 200Hz + } catch(InterruptedException e) { + e.printStackTrace(); + } + } + } + } catch(Exception e) { + e.printStackTrace(); + } + } + + public boolean updatePipes() throws IOException { + if(pipe.state == PipeState.OPEN) { + IntByReference bytesAvailable = new IntByReference(0); + if(bytesAvailable.getValue() > 0) { + while(pipe.pipe.write(ByteBuffer.wrap(buffArray)) != 0) { + int bytesRead = bytesAvailable.getValue(); + for(int i = 0; i < bytesRead; ++i) { + char c = (char) buffArray[i]; + if(c == '\n') { + executeInputCommand(); + commandBuilder.setLength(0); + } else { + commandBuilder.append(c); + if(commandBuilder.length() >= MAX_COMMAND_LENGTH) { + LogManager.log.severe("[SteamVRPipeInputBridge] Command from the pipe is too long, flushing buffer"); + commandBuilder.setLength(0); + } + } + } + if(bytesRead < buffArray.length) + return true; // All pipe data read + } + } else { + return false; // Pipe was empty, it's okay + } + // PeekNamedPipe or ReadFile returned an error + pipe.state = PipeState.ERROR; + LogManager.log.severe("[SteamVRPipeInputBridge] Pipe error: " + Native.getLastError()); + } + return false; + } + + private void executeInputCommand() throws IOException { + String[] command = commandBuilder.toString().split(" "); + switch(command[0]) { + case "ADD": // Add new tracker + if(command.length < 4) { + LogManager.log.severe("[SteamVRPipeInputBridge] Error in ADD command. Command requires at least 4 arguments. Supplied: " + commandBuilder.toString()); + return; + } + VRTracker internalTracker = new VRTracker(Integer.parseInt(command[1]), StringUtils.join(command, " ", 3, command.length), true, true); + int roleId = Integer.parseInt(command[2]); + if(roleId >= 0 && roleId < LinuxSteamVRPipeInputBridge.SteamVRInputRoles.values.length) { + LinuxSteamVRPipeInputBridge.SteamVRInputRoles svrRole = LinuxSteamVRPipeInputBridge.SteamVRInputRoles.values[roleId]; + internalTracker.bodyPosition = svrRole.bodyPosition; + } + VRTracker oldTracker; + synchronized(trackersInternal) { + oldTracker = trackersInternal.put(internalTracker.getTrackerId(), internalTracker); + } + if(oldTracker != null) { + LogManager.log.severe("[SteamVRPipeInputBridge] New tracker added with the same id. Supplied: " + commandBuilder.toString()); + return; + } + newData.set(true); + break; + case "UPD": // Update tracker data + if(command.length < 9) { + LogManager.log.severe("[SteamVRPipeInputBridge] Error in UPD command. Command requires at least 9 arguments. Supplied: " + commandBuilder.toString()); + return; + } + int id = Integer.parseInt(command[1]); + double x = Double.parseDouble(command[2]); + double y = Double.parseDouble(command[3]); + double z = Double.parseDouble(command[4]); + double qw = Double.parseDouble(command[5]); + double qx = Double.parseDouble(command[6]); + double qy = Double.parseDouble(command[7]); + double qz = Double.parseDouble(command[8]); + internalTracker = trackersInternal.get(id); + if(internalTracker != null) { + internalTracker.position.set((float) x, (float) y, (float) z); + internalTracker.rotation.set((float) qx, (float) qy, (float) qz, (float) qw); + internalTracker.dataTick(); + newData.set(true); + } + break; + case "STA": // Update tracker status + if(command.length < 3) { + LogManager.log.severe("[SteamVRPipeInputBridge] Error in STA command. Command requires at least 3 arguments. Supplied: " + commandBuilder.toString()); + return; + } + id = Integer.parseInt(command[1]); + int status = Integer.parseInt(command[2]); + TrackerStatus st = TrackerStatus.getById(status); + if(st == null) { + LogManager.log.severe("[SteamVRPipeInputBridge] Unrecognized status id. Supplied: " + commandBuilder.toString()); + return; + } + internalTracker = trackersInternal.get(id); + if(internalTracker != null) { + internalTracker.setStatus(st); + newData.set(true); + } + break; + } + } + + @Override + public void dataRead() { + if(newData.getAndSet(false)) { + if(trackers.size() < trackersInternal.size()) { + // Add new trackers + synchronized(trackersInternal) { + Iterator iterator = trackersInternal.values().iterator(); + internal: while(iterator.hasNext()) { + VRTracker internalTracker = iterator.next(); + for(int i = 0; i < trackers.size(); ++i) { + VRTracker t = trackers.get(i); + if(t.getTrackerId() == internalTracker.getTrackerId()) + continue internal; + } + // Tracker is not found in current trackers + VRTracker tracker = new VRTracker(internalTracker.getTrackerId(), internalTracker.getName(), true, true); + tracker.bodyPosition = internalTracker.bodyPosition; + trackers.add(tracker); + server.registerTracker(tracker); + } + } + } + for(int i = 0; i < trackers.size(); ++i) { + VRTracker tracker = trackers.get(i); + VRTracker internal = trackersInternal.get(tracker.getTrackerId()); + if(internal == null) + throw new NullPointerException("Lost internal tracker somehow: " + tracker.getTrackerId()); // Shouldn't really happen even, but better to catch it like this + if(internal.getPosition(vBuffer)) + tracker.position.set(vBuffer); + if(internal.getRotation(qBuffer)) + tracker.rotation.set(qBuffer); + tracker.setStatus(internal.getStatus()); + tracker.dataTick(); + } + } + } + + @Override + public void dataWrite() { + // Not used, only input + } + + private void resetPipe() { + LinuxPipe.safeDisconnect(pipe); + pipe.state = PipeState.CREATED; + //Main.vrServer.queueTask(this::disconnected); + } + + + private boolean tryOpeningPipe(LinuxPipe pipe) { + if(pipe.pipe.isOpen()) { + pipe.state = PipeState.OPEN; + LogManager.log.info("[SteamVRPipeInputBridge] Pipe " + pipe.name + " is open"); + return true; + } + + LogManager.log.info("[SteamVRPipeInputBridge] Error connecting to pipe " + pipe.name + ": " + Native.getLastError()); + return false; + } + + private void createPipes() throws IOException { + try { + RandomAccessFile rw = new RandomAccessFile(PipeName, "rw"); + FileChannel fc = rw.getChannel(); + pipe = new LinuxPipe(fc, PipeName); // lpSecurityAttributes + LogManager.log.info("[SteamVRPipeInputBridge] Pipe " + pipe.name + " created"); + if(pipe.pipe == null) + throw new IOException("Can't open " + PipeName + " pipe: " + Native.getLastError()); + LogManager.log.info("[SteamVRPipeInputBridge] Pipes are open"); + } catch(IOException e) { + LinuxPipe.safeDisconnect(pipe); + throw e; + } + } + + @Override + public void addSharedTracker(ShareableTracker tracker) { + // TODO Auto-generated method stub + + } + + @Override + public void removeSharedTracker(ShareableTracker tracker) { + // TODO Auto-generated method stub + + } + + public enum SteamVRInputRoles { + HEAD(TrackerPosition.HMD), + LEFT_HAND(TrackerPosition.LEFT_CONTROLLER), + RIGHT_HAND(TrackerPosition.RIGHT_CONTROLLER), + LEFT_FOOT(TrackerPosition.LEFT_FOOT), + RIGHT_FOOT(TrackerPosition.RIGHT_FOOT), + LEFT_SHOULDER(TrackerPosition.NONE), + RIGHT_SHOULDER(TrackerPosition.NONE), + LEFT_ELBOW(TrackerPosition.NONE), + RIGHT_ELBOW(TrackerPosition.NONE), + LEFT_KNEE(TrackerPosition.LEFT_LEG), + RIGHT_KNEE(TrackerPosition.RIGHT_LEG), + WAIST(TrackerPosition.WAIST), + CHEST(TrackerPosition.CHEST), + ; + + private static final LinuxSteamVRPipeInputBridge.SteamVRInputRoles[] values = values(); + public final TrackerPosition bodyPosition; + + private SteamVRInputRoles(TrackerPosition slimeVrPosition) { + this.bodyPosition = slimeVrPosition; + } + } + + @Override + public void startBridge() { + start(); + } +} +