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

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -18,22 +18,23 @@
package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
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.struct.MultiTargetPNPResultSerde;
import org.photonvision.targeting.proto.MultiTargetPNPResultProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class MultiTargetPNPResult implements ProtobufSerializable {
public class MultiTargetPNPResult
implements ProtobufSerializable, PhotonStructSerializable<MultiTargetPNPResult> {
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
public PNPResult estimatedPose = new PNPResult();
public List<Integer> fiducialIDsUsed = List.of();
public PnpResult estimatedPose = new PnpResult();
public List<Short> fiducialIDsUsed = List.of();
public MultiTargetPNPResult() {}
public MultiTargetPNPResult(PNPResult results, List<Integer> ids) {
public MultiTargetPNPResult(PnpResult results, List<Short> ids) {
estimatedPose = results;
fiducialIDsUsed = ids;
}
@@ -71,39 +72,13 @@ public class MultiTargetPNPResult implements ProtobufSerializable {
+ "]";
}
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();
public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto();
// tODO!
public static final MultiTargetPNPResultSerde photonStruct = new MultiTargetPNPResultSerde();
@Override
public PacketSerde<MultiTargetPNPResult> getSerde() {
return photonStruct;
}
}

107
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java Normal file
View File

@@ -0,0 +1,107 @@
/*
* Copyright (C) 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.targeting;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PhotonPipelineMetadataSerde;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class PhotonPipelineMetadata implements PhotonStructSerializable<PhotonPipelineMetadata> {
// Mirror of the heartbeat entry -- monotonically increasing
public long sequenceID;
// Image capture and NT publish timestamp, in microseconds and in the
// coprocessor timebase. As
// reported by WPIUtilJNI::now.
public long captureTimestampMicros;
public long publishTimestampMicros;
public PhotonPipelineMetadata(
long captureTimestampMicros, long publishTimestampMicros, long sequenceID) {
this.captureTimestampMicros = captureTimestampMicros;
this.publishTimestampMicros = publishTimestampMicros;
this.sequenceID = sequenceID;
}
public PhotonPipelineMetadata() {
this(-1, -1, -1);
}
/** Returns the time between image capture and publish to NT */
public double getLatencyMillis() {
return (publishTimestampMicros - captureTimestampMicros) / 1e3;
}
/** The time that this image was captured, in the coprocessor's time base. */
public long getCaptureTimestampMicros() {
return captureTimestampMicros;
}
/** The time that this result was published to NT, in the coprocessor's time base. */
public long getPublishTimestampMicros() {
return publishTimestampMicros;
}
/**
* The number of non-empty frames processed by this camera since boot. Useful to checking if a
* camera is alive.
*/
public long getSequenceID() {
return sequenceID;
}
@Override
public String toString() {
return "PhotonPipelineMetadata [sequenceID="
+ sequenceID
+ ", captureTimestampMicros="
+ captureTimestampMicros
+ ", publishTimestampMicros="
+ publishTimestampMicros
+ "]";
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32));
result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32));
result = prime * result + (int) (publishTimestampMicros ^ (publishTimestampMicros >>> 32));
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineMetadata other = (PhotonPipelineMetadata) obj;
if (sequenceID != other.sequenceID) return false;
if (captureTimestampMicros != other.captureTimestampMicros) return false;
if (publishTimestampMicros != other.publishTimestampMicros) return false;
return true;
}
public static final PhotonPipelineMetadataSerde photonStruct = new PhotonPipelineMetadataSerde();
@Override
public PacketSerde<PhotonPipelineMetadata> getSerde() {
return photonStruct;
}
}

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

