From a6b5112faf7a8ed940c173ea8499eb65cae2f22f Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sun, 19 Jul 2020 22:42:45 -0400 Subject: [PATCH] Improve data flow to NetworkTables (#52) Adds Packet class to represent byte-packed data. --- .../networktables/NTDataPublisher.java | 7 +- .../dataflow/structures/BytePackable.java | 141 --------------- .../common/dataflow/structures/Packet.java | 165 ++++++++++++++++++ .../pipeline/result/SimplePipelineResult.java | 75 +++++--- .../pipeline/result/SimpleTrackedTarget.java | 69 ++++---- 5 files changed, 251 insertions(+), 206 deletions(-) delete mode 100644 photon-server/src/main/java/org/photonvision/common/dataflow/structures/BytePackable.java create mode 100644 photon-server/src/main/java/org/photonvision/common/dataflow/structures/Packet.java 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; } }