github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-06 02:01:58 +02:00
Code Issues 159 Packages Projects Releases Wiki Activity

Added support for multiple data bridges, optimize data flow

Browse Source 
Added empty VMC bridge
This commit is contained in:
Eiren Rain
2021-06-14 01:11:52 +03:00
parent e22d3ce806
commit c5d2771ce9
7 changed files with 114 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/main/java/io/eiren/vr/VRServer.java
View File

@@ -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<Tracker> 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<VRBridge> bridges = new FastList<>();
private final Queue<Runnable> tasks = new LinkedBlockingQueue<>();
private final Map<String, TrackerConfig> 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<? extends Tracker> 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

41
src/main/java/io/eiren/vr/bridge/NamedPipeVRBridge.java
View File

@@ -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<Pipe> trackerPipes;
private final List<? extends Tracker> shareTrackers;
private final List<ComputedTracker> internalTrackers;
private final HMDTracker internalHMDTracker = new HMDTracker("itnernal://HMD");
private final AtomicBoolean newHMDData = new AtomicBoolean(false);
public NamedPipeVRBridge(HMDTracker hmd, List<? extends Tracker> 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) {

32
src/main/java/io/eiren/vr/bridge/VMCBridge.java Normal file
View File

@@ -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
}
}

3
src/main/java/io/eiren/vr/bridge/VRBridge.java
View File

@@ -1,5 +1,8 @@
package io.eiren.vr.bridge;
public interface VRBridge {
public void dataRead();
public void dataWrite();
}

27
src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java
View File

@@ -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() {

6
src/main/java/io/eiren/vr/trackers/IMUTracker.java
View File

@@ -76,7 +76,7 @@ public class IMUTracker implements Tracker, CalibratingTracker, TrackerWithTPS,
@Override
public void startCalibration(Consumer<String> 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<String> calibrationDataConsumer) {
server.requestCalibrationData(this, calibrationDataConsumer);
//server.requestCalibrationData(this, calibrationDataConsumer);
}
@Override
public void uploadNewClibrationData() {
server.uploadNewCalibrationData(this, newCalibrationData);
//server.uploadNewCalibrationData(this, newCalibrationData);
}
@Override

165
src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java
View File

@@ -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<TrackerConnection> trackers = new FastList<>();
private final Map<SocketAddress, TrackerConnection> trackersMap = new HashMap<>();
private final Map<Tracker, Consumer<String>> calibrationDataRequests = new HashMap<>();
private final Map<Tracker, Consumer<String>> newCalibrationDataRequests = new HashMap<>();
private final Consumer<IMUTracker> trackersConsumer;
private final int port;
@@ -56,157 +53,6 @@ public class TrackersUDPServer extends Thread {
this.trackersConsumer = trackersConsumer;
}
public void sendCalibrationCommand(Tracker tracker, Consumer<String> 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<String> 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<String> 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<double[]> rawCalibrationData = new FastList<>();
private double[] gyroCalibrationData;
public long lastPacket = System.currentTimeMillis();
public int lastPingPacketId = -1;
public long lastPingPacketTime = 0;