diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
index 736d5140d..05c256fef 100644
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
+++ b/photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
@@ -24,6 +24,7 @@ import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
import java.util.function.Supplier;
import org.photonvision.common.dataflow.CVPipelineResultConsumer;
+import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.pipeline.result.SimplePipelineResult;
@@ -151,8 +152,10 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
@Override
public void accept(CVPipelineResult result) {
var simplified = new SimplePipelineResult(result);
- var bytes = simplified.toByteArray();
- rawBytesEntry.forceSetRaw(bytes);
+ Packet packet = new Packet(simplified.getPacketSize());
+ simplified.populatePacket(packet);
+
+ rawBytesEntry.forceSetRaw(packet.getData());
pipelineIndexEntry.forceSetNumber(pipelineIndexSupplier.get());
driverModeEntry.forceSetBoolean(driverModeSupplier.getAsBoolean());
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/structures/BytePackable.java b/photon-server/src/main/java/org/photonvision/common/dataflow/structures/BytePackable.java
deleted file mode 100644
index 90d43d818..000000000
--- a/photon-server/src/main/java/org/photonvision/common/dataflow/structures/BytePackable.java
+++ /dev/null
@@ -1,141 +0,0 @@
-/*
- * Copyright (C) 2020 Photon Vision.
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see .
- */
-
-package org.photonvision.common.dataflow.structures;
-
-@SuppressWarnings("PointlessBitwiseExpression")
-public abstract class BytePackable {
- public abstract byte[] toByteArray();
-
- public abstract void fromByteArray(byte[] src);
-
- protected int bufferPosition = 0;
-
- public int getBufferPosition() {
- return bufferPosition;
- }
-
- public void resetBufferPosition() {
- bufferPosition = 0;
- }
-
- protected void bufferData(byte[] src, byte[] dest) {
- System.arraycopy(src, 0, dest, bufferPosition, src.length);
- bufferPosition += src.length;
- }
-
- protected void bufferData(byte src, byte[] dest) {
- System.arraycopy(new byte[] {src}, 0, dest, bufferPosition, 1);
- bufferPosition++;
- }
-
- protected void bufferData(int src, byte[] dest) {
- System.arraycopy(toBytes(src), 0, dest, bufferPosition, Integer.BYTES);
- bufferPosition += Integer.BYTES;
- }
-
- protected void bufferData(double src, byte[] dest) {
- System.arraycopy(toBytes(src), 0, dest, bufferPosition, Double.BYTES);
- bufferPosition += Double.BYTES;
- }
-
- protected void bufferData(boolean src, byte[] dest) {
- System.arraycopy(toBytes(src), 0, dest, bufferPosition, 1);
- bufferPosition += 1;
- }
-
- protected boolean unbufferBoolean(byte[] src) {
- return toBoolean(src, bufferPosition++);
- }
-
- protected byte unbufferByte(byte[] src) {
- var value = src[bufferPosition];
- bufferPosition++;
- return value;
- }
-
- protected byte[] unbufferBytes(byte[] src, int len) {
- var bytes = new byte[len];
- System.arraycopy(src, bufferPosition, bytes, 0, len);
- return bytes;
- }
-
- protected int unbufferInt(byte[] src) {
- var value = toInt(src, bufferPosition);
- bufferPosition += Integer.BYTES;
- return value;
- }
-
- protected double unbufferDouble(byte[] src) {
- var value = toDouble(src, bufferPosition);
- bufferPosition += Double.BYTES;
- return value;
- }
-
- private static boolean toBoolean(byte[] src, int pos) {
- return src[pos] != 0;
- }
-
- private static int toInt(byte[] src, int pos) {
- return (0xff & src[pos]) << 24
- | (0xff & src[pos + 1]) << 16
- | (0xff & src[pos + 2]) << 8
- | (0xff & src[pos + 3]) << 0;
- }
-
- private static long toLong(byte[] src, int pos) {
- return (long) (0xff & src[pos]) << 56
- | (long) (0xff & src[pos + 1]) << 48
- | (long) (0xff & src[pos + 2]) << 40
- | (long) (0xff & src[pos + 3]) << 32
- | (long) (0xff & src[pos + 4]) << 24
- | (long) (0xff & src[pos + 5]) << 16
- | (long) (0xff & src[pos + 6]) << 8
- | (long) (0xff & src[pos + 7]) << 0;
- }
-
- private static double toDouble(byte[] src, int pos) {
- return Double.longBitsToDouble(toLong(src, pos));
- }
-
- protected byte[] toBytes(double value) {
- long data = Double.doubleToRawLongBits(value);
- return new byte[] {
- (byte) ((data >> 56) & 0xff),
- (byte) ((data >> 48) & 0xff),
- (byte) ((data >> 40) & 0xff),
- (byte) ((data >> 32) & 0xff),
- (byte) ((data >> 24) & 0xff),
- (byte) ((data >> 16) & 0xff),
- (byte) ((data >> 8) & 0xff),
- (byte) ((data >> 0) & 0xff),
- };
- }
-
- protected byte[] toBytes(int value) {
- return new byte[] {
- (byte) ((value >> 24) & 0xff),
- (byte) ((value >> 16) & 0xff),
- (byte) ((value >> 8) & 0xff),
- (byte) ((value >> 0) & 0xff)
- };
- }
-
- protected byte[] toBytes(boolean value) {
- return new byte[] {(byte) (value ? 1 : 0)};
- }
-}
diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-server/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
new file mode 100644
index 000000000..6399670a5
--- /dev/null
+++ b/photon-server/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) 2020 Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package org.photonvision.common.dataflow.structures;
+
+/** A packet that holds byte-packed data to be sent over NetworkTables. */
+public class Packet {
+ // Size of the packet.
+ int size;
+ // Data stored in the packet.
+ byte[] packetData;
+ // Read and write positions.
+ int readPos, writePos;
+
+ /** Constructs an empty packet. */
+ public Packet(int size) {
+ this.size = size;
+ packetData = new byte[size];
+ }
+
+ /**
+ * Constructs a packet with the given data.
+ *
+ * @param data The packet data.
+ */
+ public Packet(byte[] data) {
+ packetData = data;
+ size = packetData.length;
+ }
+
+ /** Clears the packet and resets the read and write positions. */
+ public void clear() {
+ packetData = new byte[size];
+ readPos = 0;
+ writePos = 0;
+ }
+
+ /**
+ * Returns the packet data.
+ *
+ * @return The packet data.
+ */
+ public byte[] getData() {
+ return packetData;
+ }
+
+ /**
+ * Sets the packet data.
+ *
+ * @param data The packet data.
+ */
+ public void setData(byte[] data) {
+ packetData = data;
+ size = data.length;
+ }
+
+ /**
+ * Encodes the byte into the packet.
+ *
+ * @param src The byte to encode.
+ */
+ public void encode(byte src) {
+ packetData[writePos++] = src;
+ }
+
+ /**
+ * Encodes the integer into the packet.
+ *
+ * @param src The integer to encode.
+ */
+ public void encode(int src) {
+ packetData[writePos++] = (byte) (src >>> 24);
+ packetData[writePos++] = (byte) (src >>> 16);
+ packetData[writePos++] = (byte) (src >>> 8);
+ packetData[writePos++] = (byte) src;
+ }
+
+ /**
+ * Encodes the double into the packet.
+ *
+ * @param src The double to encode.
+ */
+ public void encode(double src) {
+ long data = Double.doubleToRawLongBits(src);
+ packetData[writePos++] = (byte) ((data >> 56) & 0xff);
+ packetData[writePos++] = (byte) ((data >> 48) & 0xff);
+ packetData[writePos++] = (byte) ((data >> 40) & 0xff);
+ packetData[writePos++] = (byte) ((data >> 32) & 0xff);
+ packetData[writePos++] = (byte) ((data >> 24) & 0xff);
+ packetData[writePos++] = (byte) ((data >> 16) & 0xff);
+ packetData[writePos++] = (byte) ((data >> 8) & 0xff);
+ packetData[writePos++] = (byte) (data & 0xff);
+ }
+
+ /**
+ * Encodes the boolean into the packet.
+ *
+ * @param src The boolean to encode.
+ */
+ public void encode(boolean src) {
+ packetData[writePos++] = src ? (byte) 1 : (byte) 0;
+ }
+
+ /**
+ * Returns a decoded byte from the packet.
+ *
+ * @return A decoded byte from the packet.
+ */
+ public byte decodeByte() {
+ return packetData[readPos++];
+ }
+
+ /**
+ * Returns a decoded int from the packet.
+ *
+ * @return A decoded int from the packet.
+ */
+ public int decodeInt() {
+ return (0xff & packetData[readPos++]) << 24
+ | (0xff & packetData[readPos++]) << 16
+ | (0xff & packetData[readPos++]) << 8
+ | (0xff & packetData[readPos++]);
+ }
+
+ /**
+ * Returns a decoded double from the packet.
+ *
+ * @return A decoded double from the packet.
+ */
+ public double decodeDouble() {
+ long data =
+ (long) (0xff & packetData[readPos++]) << 56
+ | (long) (0xff & packetData[readPos++]) << 48
+ | (long) (0xff & packetData[readPos++]) << 40
+ | (long) (0xff & packetData[readPos++]) << 32
+ | (long) (0xff & packetData[readPos++]) << 24
+ | (long) (0xff & packetData[readPos++]) << 16
+ | (long) (0xff & packetData[readPos++]) << 8
+ | (long) (0xff & packetData[readPos++]);
+ return Double.longBitsToDouble(data);
+ }
+
+ /**
+ * Returns a decoded boolean from the packet.
+ *
+ * @return A decoded boolean from the packet.
+ */
+ public boolean decodeBoolean() {
+ return packetData[readPos++] == 1;
+ }
+}
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
index 51a0aa77b..03f106920 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
@@ -20,9 +20,10 @@ package org.photonvision.vision.pipeline.result;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
+import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.target.TrackedTarget;
-public class SimplePipelineResult extends BytePackable {
+public class SimplePipelineResult {
private double latencyMillis;
private boolean hasTargets;
@@ -41,6 +42,15 @@ public class SimplePipelineResult extends BytePackable {
this(r.processingMillis, r.hasTargets(), simpleFromTrackedTargets(r.targets));
}
+ /**
+ * Returns the size of the packet needed to store this pipeline result.
+ *
+ * @return The size of the packet needed to store this pipeline result.
+ */
+ public int getPacketSize() {
+ return targets.size() * SimpleTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
+ }
+
public double getLatencyMillis() {
return latencyMillis;
}
@@ -65,38 +75,47 @@ public class SimplePipelineResult extends BytePackable {
return Objects.hash(latencyMillis, hasTargets, targets);
}
- @Override
- public byte[] toByteArray() {
- bufferPosition = 0;
- // latencyMillis + hasTargets + targetCount + targets
- int bufferSize = 8 + 1 + 1 + (targets.size() * 8 * SimpleTrackedTarget.PACK_SIZE_BYTES);
- var buff = new byte[bufferSize];
-
- bufferData(latencyMillis, buff);
- bufferData(hasTargets, buff);
- bufferData((byte) targets.size(), buff);
- for (var target : targets) {
- bufferData(target.toByteArray(), buff);
- }
-
- return buff;
- }
-
- @Override
- public void fromByteArray(byte[] src) {
- bufferPosition = 0;
-
- latencyMillis = unbufferDouble(src);
- hasTargets = unbufferBoolean(src);
- byte targetCount = unbufferByte(src);
+ /**
+ * Populates the fields of the pipeline result from the packet.
+ *
+ * @param packet The incoming packet.
+ * @return The incoming packet.
+ */
+ public Packet createFromPacket(Packet packet) {
+ // Decode latency, existence of targets, and number of targets.
+ latencyMillis = packet.decodeDouble();
+ hasTargets = packet.decodeBoolean();
+ byte targetCount = packet.decodeByte();
targets.clear();
- for (int i = 0; i < targetCount; i++) {
+
+ // Decode the information of each target.
+ for (int i = 0; i < (int) targetCount; ++i) {
var target = new SimpleTrackedTarget();
- target.fromByteArray(unbufferBytes(src, 48));
- bufferPosition += 48;
+ target.createFromPacket(packet);
targets.add(target);
}
+
+ return packet;
+ }
+
+ /**
+ * Populates the outgoing packet with information from this pipeline result.
+ *
+ * @param packet The outgoing packet.
+ * @return The outgoing packet.
+ */
+ public Packet populatePacket(Packet packet) {
+ // Encode latency, existence of targets, and number of targets.
+ packet.encode(latencyMillis);
+ packet.encode(hasTargets);
+ packet.encode((byte) targets.size());
+
+ // Encode the information of each target.
+ for (var target : targets) target.populatePacket(packet);
+
+ // Return the packet.
+ return packet;
}
private static List simpleFromTrackedTargets(List targets) {
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
index 3a6ba5429..bad4a1372 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
@@ -20,9 +20,10 @@ package org.photonvision.vision.pipeline.result;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
+import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.target.TrackedTarget;
-public class SimpleTrackedTarget extends BytePackable {
+public class SimpleTrackedTarget {
public static final int PACK_SIZE_BYTES = Double.BYTES * 7;
private double yaw;
@@ -77,44 +78,42 @@ public class SimpleTrackedTarget extends BytePackable {
return Objects.hash(yaw, pitch, area, robotRelativePose);
}
- @Override
- public byte[] toByteArray() {
- resetBufferPosition();
- byte[] data = new byte[PACK_SIZE_BYTES];
+ /**
+ * Populates the fields of this class with information from the incoming packet.
+ *
+ * @param packet The incoming packet.
+ * @return The incoming packet.
+ */
+ public Packet createFromPacket(Packet packet) {
+ yaw = packet.decodeDouble();
+ pitch = packet.decodeDouble();
+ area = packet.decodeDouble();
+ skew = packet.decodeDouble();
- bufferData(yaw, data);
- bufferData(pitch, data);
- bufferData(area, data);
- bufferData(skew, data);
- if (robotRelativePose != null) {
- bufferData(robotRelativePose.getTranslation().getX(), data);
- bufferData(robotRelativePose.getTranslation().getY(), data);
- bufferData(robotRelativePose.getRotation().getDegrees(), data);
- } else {
- bufferData((double) 0, data);
- bufferData((double) 0, data);
- bufferData((double) 0, data);
- }
+ double x = packet.decodeDouble();
+ double y = packet.decodeDouble();
+ double r = packet.decodeDouble();
- return data;
+ robotRelativePose = new Pose2d(x, y, Rotation2d.fromDegrees(r));
+
+ return packet;
}
- @Override
- public void fromByteArray(byte[] src) {
- resetBufferPosition();
- if (src.length < PACK_SIZE_BYTES) {
- // TODO: log error?
- return;
- }
+ /**
+ * Populates the outgoing packet with information from the current target.
+ *
+ * @param packet The outgoing packet.
+ * @return The outgoing packet.
+ */
+ public Packet populatePacket(Packet packet) {
+ packet.encode(yaw);
+ packet.encode(pitch);
+ packet.encode(area);
+ packet.encode(skew);
+ packet.encode(robotRelativePose.getTranslation().getX());
+ packet.encode(robotRelativePose.getTranslation().getY());
+ packet.encode(robotRelativePose.getRotation().getDegrees());
- yaw = unbufferDouble(src);
- pitch = unbufferDouble(src);
- area = unbufferDouble(src);
- skew = unbufferByte(src);
-
- var poseX = unbufferDouble(src);
- var poseY = unbufferDouble(src);
- var poseR = unbufferDouble(src);
- robotRelativePose = new Pose2d(poseX, poseY, Rotation2d.fromDegrees(poseR));
+ return packet;
}
}