mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 01:21:25 -08:00
protocol rework Sets -> Map
This commit is contained in:
1 parent
f961318035
commit
9fc5d41e1b
30 files changed
+3886
-3729
No files matched your search
@@ -35,6 +35,9 @@ import io.eiren.util.collections.FastList;
|
||||
import io.eiren.yaml.YamlException;
|
||||
import io.eiren.yaml.YamlFile;
|
||||
import io.eiren.yaml.YamlNode;
|
||||
import slimevr_protocol.datatypes.DeviceId;
|
||||
import slimevr_protocol.datatypes.TrackerId;
|
||||
import slimevr_protocol.datatypes.TrackerIdT;
|
||||
|
||||
public class VRServer extends Thread {
|
||||
|
||||
@@ -313,6 +316,22 @@ public class VRServer extends Thread {
|
||||
return new FastList<>(trackers);
|
||||
}
|
||||
|
||||
public Tracker getTrackerById(TrackerIdT id) {
|
||||
for (Tracker tracker : trackers) {
|
||||
if (tracker.getTrackerNum() != id.getTrackerNum())
|
||||
continue;
|
||||
if (tracker.getDevice() == null && id.getDeviceId() != null)
|
||||
continue;
|
||||
if (tracker.getDevice() != null && id.getDeviceId() == null)
|
||||
continue;
|
||||
if (tracker.getDevice().getId() != id.getDeviceId().getId())
|
||||
continue;
|
||||
return tracker;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
|
||||
public BVHRecorder getBvhRecorder() {
|
||||
return this.bvhRecorder;
|
||||
}
|
||||
|
||||
@@ -9,6 +9,7 @@ import dev.slimevr.vr.trackers.Tracker;
|
||||
import dev.slimevr.vr.trackers.TrackerConfig;
|
||||
import dev.slimevr.vr.trackers.TrackerPosition;
|
||||
import dev.slimevr.vr.trackers.TrackerStatus;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
import io.eiren.util.collections.FastList;
|
||||
|
||||
public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
|
||||
@@ -244,4 +245,14 @@ public class PoseFrameTracker implements Tracker, Iterable<TrackerFrame> {
|
||||
public int getTrackerId() {
|
||||
return this.trackerId;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getTrackerNum() {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Device getDevice() {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
@@ -7,6 +7,7 @@ import dev.slimevr.vr.trackers.Tracker;
|
||||
import dev.slimevr.vr.trackers.TrackerConfig;
|
||||
import dev.slimevr.vr.trackers.TrackerPosition;
|
||||
import dev.slimevr.vr.trackers.TrackerStatus;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
|
||||
public final class TrackerFrame implements Tracker {
|
||||
|
||||
@@ -176,4 +177,14 @@ public final class TrackerFrame implements Tracker {
|
||||
public int getTrackerId() {
|
||||
return this.trackerId;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getTrackerNum() {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Device getDevice() {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
@@ -4,6 +4,7 @@ import dev.slimevr.vr.trackers.ComputedTracker;
|
||||
import dev.slimevr.vr.trackers.ShareableTracker;
|
||||
import dev.slimevr.vr.trackers.TrackerRole;
|
||||
import dev.slimevr.vr.trackers.TrackerWithTPS;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
import io.eiren.util.BufferedTimer;
|
||||
|
||||
public class ComputedHumanPoseTracker extends ComputedTracker implements TrackerWithTPS, ShareableTracker {
|
||||
@@ -32,4 +33,9 @@ public class ComputedHumanPoseTracker extends ComputedTracker implements Tracker
|
||||
public TrackerRole getTrackerRole() {
|
||||
return trackerRole;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Device getDevice() {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
@@ -2,6 +2,7 @@ package dev.slimevr.vr.trackers;
|
||||
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
|
||||
public class ComputedTracker implements Tracker, TrackerWithTPS {
|
||||
|
||||
@@ -131,4 +132,15 @@ public class ComputedTracker implements Tracker, TrackerWithTPS {
|
||||
public int getTrackerId() {
|
||||
return this.trackerId;
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public int getTrackerNum() {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Device getDevice() {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
@@ -5,6 +5,7 @@ import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
|
||||
import dev.slimevr.VRServer;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
|
||||
import io.eiren.util.BufferedTimer;
|
||||
|
||||
@@ -44,10 +45,14 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
|
||||
public int ping = -1;
|
||||
public int signalStrength = -1;
|
||||
public float temperature = 0;
|
||||
public final Device device;
|
||||
public final int trackerNum;
|
||||
|
||||
public TrackerPosition bodyPosition = null;
|
||||
|
||||
public IMUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server, VRServer vrserver) {
|
||||
public IMUTracker(Device device, int trackerId, int trackerNum, String name, String descriptiveName, TrackersUDPServer server, VRServer vrserver) {
|
||||
this.device = device;
|
||||
this.trackerNum = trackerNum;
|
||||
this.name = name;
|
||||
this.server = server;
|
||||
this.trackerId = trackerId;
|
||||
@@ -281,7 +286,17 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
|
||||
public int getTrackerId() {
|
||||
return this.trackerId;
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public int getTrackerNum() {
|
||||
return this.trackerNum;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Device getDevice() {
|
||||
return this.device;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getDescriptiveName() {
|
||||
return this.descriptiveName;
|
||||
|
||||
@@ -3,14 +3,15 @@ package dev.slimevr.vr.trackers;
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
import dev.slimevr.VRServer;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
|
||||
|
||||
public class MPUTracker extends IMUTracker {
|
||||
|
||||
public ConfigurationData newCalibrationData;
|
||||
|
||||
public MPUTracker(int trackerId, String name, String descriptiveName, TrackersUDPServer server, VRServer vrserver) {
|
||||
super(trackerId, name, descriptiveName, server, vrserver);
|
||||
public MPUTracker(Device device, int trackerId, int trackerNum, String name, String descriptiveName, TrackersUDPServer server, VRServer vrserver) {
|
||||
super(device, trackerId, trackerNum, name, descriptiveName, server, vrserver);
|
||||
}
|
||||
|
||||
public static class ConfigurationData {
|
||||
|
||||
@@ -2,6 +2,7 @@ package dev.slimevr.vr.trackers;
|
||||
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
|
||||
public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
|
||||
|
||||
@@ -164,9 +165,19 @@ public class ReferenceAdjustedTracker<E extends Tracker> implements Tracker {
|
||||
public int getTrackerId() {
|
||||
return tracker.getTrackerId();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public int getTrackerNum() {
|
||||
return tracker.getTrackerNum();
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getDescriptiveName() {
|
||||
return tracker.getDescriptiveName();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Device getDevice() {
|
||||
return tracker.getDevice();
|
||||
}
|
||||
}
|
||||
@@ -4,6 +4,7 @@ import java.util.concurrent.atomic.AtomicInteger;
|
||||
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
|
||||
public interface Tracker {
|
||||
|
||||
@@ -42,6 +43,10 @@ public interface Tracker {
|
||||
public boolean isComputed();
|
||||
|
||||
public int getTrackerId();
|
||||
|
||||
public int getTrackerNum();
|
||||
|
||||
public Device getDevice();
|
||||
|
||||
public default String getDescriptiveName() {
|
||||
return getName();
|
||||
|
||||
+13
-4
@@ -4,12 +4,15 @@ import java.net.InetAddress;
|
||||
import java.net.SocketAddress;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
import java.util.concurrent.atomic.AtomicInteger;
|
||||
|
||||
import dev.slimevr.NetworkProtocol;
|
||||
import dev.slimevr.vr.trackers.IMUTracker;
|
||||
|
||||
public class TrackerUDPConnection {
|
||||
|
||||
public class Device {
|
||||
|
||||
public static final AtomicInteger nextLocalDeviceId = new AtomicInteger();
|
||||
|
||||
public Map<Integer, IMUTracker> sensors = new HashMap<>();
|
||||
public SocketAddress address;
|
||||
public InetAddress ipAddress;
|
||||
@@ -24,10 +27,12 @@ public class TrackerUDPConnection {
|
||||
public NetworkProtocol protocol = null;
|
||||
public int firmwareBuild = 0;
|
||||
public boolean timedOut = false;
|
||||
public final int id;
|
||||
|
||||
public TrackerUDPConnection(SocketAddress address, InetAddress ipAddress) {
|
||||
public Device(SocketAddress address, InetAddress ipAddress) {
|
||||
this.address = address;
|
||||
this.ipAddress = ipAddress;
|
||||
this.id = Device.nextLocalDeviceId.incrementAndGet();
|
||||
}
|
||||
|
||||
public boolean isNextPacket(long packetId) {
|
||||
@@ -41,4 +46,8 @@ public class TrackerUDPConnection {
|
||||
public String toString() {
|
||||
return "udp:/" + ipAddress;
|
||||
}
|
||||
}
|
||||
|
||||
public int getId() {
|
||||
return id;
|
||||
}
|
||||
}
|
||||
@@ -49,9 +49,9 @@ public class TrackersUDPServer extends Thread {
|
||||
|
||||
private final Quaternion buf = new Quaternion();
|
||||
private final Random random = new Random();
|
||||
private final List<TrackerUDPConnection> connections = new FastList<>();
|
||||
private final Map<InetAddress, TrackerUDPConnection> connectionsByAddress = new HashMap<>();
|
||||
private final Map<String, TrackerUDPConnection> connectionsByMAC = new HashMap<>();
|
||||
private final List<Device> connections = new FastList<>();
|
||||
private final Map<InetAddress, Device> connectionsByAddress = new HashMap<>();
|
||||
private final Map<String, Device> connectionsByMAC = new HashMap<>();
|
||||
private final Consumer<Tracker> trackersConsumer;
|
||||
private final int port;
|
||||
private final ArrayList<SocketAddress> broadcastAddresses = new ArrayList<>();
|
||||
@@ -93,12 +93,12 @@ public class TrackersUDPServer extends Thread {
|
||||
private void setUpNewConnection(DatagramPacket handshakePacket, UDPPacket3Handshake handshake) throws IOException {
|
||||
LogManager.log.info("[TrackerServer] Handshake received from " + handshakePacket.getAddress() + ":" + handshakePacket.getPort());
|
||||
InetAddress addr = handshakePacket.getAddress();
|
||||
TrackerUDPConnection connection;
|
||||
Device connection;
|
||||
synchronized(connections) {
|
||||
connection = connectionsByAddress.get(addr);
|
||||
}
|
||||
if(connection == null) {
|
||||
connection = new TrackerUDPConnection(handshakePacket.getSocketAddress(), addr);
|
||||
connection = new Device(handshakePacket.getSocketAddress(), addr);
|
||||
connection.firmwareBuild = handshake.firmwareBuild;
|
||||
if(handshake.firmware == null || handshake.firmware.length() == 0) {
|
||||
// Only old owoTrack doesn't report firmware and have different packet IDs with SlimeVR
|
||||
@@ -111,7 +111,7 @@ public class TrackersUDPServer extends Thread {
|
||||
int i = 0;
|
||||
synchronized(connections) {
|
||||
if(handshake.macString != null && connectionsByMAC.containsKey(handshake.macString)) {
|
||||
TrackerUDPConnection previousConnection = connectionsByMAC.get(handshake.macString);
|
||||
Device previousConnection = connectionsByMAC.get(handshake.macString);
|
||||
i = connections.indexOf(previousConnection);
|
||||
connectionsByAddress.remove(previousConnection.ipAddress);
|
||||
previousConnection.lastPacketNumber = 0;
|
||||
@@ -143,11 +143,11 @@ public class TrackersUDPServer extends Thread {
|
||||
socket.send(new DatagramPacket(rcvBuffer, bb.position(), connection.address));
|
||||
}
|
||||
|
||||
private void setUpSensor(TrackerUDPConnection connection, int trackerId, int sensorType, int sensorStatus) throws IOException {
|
||||
private void setUpSensor(Device connection, int trackerId, int sensorType, int sensorStatus) throws IOException {
|
||||
LogManager.log.info("[TrackerServer] Sensor " + trackerId + " for " + connection.name + " status: " + sensorStatus);
|
||||
IMUTracker imu = connection.sensors.get(trackerId);
|
||||
if(imu == null) {
|
||||
imu = new IMUTracker(Tracker.getNextLocalTrackerId(), connection.name + "/" + trackerId, connection.descriptiveName + "/" + trackerId, this, Main.vrServer);
|
||||
imu = new IMUTracker(connection, Tracker.getNextLocalTrackerId(), trackerId, connection.name + "/" + trackerId, connection.descriptiveName + "/" + trackerId, this, Main.vrServer);
|
||||
connection.sensors.put(trackerId, imu);
|
||||
ReferenceAdjustedTracker<IMUTracker> adjustedTracker = new ReferenceAdjustedTracker<>(imu);
|
||||
trackersConsumer.accept(adjustedTracker);
|
||||
@@ -170,7 +170,7 @@ public class TrackersUDPServer extends Thread {
|
||||
DatagramPacket received = null;
|
||||
try {
|
||||
boolean hasActiveTrackers = false;
|
||||
for(TrackerUDPConnection tracker : connections) {
|
||||
for(Device tracker : connections) {
|
||||
if(tracker.sensors.size() > 0) {
|
||||
hasActiveTrackers = true;
|
||||
break;
|
||||
@@ -194,7 +194,7 @@ public class TrackersUDPServer extends Thread {
|
||||
bb.limit(received.getLength());
|
||||
bb.rewind();
|
||||
|
||||
TrackerUDPConnection connection;
|
||||
Device connection;
|
||||
|
||||
synchronized(connections) {
|
||||
connection = connectionsByAddress.get(received.getAddress());
|
||||
@@ -211,7 +211,7 @@ public class TrackersUDPServer extends Thread {
|
||||
lastKeepup = System.currentTimeMillis();
|
||||
synchronized(connections) {
|
||||
for(int i = 0; i < connections.size(); ++i) {
|
||||
TrackerUDPConnection conn = connections.get(i);
|
||||
Device conn = connections.get(i);
|
||||
bb.limit(bb.capacity());
|
||||
bb.rewind();
|
||||
parser.write(bb, conn, new UDPPacket1Heartbeat());
|
||||
@@ -265,7 +265,7 @@ public class TrackersUDPServer extends Thread {
|
||||
}
|
||||
}
|
||||
|
||||
protected void processPacket(DatagramPacket received, UDPPacket packet, TrackerUDPConnection connection) throws IOException {
|
||||
protected void processPacket(DatagramPacket received, UDPPacket packet, Device connection) throws IOException {
|
||||
IMUTracker tracker = null;
|
||||
switch(packet.getPacketId()) {
|
||||
case UDPProtocolParser.PACKET_HEARTBEAT:
|
||||
@@ -433,7 +433,7 @@ public class TrackersUDPServer extends Thread {
|
||||
return sb.toString();
|
||||
}
|
||||
|
||||
public List<TrackerUDPConnection> getConnections() {
|
||||
public List<Device> getConnections() {
|
||||
return connections;
|
||||
}
|
||||
}
|
||||
@@ -4,8 +4,6 @@ import java.io.IOException;
|
||||
import java.io.UnsupportedEncodingException;
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
import io.eiren.util.logging.LogManager;
|
||||
|
||||
public class UDPProtocolParser {
|
||||
|
||||
public static final int PACKET_HEARTBEAT = 0;
|
||||
@@ -37,7 +35,7 @@ public class UDPProtocolParser {
|
||||
public UDPProtocolParser() {
|
||||
}
|
||||
|
||||
public UDPPacket parse(ByteBuffer buf, TrackerUDPConnection connection) throws IOException {
|
||||
public UDPPacket parse(ByteBuffer buf, Device connection) throws IOException {
|
||||
int packetId = buf.getInt();
|
||||
long packetNumber = buf.getLong();
|
||||
if(connection != null) {
|
||||
@@ -56,17 +54,17 @@ public class UDPProtocolParser {
|
||||
return newPacket;
|
||||
}
|
||||
|
||||
public void write(ByteBuffer buf, TrackerUDPConnection connection, UDPPacket packet) throws IOException {
|
||||
public void write(ByteBuffer buf, Device connection, UDPPacket packet) throws IOException {
|
||||
buf.putInt(packet.getPacketId());
|
||||
buf.putLong(0); // Packet number is always 0 when sending data to trackers
|
||||
packet.writeData(buf);
|
||||
}
|
||||
|
||||
public void writeHandshakeResponse(ByteBuffer buf, TrackerUDPConnection connection) throws IOException {
|
||||
public void writeHandshakeResponse(ByteBuffer buf, Device connection) throws IOException {
|
||||
buf.put(HANDSHAKE_BUFFER);
|
||||
}
|
||||
|
||||
public void writeSensorInfoResponse(ByteBuffer buf, TrackerUDPConnection connection, UDPPacket15SensorInfo packet) throws IOException {
|
||||
public void writeSensorInfoResponse(ByteBuffer buf, Device connection, UDPPacket15SensorInfo packet) throws IOException {
|
||||
buf.putInt(packet.getPacketId());
|
||||
buf.put((byte) packet.sensorId);
|
||||
buf.put((byte) packet.sensorStatus);
|
||||
|
||||
@@ -4,15 +4,13 @@ import com.google.flatbuffers.FlatBufferBuilder;
|
||||
import com.jme3.math.Quaternion;
|
||||
import com.jme3.math.Vector3f;
|
||||
import dev.slimevr.vr.trackers.IMUTracker;
|
||||
import dev.slimevr.vr.trackers.udp.TrackerUDPConnection;
|
||||
import slimevr_protocol.data_feed.DataFeedConfigT;
|
||||
import dev.slimevr.vr.trackers.Tracker;
|
||||
import dev.slimevr.vr.trackers.udp.Device;
|
||||
import slimevr_protocol.data_feed.DataFeedUpdate;
|
||||
import slimevr_protocol.data_feed.device_data.DeviceData;
|
||||
import slimevr_protocol.data_feed.device_data.DeviceDataMaskT;
|
||||
import slimevr_protocol.data_feed.tracker.*;
|
||||
import slimevr_protocol.datatypes.DeviceId;
|
||||
import slimevr_protocol.datatypes.Temperature;
|
||||
import slimevr_protocol.datatypes.TrackerId;
|
||||
import slimevr_protocol.datatypes.*;
|
||||
import slimevr_protocol.datatypes.hardware_info.HardwareInfo;
|
||||
import slimevr_protocol.datatypes.hardware_info.HardwareStatus;
|
||||
import slimevr_protocol.datatypes.math.Quat;
|
||||
@@ -23,7 +21,7 @@ import java.util.List;
|
||||
|
||||
public class DataFeedBuilder {
|
||||
|
||||
public static int createHardwareInfo(FlatBufferBuilder fbb, TrackerUDPConnection device) {
|
||||
public static int createHardwareInfo(FlatBufferBuilder fbb, Device device) {
|
||||
IMUTracker tracker = device.sensors.get(0);
|
||||
|
||||
HardwareInfo.startHardwareInfo(fbb);
|
||||
@@ -37,89 +35,115 @@ public class DataFeedBuilder {
|
||||
return HardwareInfo.endHardwareInfo(fbb);
|
||||
}
|
||||
|
||||
public static int createTrackerId(FlatBufferBuilder fbb, int trackerNum, int deviceId) {
|
||||
public static int createTrackerId(FlatBufferBuilder fbb, Tracker tracker) {
|
||||
TrackerId.startTrackerId(fbb);
|
||||
|
||||
TrackerId.addTrackerNum(fbb, trackerNum);
|
||||
TrackerId.addDeviceId(fbb, DeviceId.createDeviceId(fbb, deviceId));
|
||||
TrackerId.addTrackerNum(fbb, tracker.getTrackerNum());
|
||||
if (tracker.getDevice() != null)
|
||||
TrackerId.addDeviceId(fbb, DeviceId.createDeviceId(fbb, tracker.getDevice().getId()));
|
||||
|
||||
return TrackerId.endTrackerId(fbb);
|
||||
}
|
||||
|
||||
public static int createTrackerDataComponents(FlatBufferBuilder fbb, TrackerDataMaskT mask, IMUTracker tracker) {
|
||||
public static int createTrackerInfos(FlatBufferBuilder fbb, boolean infoMask, Tracker tracker) {
|
||||
|
||||
List<Integer> dataOffsets = new ArrayList<>(TrackerDataComponent.names.length);
|
||||
if (!infoMask) return -1;
|
||||
|
||||
for (int index = 0; index < TrackerDataComponent.names.length; index++) {
|
||||
if (index == TrackerDataComponent.info && mask.getInfo()) {
|
||||
// Vector3f pos = new Vector3f();
|
||||
// tracker.getPosition(pos);
|
||||
// dataOffsets[index] = TrackerInfo.;
|
||||
}
|
||||
TrackerInfo.startTrackerInfo(fbb);
|
||||
if (tracker.getBodyPosition() != null)
|
||||
TrackerInfo.addBodyPart(fbb, tracker.getBodyPosition().id);
|
||||
TrackerInfo.addEditable(fbb, tracker.userEditable());
|
||||
TrackerInfo.addComputed(fbb, tracker.isComputed());
|
||||
//TODO need support: TrackerInfo.addImuType(fbb, tracker.im);
|
||||
//TODO need support: TrackerInfo.addPollRate(fbb, tracker.);
|
||||
|
||||
if (index == TrackerDataComponent.position && mask.getPosition()) {
|
||||
Vector3f pos = new Vector3f();
|
||||
tracker.getPosition(pos);
|
||||
dataOffsets.add(
|
||||
TrackerDataComponentW.createTrackerDataComponentW(
|
||||
fbb,
|
||||
TrackerDataComponent.position,
|
||||
Vec3f.createVec3f(fbb, pos.x, pos.y, pos.z)
|
||||
if (tracker instanceof IMUTracker) {
|
||||
IMUTracker imuTracker = (IMUTracker) tracker;
|
||||
if (imuTracker.getMountingRotation() != null) {
|
||||
Quaternion quaternion = imuTracker.getMountingRotation().quaternion;
|
||||
TrackerInfo.addMountingOrientation(fbb,
|
||||
Quat.createQuat(fbb,
|
||||
quaternion.getX(),
|
||||
quaternion.getY(),
|
||||
quaternion.getZ(),
|
||||
quaternion.getW()
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
if (index == TrackerDataComponent.rotation && mask.getRotation()) {
|
||||
Quaternion quaternion = new Quaternion();
|
||||
tracker.getRotation(quaternion);
|
||||
dataOffsets.add(
|
||||
TrackerDataComponentW.createTrackerDataComponentW(
|
||||
fbb,
|
||||
TrackerDataComponent.rotation,
|
||||
Quat.createQuat(fbb,
|
||||
quaternion.getX(),
|
||||
quaternion.getY(),
|
||||
quaternion.getZ(),
|
||||
quaternion.getW()
|
||||
)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
if (index == TrackerDataComponent.temp && mask.getTemp()) {
|
||||
dataOffsets.add(TrackerDataComponentW.createTrackerDataComponentW(fbb, TrackerDataComponent.temp, Temperature.createTemperature(fbb, tracker.temperature)));
|
||||
}
|
||||
}
|
||||
|
||||
TrackerData.startDataVector(fbb, dataOffsets.size());
|
||||
dataOffsets.forEach((offset) -> {
|
||||
TrackerData.addData(fbb, offset);
|
||||
});
|
||||
return fbb.endVector();
|
||||
return TrackerInfo.endTrackerInfo(fbb);
|
||||
}
|
||||
|
||||
public static int createTrackersData(FlatBufferBuilder fbb, DeviceDataMaskT mask, TrackerUDPConnection device, int deviceId) {
|
||||
public static int createTrackerPosition(FlatBufferBuilder fbb, Tracker tracker) {
|
||||
Vector3f pos = new Vector3f();
|
||||
tracker.getPosition(pos);
|
||||
|
||||
return Vec3f.createVec3f(fbb, pos.x, pos.y, pos.z);
|
||||
}
|
||||
|
||||
public static int createTrackerRotation(FlatBufferBuilder fbb, Tracker tracker) {
|
||||
Quaternion quaternion = new Quaternion();
|
||||
tracker.getRotation(quaternion);
|
||||
|
||||
return Quat.createQuat(fbb,
|
||||
quaternion.getX(),
|
||||
quaternion.getY(),
|
||||
quaternion.getZ(),
|
||||
quaternion.getW()
|
||||
);
|
||||
}
|
||||
|
||||
public static int createTrackerTemperature(FlatBufferBuilder fbb, Tracker tracker) {
|
||||
if (!(tracker instanceof IMUTracker))
|
||||
return -1;
|
||||
IMUTracker imuTracker = (IMUTracker)tracker;
|
||||
return Temperature.createTemperature(fbb, imuTracker.temperature);
|
||||
}
|
||||
|
||||
public static int createTrackerData(FlatBufferBuilder fbb, TrackerDataMaskT mask, Tracker tracker) {
|
||||
int trackerInfosOffset = DataFeedBuilder.createTrackerInfos(fbb, mask.getInfo(), tracker);
|
||||
int trackerIdOffset = DataFeedBuilder.createTrackerId(fbb, tracker);
|
||||
|
||||
TrackerData.startTrackerData(fbb);
|
||||
|
||||
TrackerData.addTrackerId(fbb, trackerIdOffset);
|
||||
|
||||
if (trackerInfosOffset != -1)
|
||||
TrackerData.addInfo(fbb, trackerInfosOffset);
|
||||
if (mask.getStatus())
|
||||
TrackerData.addStatus(fbb, tracker.getStatus().id + 1);
|
||||
if (mask.getPosition())
|
||||
TrackerData.addRotation(fbb, DataFeedBuilder.createTrackerPosition(fbb, tracker));
|
||||
if (mask.getRotation())
|
||||
TrackerData.addRotation(fbb, DataFeedBuilder.createTrackerRotation(fbb, tracker));
|
||||
if (mask.getTemp()) {
|
||||
int trackerTemperatureOffset = DataFeedBuilder.createTrackerTemperature(fbb, tracker);
|
||||
if (trackerTemperatureOffset != -1)
|
||||
TrackerData.addTemp(fbb, trackerTemperatureOffset);
|
||||
}
|
||||
|
||||
return TrackerData.endTrackerData(fbb);
|
||||
}
|
||||
|
||||
public static int createTrackersData(FlatBufferBuilder fbb, DeviceDataMaskT mask, Device device) {
|
||||
if (mask.getTrackerData() == null) return -1;
|
||||
|
||||
int[] sensorsOffsets = new int[device.sensors.size()];
|
||||
int[] trackersOffset = new int[device.sensors.size()];
|
||||
|
||||
device.sensors.forEach((key, value) -> {
|
||||
int trackerIdOffset = DataFeedBuilder.createTrackerId(fbb, key, deviceId);
|
||||
int dataOffset = DataFeedBuilder.createTrackerDataComponents(fbb, mask.getTrackerData(), value);
|
||||
sensorsOffsets[key] = TrackerData.createTrackerData(fbb, trackerIdOffset, dataOffset);
|
||||
trackersOffset[key] = DataFeedBuilder.createTrackerData(fbb, mask.getTrackerData(), value);
|
||||
});
|
||||
|
||||
return TrackerData.createDataVector(fbb, sensorsOffsets);
|
||||
return DeviceData.createTrackersVector(fbb, trackersOffset);
|
||||
}
|
||||
|
||||
public static int createDeviceData(FlatBufferBuilder fbb, int id, DeviceDataMaskT mask, TrackerUDPConnection device) {
|
||||
public static int createDeviceData(FlatBufferBuilder fbb, int id, DeviceDataMaskT mask, Device device) {
|
||||
if (!mask.getDeviceData()) return -1;
|
||||
|
||||
IMUTracker tracker = device.sensors.get(0);
|
||||
|
||||
if (tracker == null) return -1;
|
||||
|
||||
int nameOffset = fbb.createString(device.name);
|
||||
int hardwareDataOffset = HardwareStatus.createHardwareStatus(
|
||||
fbb,
|
||||
tracker.getStatus().id,
|
||||
@@ -132,10 +156,10 @@ public class DataFeedBuilder {
|
||||
-1
|
||||
);
|
||||
int hardwareInfoOffset = DataFeedBuilder.createHardwareInfo(fbb, device);
|
||||
int trackersOffset = DataFeedBuilder.createTrackersData(fbb, mask, device, id);
|
||||
int trackersOffset = DataFeedBuilder.createTrackersData(fbb, mask, device);
|
||||
|
||||
DeviceData.startDeviceData(fbb);
|
||||
DeviceData.addCustomName(fbb, nameOffset);
|
||||
//TODO need support: DeviceData.addCustomName(fbb, nameOffset);
|
||||
DeviceData.addId(fbb, DeviceId.createDeviceId(fbb, id));
|
||||
DeviceData.addHardwareStatus(fbb, hardwareDataOffset);
|
||||
DeviceData.addHardwareInfo(fbb, hardwareInfoOffset);
|
||||
@@ -144,13 +168,28 @@ public class DataFeedBuilder {
|
||||
return DeviceData.endDeviceData(fbb);
|
||||
}
|
||||
|
||||
public static int createDevicesData(FlatBufferBuilder fbb, DataFeedConfigT config, List<TrackerUDPConnection> devices) {
|
||||
if (config.getDataMask() == null) return -1;
|
||||
public static int createSyntheticTrackersData(FlatBufferBuilder fbb, TrackerDataMaskT trackerDataMaskT, List<Tracker> trackers) {
|
||||
if (trackerDataMaskT == null) return -1;
|
||||
|
||||
List<Integer> trackerOffsets = new ArrayList<>();
|
||||
|
||||
trackers.stream().filter(Tracker::isComputed).forEach((tracker) -> {
|
||||
trackerOffsets.add(DataFeedBuilder.createTrackerData(fbb, trackerDataMaskT, tracker));
|
||||
});
|
||||
|
||||
DataFeedUpdate.startSyntheticTrackersVector(fbb, trackerOffsets.size());
|
||||
trackerOffsets.forEach((tracker -> {
|
||||
DataFeedUpdate.addSyntheticTrackers(fbb, tracker);
|
||||
}));
|
||||
return fbb.endVector();
|
||||
}
|
||||
|
||||
public static int createDevicesData(FlatBufferBuilder fbb, DeviceDataMaskT deviceDataMaskT, List<Device> devices) {
|
||||
if (deviceDataMaskT == null) return -1;
|
||||
|
||||
DeviceDataMaskT deviceDataMaskT = config.getDataMask();
|
||||
int[] devicesDataOffsets = new int[devices.size()];
|
||||
for (int i = 0; i < devices.size(); i++) {
|
||||
TrackerUDPConnection device = devices.get(i);
|
||||
Device device = devices.get(i);
|
||||
devicesDataOffsets[i] = DataFeedBuilder.createDeviceData(fbb, i, deviceDataMaskT, device);
|
||||
}
|
||||
|
||||
|
||||
@@ -4,6 +4,7 @@ import com.google.flatbuffers.FlatBufferBuilder;
|
||||
import io.eiren.util.logging.LogManager;
|
||||
import slimevr_protocol.MessageBundle;
|
||||
import slimevr_protocol.data_feed.*;
|
||||
|
||||
import java.util.function.BiConsumer;
|
||||
|
||||
public class DataFeedHandler extends ProtocolHandler<DataFeedMessageHeader> {
|
||||
@@ -30,18 +31,37 @@ public class DataFeedHandler extends ProtocolHandler<DataFeedMessageHeader> {
|
||||
DataFeedConfigT config = req.dataFeeds(i).unpack();
|
||||
conn.getContext().getDataFeedConfigList().add(config);
|
||||
conn.getContext().getDataFeedTimers().add(System.currentTimeMillis());
|
||||
System.out.println("NEW CONFIG");
|
||||
}
|
||||
}
|
||||
|
||||
private void onPollDataFeedRequest(GenericConnection conn, DataFeedMessageHeader inboundPacket) {
|
||||
private void onPollDataFeedRequest(GenericConnection conn, DataFeedMessageHeader messageHeader) {
|
||||
|
||||
PollDataFeed req = (PollDataFeed) messageHeader.message(new PollDataFeed());
|
||||
if (req == null) return;
|
||||
|
||||
FlatBufferBuilder fbb = new FlatBufferBuilder(300);
|
||||
|
||||
int messageOffset = this.buildDatafeed(fbb, req.config().unpack());
|
||||
|
||||
DataFeedMessageHeader.startDataFeedMessageHeader(fbb);
|
||||
DataFeedMessageHeader.addMessage(fbb, messageOffset);
|
||||
DataFeedMessageHeader.addMessageType(fbb, DataFeedMessage.DataFeedUpdate);
|
||||
int headerOffset = DataFeedMessageHeader.endDataFeedMessageHeader(fbb);
|
||||
|
||||
MessageBundle.startDataFeedMsgsVector(fbb, 1);
|
||||
MessageBundle.addDataFeedMsgs(fbb, headerOffset);
|
||||
int datafeedMessagesOffset = fbb.endVector();
|
||||
|
||||
int packet = createMessage(fbb, datafeedMessagesOffset, -1);
|
||||
fbb.finish(packet);
|
||||
conn.send(fbb.dataBuffer());
|
||||
}
|
||||
|
||||
public int dataFeedBuffer(FlatBufferBuilder fbb, DataFeedConfigT config) {
|
||||
int devicesOffset = DataFeedBuilder.createDevicesData(fbb, config, this.api.server.getTrackersServer().getConnections());
|
||||
public int buildDatafeed(FlatBufferBuilder fbb, DataFeedConfigT config) {
|
||||
int devicesOffset = DataFeedBuilder.createDevicesData(fbb, config.getDataMask(), this.api.server.getTrackersServer().getConnections());
|
||||
int trackersOffset = DataFeedBuilder.createSyntheticTrackersData(fbb, config.getSyntheticTrackersMask(), this.api.server.getAllTrackers());
|
||||
|
||||
return DataFeedUpdate.createDataFeedUpdate(fbb, devicesOffset, -1);
|
||||
return DataFeedUpdate.createDataFeedUpdate(fbb, devicesOffset, trackersOffset);
|
||||
}
|
||||
|
||||
public void sendDataFeedUpdate() {
|
||||
@@ -64,7 +84,7 @@ public class DataFeedHandler extends ProtocolHandler<DataFeedMessageHeader> {
|
||||
fbb = new FlatBufferBuilder(300);
|
||||
}
|
||||
|
||||
int messageOffset = this.dataFeedBuffer(fbb, configT);
|
||||
int messageOffset = this.buildDatafeed(fbb, configT);
|
||||
|
||||
DataFeedMessageHeader.startDataFeedMessageHeader(fbb);
|
||||
DataFeedMessageHeader.addMessage(fbb, messageOffset);
|
||||
|
||||
@@ -41,7 +41,7 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader> {
|
||||
AssignTrackerRequest req = (AssignTrackerRequest) messageHeader.message(new AssignTrackerRequest());
|
||||
if (req == null) return;
|
||||
|
||||
Tracker tracker = this.api.server.getAllTrackers().get(req.trackerId().trackerNum());
|
||||
Tracker tracker = this.api.server.getTrackerById(req.trackerId().unpack());
|
||||
if (tracker == null)
|
||||
return ;
|
||||
|
||||
|
||||
Reference in new issue
Block a user