Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Improve data flow to NetworkTables (#52)

Browse Source 
Adds Packet class to represent byte-packed data.
This commit is contained in:
Prateek Machiraju
2020-07-19 22:42:45 -04:00
committed by GitHub
parent 82bb8c6e3c
commit a6b5112faf
5 changed files with 251 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
photon-server/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

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

141
photon-server/src/main/java/org/photonvision/common/dataflow/structures/BytePackable.java
View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
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)};
}
}

165
photon-server/src/main/java/org/photonvision/common/dataflow/structures/Packet.java Normal file
View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
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;
}
}

75
photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
View File

@@ -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<SimpleTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {

69
photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
View File

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