diff --git a/build.gradle b/build.gradle index 8be45726a..450846e17 100644 --- a/build.gradle +++ b/build.gradle @@ -52,6 +52,7 @@ dependencies { compile 'com.illposed.osc:javaosc-core:0.8' compile 'com.fazecast:jSerialComm:[2.0.0,3.0.0)' compile 'com.google.protobuf:protobuf-java:3.17.3' + compile "org.java-websocket:Java-WebSocket:1.5.1" // This dependency is used internally, and not exposed to consumers on their own compile classpath. implementation 'com.google.guava:guava:28.2-jre' diff --git a/src/main/java/io/eiren/gui/TrackersList.java b/src/main/java/io/eiren/gui/TrackersList.java index 368377402..a4798d2d1 100644 --- a/src/main/java/io/eiren/gui/TrackersList.java +++ b/src/main/java/io/eiren/gui/TrackersList.java @@ -214,13 +214,15 @@ public class TrackersList extends EJBoxNoStretch { if(t.hasPosition()) add(new JLabel("Position"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); add(new JLabel("TPS"), c(3, row, 2, GridBagConstraints.FIRST_LINE_START)); + if(realTracker instanceof IMUTracker) { + add(new JLabel("Ping"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); + } row++; if(t.hasRotation()) add(rotation = new JLabel("0 0 0"), c(0, row, 2, GridBagConstraints.FIRST_LINE_START)); if(t.hasPosition()) add(position = new JLabel("0 0 0"), c(1, row, 2, GridBagConstraints.FIRST_LINE_START)); if(realTracker instanceof IMUTracker) { - add(new JLabel("Ping"), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); add(ping = new JLabel(""), c(2, row, 2, GridBagConstraints.FIRST_LINE_START)); } if(realTracker instanceof TrackerWithTPS) { diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java index 956cef250..c3f9cebc3 100644 --- a/src/main/java/io/eiren/vr/VRServer.java +++ b/src/main/java/io/eiren/vr/VRServer.java @@ -23,6 +23,7 @@ import io.eiren.vr.bridge.NamedPipeVRBridge; import io.eiren.vr.bridge.SteamVRPipeInputBridge; import io.eiren.vr.bridge.VMCBridge; import io.eiren.vr.bridge.VRBridge; +import io.eiren.vr.bridge.WebSocketVRBridge; import io.eiren.vr.processor.HumanPoseProcessor; import io.eiren.vr.processor.HumanSkeleton; import io.eiren.vr.trackers.HMDTracker; @@ -63,6 +64,10 @@ public class VRServer extends Thread { SteamVRPipeInputBridge steamVRInput = new SteamVRPipeInputBridge(this); tasks.add(() -> steamVRInput.start()); bridges.add(steamVRInput); + // Create WebSocket server + WebSocketVRBridge wsBridge = new WebSocketVRBridge(hmdTracker, shareTrackers, this); + tasks.add(() -> wsBridge.start()); + bridges.add(wsBridge); // Create VMCBridge try { diff --git a/src/main/java/io/eiren/vr/bridge/WebSocketVRBridge.java b/src/main/java/io/eiren/vr/bridge/WebSocketVRBridge.java new file mode 100644 index 000000000..c4eb59537 --- /dev/null +++ b/src/main/java/io/eiren/vr/bridge/WebSocketVRBridge.java @@ -0,0 +1,176 @@ +package io.eiren.vr.bridge; + +import java.net.InetSocketAddress; +import java.nio.ByteBuffer; +import java.util.Collections; +import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; + +import org.java_websocket.WebSocket; +import org.java_websocket.drafts.Draft; +import org.java_websocket.drafts.Draft_6455; +import org.java_websocket.handshake.ClientHandshake; +import org.java_websocket.server.WebSocketServer; +import org.json.JSONException; +import org.json.JSONObject; + +import com.jme3.math.Quaternion; +import com.jme3.math.Vector3f; + +import io.eiren.util.collections.FastList; +import io.eiren.util.logging.LogManager; +import io.eiren.vr.Main; +import io.eiren.vr.VRServer; +import io.eiren.vr.trackers.ComputedTracker; +import io.eiren.vr.trackers.HMDTracker; +import io.eiren.vr.trackers.Tracker; +import io.eiren.vr.trackers.TrackerStatus; + +public class WebSocketVRBridge extends WebSocketServer implements VRBridge { + + private final Vector3f vBuffer = new Vector3f(); + private final Quaternion qBuffer = new Quaternion(); + + private final HMDTracker hmd; + private final List shareTrackers; + private final List internalTrackers; + + private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD"); + private final AtomicBoolean newHMDData = new AtomicBoolean(false); + + public WebSocketVRBridge(HMDTracker hmd, List shareTrackers, VRServer server) { + super(new InetSocketAddress(21110), Collections.singletonList(new Draft_6455())); + this.hmd = hmd; + this.shareTrackers = new FastList<>(shareTrackers); + this.internalTrackers = new FastList<>(shareTrackers.size()); + for(int i = 0; i < shareTrackers.size(); ++i) { + Tracker t = shareTrackers.get(i); + ComputedTracker ct = new ComputedTracker("internal://" + t.getName(), true, true); + ct.setStatus(TrackerStatus.OK); + ct.bodyPosition = t.getBodyPosition(); + this.internalTrackers.add(ct); + } + } + + @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(vBuffer)) + it.position.set(vBuffer); + if(t.getRotation(qBuffer)) + it.rotation.set(qBuffer); + } + } + + @Override + public void onOpen(WebSocket conn, ClientHandshake handshake) { + LogManager.log.info("[WebSocket] New connection from: " + conn.getRemoteSocketAddress().getAddress().getHostAddress()); + // Register trackers + for(int i = 0; i < internalTrackers.size(); ++i) { + JSONObject message = new JSONObject(); + message.put("type", "config"); + message.put("tracker_id", "SlimeVR Tracker " + (i + 1)); + message.put("location", internalTrackers.get(i).bodyPosition.designation); + message.put("tracker_type", message.optString("location")); + conn.send(message.toString()); + } + } + + @Override + public void onClose(WebSocket conn, int code, String reason, boolean remote) { + LogManager.log.info("[WebSocket] Disconnected: " + conn.getRemoteSocketAddress().getAddress().getHostAddress() + ", (" + code + ") " + reason + ". Remote: " + remote); + } + + @Override + public void onMessage(WebSocket conn, ByteBuffer message) { + StringBuilder sb = new StringBuilder(message.limit()); + while(message.hasRemaining()) { + sb.append((char) message.get()); + } + onMessage(conn, sb.toString()); + } + + @Override + public void onMessage(WebSocket conn, String message) { + //LogManager.log.info(message); + try { + JSONObject json = new JSONObject(message); + if(json.has("type")) { + switch(json.optString("type")) { + case "pos": + parsePosition(json, conn); + return; + case "action": + parseAction(json, conn); + return; + case "config": // TODO Ignore it for now, it should only register HMD in our test case with id 0 + LogManager.log.info("[WebSocket] Config recieved: " + json.toString()); + return; + } + } + LogManager.log.warning("[WebSocket] Unrecognized message from " + conn.getRemoteSocketAddress().getAddress().getHostAddress() + ": " + message); + } catch(Exception e) { + LogManager.log.severe("[WebSocket] Exception parsing message from " + conn.getRemoteSocketAddress().getAddress().getHostAddress() + ". Message: " + message, e); + } + } + + private void parsePosition(JSONObject json, WebSocket conn) throws JSONException { + if(json.optInt("tracker_id") == 0) { + // Read HMD information + internalHMDTracker.position.set(json.optFloat("x"), json.optFloat("y") + 0.2f, json.optFloat("z")); // TODO Wtf is this hack? VRWorkout issue? + internalHMDTracker.rotation.set(json.optFloat("qx"), json.optFloat("qy"), json.optFloat("qz"), json.optFloat("qw")); + internalHMDTracker.dataTick(); + newHMDData.set(true); + + // Send tracker info in reply + for(int i = 0; i < internalTrackers.size(); ++i) { + JSONObject message = new JSONObject(); + message.put("type", "pos"); + message.put("src", "full"); + message.put("tracker_id", "SlimeVR Tracker " + (i + 1)); + + ComputedTracker t = internalTrackers.get(i); + message.put("x", t.position.x); + message.put("y", t.position.y); + message.put("z", t.position.z); + message.put("qx", t.rotation.getX()); + message.put("qy", t.rotation.getY()); + message.put("qz", t.rotation.getZ()); + message.put("qw", t.rotation.getW()); + + conn.send(message.toString()); + } + } + } + + private void parseAction(JSONObject json, WebSocket conn) throws JSONException { + switch(json.optString("name")) { + case "calibrate": + Main.vrServer.resetTrackersYaw(); + break; + } + } + + @Override + public void onError(WebSocket conn, Exception ex) { + LogManager.log.severe("[WebSocket] Exception on connection " + (conn != null ? conn.getRemoteSocketAddress().getAddress().getHostAddress() : null), ex); + } + + @Override + public void onStart() { + LogManager.log.info("[WebSocket] Web Socket VR Bridge started on port " + getPort()); + setConnectionLostTimeout(0); + setConnectionLostTimeout(1); + } +} diff --git a/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTracker.java b/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTracker.java index 59e3c7ab4..d78ee7ee6 100644 --- a/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTracker.java +++ b/src/main/java/io/eiren/vr/processor/ComputedHumanPoseTracker.java @@ -9,9 +9,10 @@ public class ComputedHumanPoseTracker extends ComputedTracker implements Tracker public final ComputedHumanPoseTrackerPosition skeletonPosition; protected BufferedTimer timer = new BufferedTimer(1f); - public ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition skeletonPosition) { + public ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition skeletonPosition, TrackerBodyPosition bodyPosition) { super("human://" + skeletonPosition.name(), true, true); this.skeletonPosition = skeletonPosition; + this.bodyPosition = bodyPosition; } @Override diff --git a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java index e80381d14..a1923ec26 100644 --- a/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java +++ b/src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java @@ -20,16 +20,16 @@ public class HumanPoseProcessor { public HumanPoseProcessor(VRServer server, HMDTracker hmd, int trackersAmount) { this.server = server; - computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.WAIST)); + computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.WAIST, TrackerBodyPosition.WAIST)); if(trackersAmount > 2) { - computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT)); - computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT)); + computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerBodyPosition.LEFT_FOOT)); + computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerBodyPosition.RIGHT_FOOT)); if(trackersAmount == 4 || trackersAmount >= 6) { - computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.CHEST)); + computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.CHEST, TrackerBodyPosition.CHEST)); } if(trackersAmount >= 5) { - computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_KNEE)); - computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_KNEE)); + computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_KNEE, TrackerBodyPosition.LEFT_ANKLE)); + computedTrackers.add(new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_KNEE, TrackerBodyPosition.RIGHT_ANKLE)); } } } diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeletonWithLegs.java b/src/main/java/io/eiren/vr/processor/HumanSkeletonWithLegs.java index 058a8a557..6046c8b23 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeletonWithLegs.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeletonWithLegs.java @@ -91,9 +91,9 @@ public class HumanSkeletonWithLegs extends HumanSkeletonWithWaist { rkt = t; } if(lat == null) - lat = new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT); + lat = new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.LEFT_FOOT, TrackerBodyPosition.LEFT_FOOT); if(rat == null) - rat = new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT); + rat = new ComputedHumanPoseTracker(ComputedHumanPoseTrackerPosition.RIGHT_FOOT, TrackerBodyPosition.RIGHT_FOOT); computedLeftFootTracker = lat; computedRightFootTracker = rat; computedLeftKneeTracker = lkt; diff --git a/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java b/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java index 3780953ae..d814c1371 100644 --- a/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java +++ b/src/main/java/io/eiren/vr/processor/TrackerBodyPosition.java @@ -6,17 +6,17 @@ import java.util.Map; public enum TrackerBodyPosition { NONE(""), - HMD("body:HMD"), - CHEST("body:chest"), - WAIST("body:waist"), - LEFT_LEG("body:left_leg"), - RIGHT_LEG("body:right_leg"), - LEFT_ANKLE("body:left_ankle"), - RIGHT_ANKLE("body:right_ankle"), - LEFT_FOOT("body:left_foot"), - RIGHT_FOOT("body:right_foot"), - LEFT_CONTROLLER("body:left_controller"), - RIGHT_CONTROLLER("body:right_conroller"), + HMD("HMD"), + CHEST("chest"), + WAIST("waist"), + LEFT_LEG("left_leg"), + RIGHT_LEG("right_leg"), + LEFT_ANKLE("left_ankle"), + RIGHT_ANKLE("right_ankle"), + LEFT_FOOT("left_foot"), + RIGHT_FOOT("right_foot"), + LEFT_CONTROLLER("left_controller"), + RIGHT_CONTROLLER("right_conroller"), ; public final String designation;