diff --git a/src/main/java/io/eiren/vr/VRServer.java b/src/main/java/io/eiren/vr/VRServer.java index 75654cc0c..957e455ae 100644 --- a/src/main/java/io/eiren/vr/VRServer.java +++ b/src/main/java/io/eiren/vr/VRServer.java @@ -4,6 +4,8 @@ import java.io.File; import java.io.FileInputStream; import java.io.FileOutputStream; import java.io.IOException; +import java.net.InetAddress; +import java.net.UnknownHostException; import java.util.HashMap; import java.util.Iterator; import java.util.List; @@ -17,6 +19,8 @@ import io.eiren.util.ann.ThreadSecure; import io.eiren.util.ann.VRServerThread; import io.eiren.util.collections.FastList; import io.eiren.vr.bridge.NamedPipeVRBridge; +import io.eiren.vr.bridge.VMCBridge; +import io.eiren.vr.bridge.VRBridge; import io.eiren.vr.processor.HumanPoseProcessor; import io.eiren.vr.processor.HumanSkeleton; import io.eiren.vr.trackers.HMDTracker; @@ -32,7 +36,7 @@ public class VRServer extends Thread { private final List trackers = new FastList<>(); public final HumanPoseProcessor humanPoseProcessor; private final TrackersUDPServer trackersServer = new TrackersUDPServer(6969, "Sensors UDP server", this::registerTracker); - private final NamedPipeVRBridge driverBridge; + private final List bridges = new FastList<>(); private final Queue tasks = new LinkedBlockingQueue<>(); private final Map configuration = new HashMap<>(); public final YamlFile config = new YamlFile(); @@ -46,7 +50,19 @@ public class VRServer extends Thread { hmdTracker.position.set(0, 1.8f, 0); // Set starting position for easier debugging humanPoseProcessor = new HumanPoseProcessor(this, hmdTracker); List shareTrackers = humanPoseProcessor.getComputedTrackers(); - driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers); + + // Create named pipe bridge for SteamVR driver + NamedPipeVRBridge driverBridge = new NamedPipeVRBridge(hmdTracker, shareTrackers); + tasks.add(() -> driverBridge.start()); + + // Create VMCBridge + try { + VMCBridge vmcBridge = new VMCBridge(39539, 39540, InetAddress.getLocalHost()); + tasks.add(() -> vmcBridge.start()); + } catch(UnknownHostException e) { + e.printStackTrace(); + } + registerTracker(hmdTracker); for(int i = 0; i < shareTrackers.size(); ++i) @@ -142,7 +158,6 @@ public class VRServer extends Thread { public void run() { loadConfig(); trackersServer.start(); - driverBridge.start(); while(true) { //final long start = System.currentTimeMillis(); do { @@ -155,9 +170,11 @@ public class VRServer extends Thread { for(int i = 0; i < onTick.size(); ++i) { this.onTick.get(i).run(); } - + for(int i = 0; i < bridges.size(); ++i) + bridges.get(i).dataRead(); humanPoseProcessor.update(); - + for(int i = 0; i < bridges.size(); ++i) + bridges.get(i).dataWrite(); //final long time = System.currentTimeMillis() - start; try { Thread.sleep(1); // 1000Hz diff --git a/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java b/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java index a2f551226..47256e1e7 100644 --- a/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java +++ b/src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java @@ -3,6 +3,7 @@ package io.eiren.vr.bridge; import java.io.IOException; import java.nio.charset.Charset; import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; @@ -13,6 +14,7 @@ import com.sun.jna.ptr.IntByReference; import io.eiren.util.collections.FastList; import io.eiren.util.logging.LogManager; +import io.eiren.vr.trackers.ComputedTracker; import io.eiren.vr.trackers.HMDTracker; import io.eiren.vr.trackers.Tracker; import io.eiren.vr.trackers.TrackerStatus; @@ -32,12 +34,21 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { private final HMDTracker hmd; private final List trackerPipes; private final List shareTrackers; + private final List internalTrackers; + + private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD"); + private final AtomicBoolean newHMDData = new AtomicBoolean(false); public NamedPipeVRBridge(HMDTracker hmd, List shareTrackers) { 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); + this.internalTrackers.add(new ComputedTracker("internal://" + t.getName())); + } } @Override @@ -60,6 +71,27 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { 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(vBuffer)) + it.position.set(vBuffer); + if(t.getRotation(qBuffer)) + it.rotation.set(qBuffer); + } + } private void waitForPipesToOpen() { if(hmdPipe.state == PipeState.CREATED) { @@ -92,9 +124,10 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { double qy = Double.parseDouble(split[5]); double qz = Double.parseDouble(split[6]); - hmd.position.set((float) x, (float) y, (float) z); - hmd.rotation.set((float) qx, (float) qy, (float) qz, (float) qw); - hmd.dataTick(); + 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(); } @@ -107,7 +140,7 @@ public class NamedPipeVRBridge extends Thread implements VRBridge { } public void updateTracker(int trackerId, boolean hmdUpdated) { - Tracker sensor = shareTrackers.get(trackerId); + Tracker sensor = internalTrackers.get(trackerId); if(sensor.getStatus().sendData) { Pipe trackerPipe = trackerPipes.get(trackerId); if(hmdUpdated && trackerPipe.state == PipeState.OPEN) { diff --git a/src/main/java/io/eiren/vr/bridge/VMCBridge.java b/src/main/java/io/eiren/vr/bridge/VMCBridge.java new file mode 100644 index 000000000..0e0cf6976 --- /dev/null +++ b/src/main/java/io/eiren/vr/bridge/VMCBridge.java @@ -0,0 +1,32 @@ +package io.eiren.vr.bridge; + +import java.net.InetAddress; + +public class VMCBridge extends Thread implements VRBridge { + + public final int readPort; + public final int writePort; + public final InetAddress writeAddr; + + public VMCBridge(int readPort, int writePort, InetAddress writeAddr) { + super("Virtual Motion Capture bridge"); + if(readPort == writePort) + throw new IllegalArgumentException("Read and write port shouldn't be the same!"); + this.readPort = readPort; + this.writePort = writePort; + this.writeAddr = writeAddr; + } + + @Override + public void dataRead() { + // TODO Auto-generated method stub + + } + + @Override + public void dataWrite() { + // TODO Auto-generated method stub + + } + +} diff --git a/src/main/java/io/eiren/vr/bridge/VRBridge.java b/src/main/java/io/eiren/vr/bridge/VRBridge.java index aadea5111..7c48a4eff 100644 --- a/src/main/java/io/eiren/vr/bridge/VRBridge.java +++ b/src/main/java/io/eiren/vr/bridge/VRBridge.java @@ -1,5 +1,8 @@ package io.eiren.vr.bridge; public interface VRBridge { + + public void dataRead(); + public void dataWrite(); } diff --git a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java index 236996654..509c2a75c 100644 --- a/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java +++ b/src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java @@ -147,19 +147,22 @@ public class HumanSkeleonWithWaist extends HumanSkeleton { } protected void updateLocalTransforms() { - hmdTracker.getPosition(vBuf); - hmdTracker.getRotation(qBuf); - hmdNode.localTransform.setTranslation(vBuf); - hmdNode.localTransform.setRotation(qBuf); - headNode.localTransform.setRotation(qBuf); + if(hmdTracker.getPosition(vBuf)) { + hmdNode.localTransform.setTranslation(vBuf); + } + if(hmdTracker.getRotation(qBuf)) { + hmdNode.localTransform.setRotation(qBuf); + headNode.localTransform.setRotation(qBuf); + } + + if(chestTracker.getRotation(qBuf)) + neckNode.localTransform.setRotation(qBuf); - chestTracker.getRotation(qBuf); - neckNode.localTransform.setRotation(qBuf); - - waistTracker.getRotation(qBuf); - trackerWaistNode.localTransform.setRotation(qBuf); - chestNode.localTransform.setRotation(qBuf); - waistNode.localTransform.setRotation(qBuf); + if(waistTracker.getRotation(qBuf)) { + trackerWaistNode.localTransform.setRotation(qBuf); + chestNode.localTransform.setRotation(qBuf); + waistNode.localTransform.setRotation(qBuf); + } } protected void updateComputedTrackers() { diff --git a/src/main/java/io/eiren/vr/trackers/IMUTracker.java b/src/main/java/io/eiren/vr/trackers/IMUTracker.java index a393629c7..389e885a3 100644 --- a/src/main/java/io/eiren/vr/trackers/IMUTracker.java +++ b/src/main/java/io/eiren/vr/trackers/IMUTracker.java @@ -76,7 +76,7 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, @Override public void startCalibration(Consumer calibrationDataConsumer) { - server.sendCalibrationCommand(this, calibrationDataConsumer); + //server.sendCalibrationCommand(this, calibrationDataConsumer); } @Override @@ -91,12 +91,12 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS, @Override public void requestCalibrationData(Consumer calibrationDataConsumer) { - server.requestCalibrationData(this, calibrationDataConsumer); + //server.requestCalibrationData(this, calibrationDataConsumer); } @Override public void uploadNewClibrationData() { - server.uploadNewCalibrationData(this, newCalibrationData); + //server.uploadNewCalibrationData(this, newCalibrationData); } @Override diff --git a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java index 6fea93c16..6cafce59f 100644 --- a/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java +++ b/src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java @@ -18,10 +18,8 @@ import com.jme3.math.FastMath; import com.jme3.math.Quaternion; import com.jme3.math.Vector3f; -import io.eiren.hardware.magentometer.Magneto; import io.eiren.util.Util; import io.eiren.util.collections.FastList; -import io.eiren.vr.trackers.IMUTracker.ConfigurationData; /** * Recieves trackers data by UDP using extended owoTrack protocol. @@ -43,7 +41,6 @@ public class TrackersUDPServer extends Thread { private final List trackers = new FastList<>(); private final Map trackersMap = new HashMap<>(); private final Map> calibrationDataRequests = new HashMap<>(); - private final Map> newCalibrationDataRequests = new HashMap<>(); private final Consumer trackersConsumer; private final int port; @@ -56,157 +53,6 @@ public class TrackersUDPServer extends Thread { this.trackersConsumer = trackersConsumer; } - public void sendCalibrationCommand(Tracker tracker, Consumer calibrationDataConsumer) { - TrackerConnection connection = null; - synchronized(trackers) { - for(int i = 0; i < trackers.size(); ++i) { - if(trackers.get(i).tracker == tracker) { - connection = trackers.get(i); - break; - } - } - } - if(connection == null) - return; - synchronized(connection) { - if(connection.isCalibrating) - return; - connection.tracker.setStatus(TrackerStatus.BUSY); - connection.isCalibrating = true; - connection.rawCalibrationData.clear(); - } - if(calibrationDataConsumer != null) - newCalibrationDataRequests.put(tracker, calibrationDataConsumer); - try { - socket.send(new DatagramPacket(CALIBRATION_BUFFER, CALIBRATION_BUFFER.length, connection.address)); - System.out.println("[TrackerServer] Calibrating sensor on " + connection.address); - } catch(IOException e) { - e.printStackTrace(); - } - } - - public void requestCalibrationData(Tracker tracker, Consumer consumer) { - TrackerConnection connection = null; - synchronized(trackers) { - for(int i = 0; i < trackers.size(); ++i) { - if(trackers.get(i).tracker == tracker) { - connection = trackers.get(i); - break; - } - } - } - if(connection == null) - return; - calibrationDataRequests.put(tracker, consumer); - try { - socket.send(new DatagramPacket(CALIBRATION_REQUEST_BUFFER, CALIBRATION_REQUEST_BUFFER.length, connection.address)); - System.out.println("[TrackerServer] Requesting config from " + connection.address); - } catch(IOException e) { - e.printStackTrace(); - } - } - - public void uploadNewCalibrationData(Tracker tracker, ConfigurationData data) { - TrackerConnection connection = null; - synchronized(trackers) { - for(int i = 0; i < trackers.size(); ++i) { - if(trackers.get(i).tracker == tracker) { - connection = trackers.get(i); - break; - } - } - } - if(connection == null) - return; - // TODO - try { - socket.send(new DatagramPacket(CALIBRATION_REQUEST_BUFFER, CALIBRATION_REQUEST_BUFFER.length, connection.address)); - System.out.println("[TrackerServer] Requesting config from " + connection.address); - } catch(IOException e) { - e.printStackTrace(); - } - } - - private void stopCalibration(TrackerConnection sensor) { - synchronized(sensor) { - if(!sensor.isCalibrating) - return; - if(sensor.gyroCalibrationData == null || sensor.rawCalibrationData.size() == 0) - return; // Calibration not started yet - sensor.tracker.setStatus(TrackerStatus.OK); - sensor.isCalibrating = false; - } - if(sensor.rawCalibrationData.size() > 50 && sensor.gyroCalibrationData != null) { - System.out.println("[TrackerServer] Gathered " + sensor.address + " calibrration data, processing..."); - } else { - System.out.println("[TrackerServer] Can't gather enough calibration data, aboring..."); - return; - } - double[] accelBasis = new double[3]; - double[] accelAInv = new double[3 * 3]; - double[] magBasis = new double[3]; - double[] magAInv = new double[3 * 3]; - double[] gyroOffset = sensor.gyroCalibrationData; - - double[] accelData = new double[sensor.rawCalibrationData.size() * 3]; - double[] magData = new double[sensor.rawCalibrationData.size() * 3]; - double magMinX = Double.MAX_VALUE; - double magMinY = Double.MAX_VALUE; - double magMinZ = Double.MAX_VALUE; - double magMaxX = Double.MIN_VALUE; - double magMaxY = Double.MIN_VALUE; - double magMaxZ = Double.MIN_VALUE; - for(int i = 0; i < sensor.rawCalibrationData.size(); ++i) { - double[] line = sensor.rawCalibrationData.get(i); - accelData[i * 3 + 0] = line[0]; - accelData[i * 3 + 1] = line[1]; - accelData[i * 3 + 2] = line[2]; - magData[i * 3 + 0] = line[3]; - magData[i * 3 + 1] = line[4]; - magData[i * 3 + 2] = line[5]; - magMinX = Math.min(magMinX, line[3]); - magMinY = Math.min(magMinY, line[4]); - magMinZ = Math.min(magMinZ, line[5]); - magMaxX = Math.max(magMaxX, line[3]); - magMaxY = Math.max(magMaxY, line[4]); - magMaxZ = Math.max(magMaxZ, line[5]); - } - - double accelHnorm = 10000; - double magentometerHnorm = 100; - - System.out.println("[TrackerServer] Accelerometer Hnorm: " + (accelHnorm = Magneto.INSTANCE.calculateHnorm(accelData, sensor.rawCalibrationData.size()))); - Magneto.INSTANCE.calculate(accelData, sensor.rawCalibrationData.size(), 2, accelHnorm, accelBasis, accelAInv); - System.out.println("[TrackerServer] Magentometer Hnorm: " + (magentometerHnorm = Magneto.INSTANCE.calculateHnorm(magData, sensor.rawCalibrationData.size()))); - Magneto.INSTANCE.calculate(magData, sensor.rawCalibrationData.size(), 2, magentometerHnorm, magBasis, magAInv); - - System.out.println("float A_B[3] ="); - System.out.println(String.format(" {%8.2f,%8.2f,%8.2f},", accelBasis[0], accelBasis[1], accelBasis[2])); - System.out.println("float A_Ainv[3][3] ="); - System.out.println(String.format(" {{%9.5f,%9.5f,%9.5f},", accelAInv[0], accelAInv[1], accelAInv[2])); - System.out.println(String.format(" {%9.5f,%9.5f,%9.5f},", accelAInv[3], accelAInv[4], accelAInv[5])); - System.out.println(String.format(" {%9.5f,%9.5f,%9.5f}},", accelAInv[6], accelAInv[7], accelAInv[8])); - System.out.println("float M_B[3] ="); - System.out.println(String.format(" {%8.2f,%8.2f,%8.2f},", magBasis[0], magBasis[1], magBasis[2])); - System.out.println("float M_Ainv[3][3] ="); - System.out.println(String.format(" {{%9.5f,%9.5f,%9.5f},", magAInv[0], magAInv[1], magAInv[2])); - System.out.println(String.format(" {%9.5f,%9.5f,%9.5f},", magAInv[3], magAInv[4], magAInv[5])); - System.out.println(String.format(" {%9.5f,%9.5f,%9.5f}},", magAInv[6], magAInv[7], magAInv[8])); - System.out.println("float G_off[3] ="); - System.out.println(String.format(" {%8.2f, %8.2f, %8.2f}};", gyroOffset[0], gyroOffset[1], gyroOffset[2])); - System.out.println(String.format("Min/Max {%8.2f, %8.2f, %8.2f} / {%8.2f, %8.2f, %8.2f}", magMinX, magMinY, magMinZ, magMaxX, magMaxY, magMaxZ)); - System.out.println(String.format("Mag agv {%8.2f, %8.2f, %8.2f},", (magMaxX + magMinX) / 2, (magMaxY + magMinY) / 2, (magMaxZ + magMinZ) / 2)); - - - IMUTracker.ConfigurationData data = new IMUTracker.ConfigurationData(accelBasis, accelAInv, magBasis, magAInv, gyroOffset); - sensor.tracker.newCalibrationData = data; - - Consumer consumer = newCalibrationDataRequests.remove(sensor.tracker); - if(consumer != null) { - consumer.accept(data.toTextMatrix()); - } - } - private void setUpNewSensor(DatagramPacket handshakePacket, ByteBuffer data) throws IOException { System.out.println("[TrackerServer] Handshake recieved from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort()); SocketAddress addr = handshakePacket.getSocketAddress(); @@ -297,7 +143,6 @@ public class TrackersUDPServer extends Thread { if(sensor == null) break; bb.getLong(); - stopCalibration(sensor); buf.set(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getFloat()); offset.mult(buf, buf); IMUTracker tracker; @@ -315,14 +160,12 @@ public class TrackersUDPServer extends Thread { if(sensor == null) break; bb.getLong(); - stopCalibration(sensor); sensor.tracker.gyroVector.set(bb.getFloat(), bb.getFloat(), bb.getFloat()); break; case 4: if(sensor == null) break; bb.getLong(); - stopCalibration(sensor); float x = bb.getFloat(); float z = bb.getFloat(); float y = bb.getFloat(); @@ -332,7 +175,6 @@ public class TrackersUDPServer extends Thread { if(sensor == null) break; bb.getLong(); - stopCalibration(sensor); x = bb.getFloat(); z = bb.getFloat(); y = bb.getFloat(); @@ -342,13 +184,13 @@ public class TrackersUDPServer extends Thread { if(sensor == null) break; bb.getLong(); - sensor.rawCalibrationData.add(new double[] {bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt()}); + //sensor.rawCalibrationData.add(new double[] {bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt(), bb.getInt()}); break; case 7: // PACKET_GYRO_CALIBRATION_DATA if(sensor == null) break; bb.getLong(); - sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()}; + //sensor.gyroCalibrationData = new double[] {bb.getFloat(), bb.getFloat(), bb.getFloat()}; break; case 8: // PACKET_CONFIG if(sensor == null) @@ -494,9 +336,6 @@ public class TrackersUDPServer extends Thread { IMUTracker tracker; IMUTracker secondTracker; SocketAddress address; - boolean isCalibrating; - private List rawCalibrationData = new FastList<>(); - private double[] gyroCalibrationData; public long lastPacket = System.currentTimeMillis(); public int lastPingPacketId = -1; public long lastPingPacketTime = 0;