mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-26 01:51:40 +00:00
Auto-generate packet dataclasses with Jinja (#1374)
This commit is contained in:
Matt
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user