mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Add PacketSerde interface and expand PacketUtils for more wpimath classes (#1058)
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
@@ -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();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user