Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add PacketSerde interface and expand PacketUtils for more wpimath classes (#1058)

Browse Source 
Follows a similar system to the current Protobuf implementation that helps make code more readable and expandable. Also wraps the NT topic to be more useful. Impl stuff is hidden so it can't be extended. Optimizes AT-specific classes by only serializing data when needed, won't save on size but will on time.

closes #1003
This commit is contained in:
Sriman Achanta
2023-12-24 19:56:08 -05:00
committed by GitHub
parent 0356eeeb50
commit 5be9b8be2c
17 changed files with 792 additions and 433 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java
View File

@@ -20,12 +20,11 @@ package org.photonvision.targeting;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
public class MultiTargetPNPResult {
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
// pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
public static final int PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);
public PNPResult estimatedPose = new PNPResult();
public List<Integer> fiducialIDsUsed = List.of();
@@ -37,27 +36,6 @@ public class MultiTargetPNPResult {
fiducialIDsUsed = ids;
}
public static MultiTargetPNPResult createFromPacket(Packet packet) {
var results = PNPResult.createFromPacket(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResult(results, ids);
}
public void populatePacket(Packet packet) {
estimatedPose.populatePacket(packet);
for (int i = 0; i < MAX_IDS; i++) {
if (i < fiducialIDsUsed.size()) {
packet.encode((short) fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public int hashCode() {
final int prime = 31;
@@ -90,4 +68,39 @@ public class MultiTargetPNPResult {
+ fiducialIDsUsed
+ "]";
}
public static final class APacketSerde implements PacketSerde<MultiTargetPNPResult> {
@Override
public int getMaxByteSize() {
// PNPResult + MAX_IDS possible targets (arbitrary upper limit that should never be hit,
// ideally)
return PNPResult.serde.getMaxByteSize() + (Short.BYTES * MAX_IDS);
}
@Override
public void pack(Packet packet, MultiTargetPNPResult result) {
PNPResult.serde.pack(packet, result.estimatedPose);
for (int i = 0; i < MAX_IDS; i++) {
if (i < result.fiducialIDsUsed.size()) {
packet.encode((short) result.fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public MultiTargetPNPResult unpack(Packet packet) {
var results = PNPResult.serde.unpack(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResult(results, ids);
}
}
public static final APacketSerde serde = new APacketSerde();
}

64
photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java
View File

@@ -19,6 +19,7 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
/**
@@ -86,31 +87,6 @@ public class PNPResult {
this.altReprojErr = altReprojErr;
}
public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
public static PNPResult createFromPacket(Packet packet) {
var present = packet.decodeBoolean();
var best = PacketUtils.decodeTransform(packet);
var alt = PacketUtils.decodeTransform(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
if (present) {
return new PNPResult(best, alt, ambiguity, bestEr, altEr);
} else {
return new PNPResult();
}
}
public void populatePacket(Packet packet) {
packet.encode(isPresent);
PacketUtils.encodeTransform(packet, best);
PacketUtils.encodeTransform(packet, alt);
packet.encode(bestReprojErr);
packet.encode(altReprojErr);
packet.encode(ambiguity);
}
@Override
public int hashCode() {
final int prime = 31;
@@ -166,4 +142,42 @@ public class PNPResult {
+ ambiguity
+ "]";
}
public static final class APacketSerde implements PacketSerde<PNPResult> {
@Override
public int getMaxByteSize() {
return 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
}
@Override
public void pack(Packet packet, PNPResult value) {
packet.encode(value.isPresent);
if (value.isPresent) {
PacketUtils.packTransform3d(packet, value.best);
PacketUtils.packTransform3d(packet, value.alt);
packet.encode(value.bestReprojErr);
packet.encode(value.altReprojErr);
packet.encode(value.ambiguity);
}
}
@Override
public PNPResult unpack(Packet packet) {
var present = packet.decodeBoolean();
if (!present) {
return new PNPResult();
}
var best = PacketUtils.unpackTransform3d(packet);
var alt = PacketUtils.unpackTransform3d(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
return new PNPResult(best, alt, ambiguity, bestEr, altEr);
}
}
public static final APacketSerde serde = new APacketSerde();
}

83
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View File

@@ -20,6 +20,7 @@ package org.photonvision.targeting;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
/** Represents a pipeline result from a PhotonCamera. */
public class PhotonPipelineResult {
@@ -71,10 +72,10 @@ public class PhotonPipelineResult {
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES
+ 8 // latency
+ MultiTargetPNPResult.PACK_SIZE_BYTES
+ 1; // target count
return Double.BYTES // latency
+ 1 // target count
+ targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
+ MultiTargetPNPResult.serde.getMaxByteSize();
}
/**
@@ -150,49 +151,6 @@ public class PhotonPipelineResult {
return multiTagResult;
}
/**
* 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();
this.multiTagResult = MultiTargetPNPResult.createFromPacket(packet);
byte targetCount = packet.decodeByte();
targets.clear();
// Decode the information of each target.
for (int i = 0; i < (int) targetCount; ++i) {
var target = new PhotonTrackedTarget();
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);
multiTagResult.populatePacket(packet);
packet.encode((byte) targets.size());
// Encode the information of each target.
for (var target : targets) target.populatePacket(packet);
// Return the packet.
return packet;
}
@Override
public int hashCode() {
final int prime = 31;
@@ -236,4 +194,35 @@ public class PhotonPipelineResult {
+ multiTagResult
+ "]";
}
public static final class APacketSerde implements PacketSerde<PhotonPipelineResult> {
@Override
public int getMaxByteSize() {
// This uses dynamic packets so it doesn't matter
return -1;
}
@Override
public void pack(Packet packet, PhotonPipelineResult value) {
packet.encode(value.latencyMillis);
packet.encode((byte) value.targets.size());
for (var target : value.targets) PhotonTrackedTarget.serde.pack(packet, target);
MultiTargetPNPResult.serde.pack(packet, value.multiTagResult);
}
@Override
public PhotonPipelineResult unpack(Packet packet) {
var latency = packet.decodeDouble();
var len = packet.decodeByte();
var targets = new ArrayList<PhotonTrackedTarget>(len);
for (int i = 0; i < len; i++) {
targets.add(PhotonTrackedTarget.serde.unpack(packet));
}
var result = MultiTargetPNPResult.serde.unpack(packet);
return new PhotonPipelineResult(latency, targets, result);
}
}
public static final APacketSerde serde = new APacketSerde();
}

176
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -21,29 +21,26 @@ import edu.wpi.first.math.geometry.Transform3d;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
public class PhotonTrackedTarget {
private static final int MAX_CORNERS = 8;
public static final int PACK_SIZE_BYTES =
Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS);
private double yaw;
private double pitch;
private double area;
private double skew;
private int fiducialId;
private Transform3d bestCameraToTarget = new Transform3d();
private Transform3d altCameraToTarget = new Transform3d();
private double poseAmbiguity;
private final double yaw;
private final double pitch;
private final double area;
private final double skew;
private final int fiducialId;
private final Transform3d bestCameraToTarget;
private final Transform3d altCameraToTarget;
private final double poseAmbiguity;
// Corners from the min-area rectangle bounding the target
private List<TargetCorner> minAreaRectCorners;
private final List<TargetCorner> minAreaRectCorners;
// Corners from whatever corner detection method was used
private List<TargetCorner> detectedCorners;
public PhotonTrackedTarget() {}
private final List<TargetCorner> detectedCorners;
/** Construct a tracked target, given exactly 4 corners */
public PhotonTrackedTarget(
@@ -197,81 +194,6 @@ public class PhotonTrackedTarget {
return true;
}
private static void encodeList(Packet packet, List<TargetCorner> list) {
packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
for (TargetCorner targetCorner : list) {
packet.encode(targetCorner.x);
packet.encode(targetCorner.y);
}
}
private static List<TargetCorner> decodeList(Packet p) {
byte len = p.decodeByte();
var ret = new ArrayList<TargetCorner>();
for (int i = 0; i < len; i++) {
double cx = p.decodeDouble();
double cy = p.decodeDouble();
ret.add(new TargetCorner(cx, cy));
}
return ret;
}
/**
* 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) {
this.yaw = packet.decodeDouble();
this.pitch = packet.decodeDouble();
this.area = packet.decodeDouble();
this.skew = packet.decodeDouble();
this.fiducialId = packet.decodeInt();
this.bestCameraToTarget = PacketUtils.decodeTransform(packet);
this.altCameraToTarget = PacketUtils.decodeTransform(packet);
this.poseAmbiguity = packet.decodeDouble();
this.minAreaRectCorners = new ArrayList<>(4);
for (int i = 0; i < 4; i++) {
double cx = packet.decodeDouble();
double cy = packet.decodeDouble();
minAreaRectCorners.add(new TargetCorner(cx, cy));
}
detectedCorners = decodeList(packet);
return packet;
}
/**
* 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(fiducialId);
PacketUtils.encodeTransform(packet, bestCameraToTarget);
PacketUtils.encodeTransform(packet, altCameraToTarget);
packet.encode(poseAmbiguity);
for (int i = 0; i < 4; i++) {
packet.encode(minAreaRectCorners.get(i).x);
packet.encode(minAreaRectCorners.get(i).y);
}
encodeList(packet, detectedCorners);
return packet;
}
@Override
public String toString() {
return "PhotonTrackedTarget{"
@@ -291,4 +213,80 @@ public class PhotonTrackedTarget {
+ minAreaRectCorners
+ '}';
}
public static final class APacketSerde implements PacketSerde<PhotonTrackedTarget> {
@Override
public int getMaxByteSize() {
return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS);
}
@Override
public void pack(Packet packet, PhotonTrackedTarget value) {
packet.encode(value.yaw);
packet.encode(value.pitch);
packet.encode(value.area);
packet.encode(value.skew);
packet.encode(value.fiducialId);
if (value.fiducialId != -1) {
PacketUtils.packTransform3d(packet, value.bestCameraToTarget);
PacketUtils.packTransform3d(packet, value.altCameraToTarget);
packet.encode(value.poseAmbiguity);
}
for (int i = 0; i < 4; i++) {
TargetCorner.serde.pack(packet, value.minAreaRectCorners.get(i));
}
packet.encode((byte) Math.min(value.detectedCorners.size(), Byte.MAX_VALUE));
for (TargetCorner targetCorner : value.detectedCorners) {
TargetCorner.serde.pack(packet, targetCorner);
}
}
@Override
public PhotonTrackedTarget unpack(Packet packet) {
var yaw = packet.decodeDouble();
var pitch = packet.decodeDouble();
var area = packet.decodeDouble();
var skew = packet.decodeDouble();
var fiducialId = packet.decodeInt();
Transform3d best;
Transform3d alt;
double ambiguity;
if (fiducialId != -1.0) {
best = PacketUtils.unpackTransform3d(packet);
alt = PacketUtils.unpackTransform3d(packet);
ambiguity = packet.decodeDouble();
} else {
best = new Transform3d();
alt = new Transform3d();
ambiguity = -1.0;
}
var minAreaRectCorners = new ArrayList<TargetCorner>(4);
for (int i = 0; i < 4; i++) {
minAreaRectCorners.add(TargetCorner.serde.unpack(packet));
}
var len = packet.decodeByte();
var detectedCorners = new ArrayList<TargetCorner>(len);
for (int i = 0; i < len; i++) {
detectedCorners.add(TargetCorner.serde.unpack(packet));
}
return new PhotonTrackedTarget(
yaw,
pitch,
area,
skew,
fiducialId,
best,
alt,
ambiguity,
minAreaRectCorners,
detectedCorners);
}
}
public static final APacketSerde serde = new APacketSerde();
}

22
photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java
View File

@@ -18,6 +18,8 @@
package org.photonvision.targeting;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
/**
* Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels.
@@ -49,4 +51,24 @@ public class TargetCorner {
public String toString() {
return "(" + x + "," + y + ')';
}
public static final class APacketSerde implements PacketSerde<TargetCorner> {
@Override
public int getMaxByteSize() {
return Double.BYTES * 2;
}
@Override
public void pack(Packet packet, TargetCorner corner) {
packet.encode(corner.x);
packet.encode(corner.y);
}
@Override
public TargetCorner unpack(Packet packet) {
return new TargetCorner(packet.decodeDouble(), packet.decodeDouble());
}
}
public static final APacketSerde serde = new APacketSerde();
}