@@ -20,33 +20,33 @@ package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import java.util.Optional;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PhotonPipelineResultSerde;
import org.photonvision.targeting.proto.PhotonPipelineResultProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/** Represents a pipeline result from a PhotonCamera. */
public class PhotonPipelineResult implements ProtobufSerializable {
public class PhotonPipelineResult
implements ProtobufSerializable, PhotonStructSerializable<PhotonPipelineResult> {
private static boolean HAS_WARNED = false;
// Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As
// reported by WPIUtilJNI::now.
private long captureTimestampMicros = -1;
private long publishTimestampMicros = -1;
// Mirror of the heartbeat entry -- monotonically increasing
private long sequenceID = -1;
// Frame capture metadata
public PhotonPipelineMetadata metadata;
// Targets to store.
public final List<PhotonTrackedTarget> targets = new ArrayList<>();
public List<PhotonTrackedTarget> targets = new ArrayList<>();
// Multi-tag result
private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
public Optional<MultiTargetPNPResult> multitagResult;
// Since we don't trust NT time sync, keep track of when we got this packet into robot code
private long ntRecieveTimestampMicros;
// HACK: Since we don't trust NT time sync, keep track of when we got this packet into robot code
public long ntReceiveTimestampMicros = -1;
/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {}
public PhotonPipelineResult() {
this(new PhotonPipelineMetadata(), List.of(), Optional.empty());
}
/**
* Constructs a pipeline result.
@@ -63,10 +63,10 @@ public class PhotonPipelineResult implements ProtobufSerializable {
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
this.targets.addAll(targets);
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
targets,
Optional.empty());
}
/**
@@ -85,12 +85,20 @@ public class PhotonPipelineResult implements ProtobufSerializable {
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets,
MultiTargetPNPResult result) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
Optional<MultiTargetPNPResult> result) {
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
targets,
result);
}
public PhotonPipelineResult(
PhotonPipelineMetadata metadata,
List<PhotonTrackedTarget> targets,
Optional<MultiTargetPNPResult> result) {
this.metadata = metadata;
this.targets.addAll(targets);
this.multiTagResult = result;
this.multitagResult = result;
}
/**
@@ -99,10 +107,11 @@ public class PhotonPipelineResult implements ProtobufSerializable {
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return Double.BYTES // latency
+ 1 // target count
+ targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
+ MultiTargetPNPResult.serde.getMaxByteSize();
throw new RuntimeException("TODO");
// return Double.BYTES // latency
// + 1 // target count
// + targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
// + MultiTargetPNPResult.serde.getMaxByteSize();
}
/**
@@ -124,50 +133,6 @@ public class PhotonPipelineResult implements ProtobufSerializable {
return hasTargets() ? targets.get(0) : null;
}
/** Returns the time between image capture and publish to NT */
public double getLatencyMillis() {
return (publishTimestampMicros - captureTimestampMicros) / 1e3;
}
/**
* Returns the estimated time the frame was taken, in the recieved system's time base. This is
* calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture
* timestamp, coproc timebase))
*
* @return The timestamp in seconds
*/
public double getTimestampSeconds() {
return (ntRecieveTimestampMicros - (publishTimestampMicros - captureTimestampMicros)) / 1e6;
}
/** The time that this image was captured, in the coprocessor's time base. */
public long getCaptureTimestampMicros() {
return captureTimestampMicros;
}
/** The time that this result was published to NT, in the coprocessor's time base. */
public long getPublishTimestampMicros() {
return publishTimestampMicros;
}
/**
* The number of non-empty frames processed by this camera since boot. Useful to checking if a
* camera is alive.
*/
public long getSequenceID() {
return sequenceID;
}
/** The time that the robot recieved this result, in the FPGA timebase. */
public long getNtRecieveTimestampMicros() {
return ntRecieveTimestampMicros;
}
/** Sets the FPGA timestamp this result was recieved by robot code */
public void setRecieveTimestampMicros(long timestampMicros) {
this.ntRecieveTimestampMicros = timestampMicros;
}
/**
* Returns whether the pipeline has targets.
*
@@ -192,22 +157,54 @@ public class PhotonPipelineResult implements ProtobufSerializable {
* Return the latest multi-target result. Be sure to check
* getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
*/
public MultiTargetPNPResult getMultiTagResult() {
return multiTagResult;
public Optional<MultiTargetPNPResult> getMultiTagResult() {
return multitagResult;
}
/**
* Returns the estimated time the frame was taken, in the Received system's time base. This is
* calculated as (NT Receive time (robot base) - (publish timestamp, coproc timebase - capture
* timestamp, coproc timebase))
*
* @return The timestamp in seconds
*/
public double getTimestampSeconds() {
return (ntReceiveTimestampMicros
- (metadata.publishTimestampMicros - metadata.captureTimestampMicros))
/ 1e6;
}
/** The time that the robot Received this result, in the FPGA timebase. */
public long getNtReceiveTimestampMicros() {
return ntReceiveTimestampMicros;
}
/** Sets the FPGA timestamp this result was Received by robot code */
public void setReceiveTimestampMicros(long timestampMicros) {
this.ntReceiveTimestampMicros = timestampMicros;
}
@Override
public String toString() {
return "PhotonPipelineResult [metadata="
+ metadata
+ ", targets="
+ targets
+ ", multitagResult="
+ multitagResult
+ ", ntReceiveTimestampMicros="
+ ntReceiveTimestampMicros
+ "]";
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32));
long temp;
temp = Double.doubleToLongBits(publishTimestampMicros);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32));
result = prime * result + ((metadata == null) ? 0 : metadata.hashCode());
result = prime * result + ((targets == null) ? 0 : targets.hashCode());
result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode());
result = prime * result + (int) (ntRecieveTimestampMicros ^ (ntRecieveTimestampMicros >>> 32));
result = prime * result + ((multitagResult == null) ? 0 : multitagResult.hashCode());
result = prime * result + (int) (ntReceiveTimestampMicros ^ (ntReceiveTimestampMicros >>> 32));
return result;
}
@@ -217,70 +214,24 @@ public class PhotonPipelineResult implements ProtobufSerializable {
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineResult other = (PhotonPipelineResult) obj;
if (captureTimestampMicros != other.captureTimestampMicros) return false;
if (Double.doubleToLongBits(publishTimestampMicros)
!= Double.doubleToLongBits(other.publishTimestampMicros)) return false;
if (sequenceID != other.sequenceID) return false;
if (metadata == null) {
if (other.metadata != null) return false;
} else if (!metadata.equals(other.metadata)) return false;
if (targets == null) {
if (other.targets != null) return false;
} else if (!targets.equals(other.targets)) return false;
if (multiTagResult == null) {
if (other.multiTagResult != null) return false;
} else if (!multiTagResult.equals(other.multiTagResult)) return false;
if (ntRecieveTimestampMicros != other.ntRecieveTimestampMicros) return false;
if (multitagResult == null) {
if (other.multitagResult != null) return false;
} else if (!multitagResult.equals(other.multitagResult)) return false;
if (ntReceiveTimestampMicros != other.ntReceiveTimestampMicros) return false;
return true;
}
@Override
public String toString() {
return "PhotonPipelineResult [captureTimestamp="
+ captureTimestampMicros
+ ", publishTimestamp="
+ publishTimestampMicros
+ ", sequenceID="
+ sequenceID
+ ", targets="
+ targets
+ ", multiTagResult="
+ multiTagResult
+ ", ntRecieveTimestamp="
+ ntRecieveTimestampMicros
+ "]";
}
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.sequenceID);
packet.encode(value.captureTimestampMicros);
packet.encode(value.publishTimestampMicros);
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 seq = packet.decodeLong();
var cap = packet.decodeLong();
var pub = packet.decodeLong();
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(seq, cap, pub, targets, result);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PhotonPipelineResultSerde photonStruct = new PhotonPipelineResultSerde();
public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto();
@Override
public PacketSerde<PhotonPipelineResult> getSerde() {
return photonStruct;
}
}

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

@@ -19,32 +19,32 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
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.struct.PhotonTrackedTargetSerde;
import org.photonvision.targeting.proto.PhotonTrackedTargetProto;
import org.photonvision.utils.PacketUtils;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class PhotonTrackedTarget implements ProtobufSerializable {
public class PhotonTrackedTarget
implements ProtobufSerializable, PhotonStructSerializable<PhotonTrackedTarget> {
private static final int MAX_CORNERS = 8;
private final double yaw;
private final double pitch;
private final double area;
private final double skew;
private final int fiducialId;
private final int classId;
private final float objDetectConf;
private final Transform3d bestCameraToTarget;
private final Transform3d altCameraToTarget;
private final double poseAmbiguity;
public double yaw;
public double pitch;
public double area;
public double skew;
public int fiducialId;
public int objDetectId;
public float objDetectConf;
public Transform3d bestCameraToTarget;
public Transform3d altCameraToTarget;
public double poseAmbiguity;
// Corners from the min-area rectangle bounding the target
private final List<TargetCorner> minAreaRectCorners;
public List<TargetCorner> minAreaRectCorners;
// Corners from whatever corner detection method was used
private final List<TargetCorner> detectedCorners;
public List<TargetCorner> detectedCorners;
/** Construct a tracked target, given exactly 4 corners */
public PhotonTrackedTarget(
@@ -71,7 +71,7 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
this.area = area;
this.skew = skew;
this.fiducialId = fiducialId;
this.classId = classId;
this.objDetectId = classId;
this.objDetectConf = objDetectConf;
this.bestCameraToTarget = pose;
this.altCameraToTarget = altPose;
@@ -80,6 +80,10 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
this.poseAmbiguity = ambiguity;
}
public PhotonTrackedTarget() {
// TODO Auto-generated constructor stub
}
public double getYaw() {
return yaw;
}
@@ -103,7 +107,7 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
/** Get the object detection class ID number, or -1 if not set. */
public int getDetectedObjectClassID() {
return classId;
return objDetectId;
}
/**
@@ -235,75 +239,11 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
+ '}';
}
public static final class APacketSerde implements PacketSerde<PhotonTrackedTarget> {
@Override
public int getMaxByteSize() {
return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 1 + 4 + 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);
packet.encode(value.classId);
packet.encode(value.objDetectConf);
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();
var classId = packet.decodeInt();
var objDetectConf = packet.decodeFloat();
Transform3d best = PacketUtils.unpackTransform3d(packet);
Transform3d alt = PacketUtils.unpackTransform3d(packet);
double ambiguity = packet.decodeDouble();
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,
classId,
objDetectConf,
best,
alt,
ambiguity,
minAreaRectCorners,
detectedCorners);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto();
public static final PhotonTrackedTargetSerde photonStruct = new PhotonTrackedTargetSerde();
@Override
public PacketSerde<PhotonTrackedTarget> getSerde() {
return photonStruct;
}
}

81
photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java → photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java
View File

@@ -19,10 +19,10 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PnpResultSerde;
import org.photonvision.targeting.proto.PNPResultProto;
import org.photonvision.utils.PacketUtils;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/**
* The best estimated transformation from solvePnP, and possibly an alternate transformation
@@ -33,37 +33,30 @@ import org.photonvision.utils.PacketUtils;
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResult implements ProtobufSerializable {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
public class PnpResult implements ProtobufSerializable, PhotonStructSerializable<PnpResult> {
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
public Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
public double bestReprojErr;
/**
* Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
* to the best solution.
*/
public final Transform3d alt;
public Transform3d alt;
/** If no alternate solution is found, this is bestReprojErr */
public final double altReprojErr;
public double altReprojErr;
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
public double ambiguity;
/** An empty (invalid) result. */
public PNPResult() {
this.isPresent = false;
public PnpResult() {
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
@@ -71,17 +64,16 @@ public class PNPResult implements ProtobufSerializable {
this.altReprojErr = 0;
}
public PNPResult(Transform3d best, double bestReprojErr) {
public PnpResult(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResult(
public PnpResult(
Transform3d best,
Transform3d alt,
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
@@ -93,7 +85,6 @@ public class PNPResult implements ProtobufSerializable {
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (isPresent ? 1231 : 1237);
result = prime * result + ((best == null) ? 0 : best.hashCode());
long temp;
temp = Double.doubleToLongBits(bestReprojErr);
@@ -111,8 +102,7 @@ public class PNPResult implements ProtobufSerializable {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PNPResult other = (PNPResult) obj;
if (isPresent != other.isPresent) return false;
PnpResult other = (PnpResult) obj;
if (best == null) {
if (other.best != null) return false;
} else if (!best.equals(other.best)) return false;
@@ -130,9 +120,7 @@ public class PNPResult implements ProtobufSerializable {
@Override
public String toString() {
return "PNPResult [isPresent="
+ isPresent
+ ", best="
return "PnpResult [best="
+ best
+ ", bestReprojErr="
+ bestReprojErr
@@ -145,42 +133,11 @@ public class PNPResult implements ProtobufSerializable {
+ "]";
}
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();
public static final PNPResultProto proto = new PNPResultProto();
public static final PnpResultSerde photonStruct = new PnpResultSerde();
@Override
public PacketSerde<PnpResult> getSerde() {
return photonStruct;
}
}

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

@@ -19,23 +19,28 @@ package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.TargetCornerSerde;
import org.photonvision.targeting.proto.TargetCornerProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/**
* Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels.
* Origin at the top left, plus-x to the right, plus-y down.
*/
public class TargetCorner implements ProtobufSerializable {
public final double x;
public final double y;
public class TargetCorner implements ProtobufSerializable, PhotonStructSerializable<TargetCorner> {
public double x;
public double y;
public TargetCorner(double cx, double cy) {
this.x = cx;
this.y = cy;
}
public TargetCorner() {
this(0, 0);
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
@@ -54,24 +59,11 @@ public class TargetCorner implements ProtobufSerializable {
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();
public static final TargetCornerProto proto = new TargetCornerProto();
public static final TargetCornerSerde photonStruct = new TargetCornerSerde();
@Override
public PacketSerde<TargetCorner> getSerde() {
return photonStruct;
}
}

12
photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java
View File

@@ -21,7 +21,7 @@ import edu.wpi.first.util.protobuf.Protobuf;
import java.util.ArrayList;
import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import us.hebi.quickbuf.Descriptors.Descriptor;
import us.hebi.quickbuf.RepeatedInt;
@@ -39,7 +39,7 @@ public class MultiTargetPNPResultProto
@Override
public Protobuf<?, ?>[] getNested() {
return new Protobuf<?, ?>[] {PNPResult.proto};
return new Protobuf<?, ?>[] {PnpResult.proto};
}
@Override
@@ -49,17 +49,17 @@ public class MultiTargetPNPResultProto
@Override
public MultiTargetPNPResult unpack(ProtobufMultiTargetPNPResult msg) {
ArrayList<Integer> fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length());
ArrayList<Short> fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length());
for (var packedFidId : msg.getFiducialIdsUsed()) {
fidIdsUsed.add(packedFidId);
fidIdsUsed.add(packedFidId.shortValue());
}
return new MultiTargetPNPResult(PNPResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed);
return new MultiTargetPNPResult(PnpResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed);
}
@Override
public void pack(ProtobufMultiTargetPNPResult msg, MultiTargetPNPResult value) {
PNPResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
PnpResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size());
for (int i = 0; i < value.fiducialIDsUsed.size(); i++) {

21
photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java
View File

@@ -20,13 +20,13 @@ package org.photonvision.targeting.proto;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.Protobuf;
import org.photonvision.proto.Photon.ProtobufPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
public class PNPResultProto implements Protobuf<PnpResult, ProtobufPNPResult> {
@Override
public Class<PNPResult> getTypeClass() {
return PNPResult.class;
public Class<PnpResult> getTypeClass() {
return PnpResult.class;
}
@Override
@@ -45,12 +45,8 @@ public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
}
@Override
public PNPResult unpack(ProtobufPNPResult msg) {
if (!msg.getIsPresent()) {
return new PNPResult();
}
return new PNPResult(
public PnpResult unpack(ProtobufPNPResult msg) {
return new PnpResult(
Transform3d.proto.unpack(msg.getBest()),
Transform3d.proto.unpack(msg.getAlt()),
msg.getAmbiguity(),
@@ -59,12 +55,11 @@ public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
}
@Override
public void pack(ProtobufPNPResult msg, PNPResult value) {
public void pack(ProtobufPNPResult msg, PnpResult value) {
Transform3d.proto.pack(msg.getMutableBest(), value.best);
Transform3d.proto.pack(msg.getMutableAlt(), value.alt);
msg.setAmbiguity(value.ambiguity)
.setBestReprojErr(value.bestReprojErr)
.setAltReprojErr(value.altReprojErr)
.setIsPresent(value.isPresent);
.setAltReprojErr(value.altReprojErr);
}
}

19
photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java
View File

@@ -18,6 +18,7 @@
package org.photonvision.targeting.proto;
import edu.wpi.first.util.protobuf.Protobuf;
import java.util.Optional;
import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
@@ -53,16 +54,24 @@ public class PhotonPipelineResultProto
msg.getCaptureTimestampMicros(),
msg.getNtPublishTimestampMicros(),
PhotonTrackedTarget.proto.unpack(msg.getTargets()),
MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()));
msg.hasMultiTargetResult()
? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()))
: Optional.empty());
}
@Override
public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) {
PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets());
MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.getMultiTagResult());
msg.setSequenceId(value.getSequenceID());
msg.setCaptureTimestampMicros(value.getCaptureTimestampMicros());
msg.setNtPublishTimestampMicros(value.getPublishTimestampMicros());
if (value.getMultiTagResult().isPresent()) {
MultiTargetPNPResult.proto.pack(
msg.getMutableMultiTargetResult(), value.getMultiTagResult().get());
} else {
msg.clearMultiTargetResult();
}
msg.setSequenceId(value.metadata.getSequenceID());
msg.setCaptureTimestampMicros(value.metadata.getCaptureTimestampMicros());
msg.setNtPublishTimestampMicros(value.metadata.getPublishTimestampMicros());
}
}

24
photon-targeting/src/main/java/org/photonvision/targeting/serde/PhotonStructSerializable.java Normal file
View File

@@ -0,0 +1,24 @@
/*
* Copyright (C) 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.targeting.serde;
import org.photonvision.common.dataflow.structures.PacketSerde;
public interface PhotonStructSerializable<T> {
PacketSerde<T> getSerde();
}