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

Add sequence ID, capture, publish and recieve timestamp to PhotonPipelineResult (#1305)

Browse Source 
Closes #1304
This commit is contained in:
Matt
2024-05-10 14:04:34 -04:00
committed by GitHub
parent 70c2cdebe0
commit 113951100e
46 changed files with 513 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -107,6 +107,22 @@ public class Packet {
packetData[writePos++] = (byte) src;
}
/**
* Encodes the double into the packet.
*
* @param data The double to encode.
*/
public void encode(long data) {
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 double into the packet.
*
@@ -160,6 +176,22 @@ public class Packet {
| (0xff & packetData[readPos++]);
}
public long decodeLong() {
if (packetData.length < (readPos + 7)) {
return 0;
}
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 data;
}
/**
* Returns a decoded double from the packet.
*

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

@@ -28,42 +28,67 @@ import org.photonvision.targeting.proto.PhotonPipelineResultProto;
public class PhotonPipelineResult implements ProtobufSerializable {
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;
// Targets to store.
public final List<PhotonTrackedTarget> targets = new ArrayList<>();
// Latency in milliseconds.
private double latencyMillis;
// Timestamp in milliseconds.
private double timestampSeconds = -1;
// Multi-tag result
private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
// Since we don't trust NT time sync, keep track of when we got this packet into robot code
private long ntRecieveTimestampMicros;
/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {}
/**
* Constructs a pipeline result.
*
* @param latencyMillis The latency in the pipeline.
* @param sequenceID The number of frames processed by this camera since boot
* @param captureTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
* captured the image this result contains the targeting info of
* @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
* published targeting info
* @param targets The list of targets identified by the pipeline.
*/
public PhotonPipelineResult(double latencyMillis, List<PhotonTrackedTarget> targets) {
this.latencyMillis = latencyMillis;
public PhotonPipelineResult(
long sequenceID,
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
this.targets.addAll(targets);
}
/**
* Constructs a pipeline result.
*
* @param latencyMillis The latency in the pipeline.
* @param sequenceID The number of frames processed by this camera since boot
* @param captureTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
* captured the image this result contains the targeting info of
* @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
* published targeting info
* @param targets The list of targets identified by the pipeline.
* @param result Result from multi-target PNP.
*/
public PhotonPipelineResult(
double latencyMillis, List<PhotonTrackedTarget> targets, MultiTargetPNPResult result) {
this.latencyMillis = latencyMillis;
long sequenceID,
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets,
MultiTargetPNPResult result) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
this.targets.addAll(targets);
this.multiTagResult = result;
}
@@ -99,32 +124,48 @@ public class PhotonPipelineResult implements ProtobufSerializable {
return hasTargets() ? targets.get(0) : null;
}
/**
* Returns the latency in the pipeline.
*
* @return The latency in the pipeline.
*/
/** Returns the time between image capture and publish to NT */
public double getLatencyMillis() {
return latencyMillis;
return (publishTimestampMicros - captureTimestampMicros) / 1e3;
}
/**
* Returns the estimated time the frame was taken, This is more accurate than using <code>
* getLatencyMillis()</code>
* 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, or -1 if this result has no timestamp set.
* @return The timestamp in seconds
*/
public double getTimestampSeconds() {
return timestampSeconds;
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;
}
/**
* Sets the FPGA timestamp of this result in seconds.
*
* @param timestampSeconds The timestamp in seconds.
* The number of non-empty frames processed by this camera since boot. Useful to checking if a
* camera is alive.
*/
public void setTimestampSeconds(double timestampSeconds) {
this.timestampSeconds = timestampSeconds;
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;
}
/**
@@ -157,13 +198,14 @@ public class PhotonPipelineResult implements ProtobufSerializable {
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + targets.hashCode();
result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32));
long temp;
temp = Double.doubleToLongBits(latencyMillis);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(timestampSeconds);
temp = Double.doubleToLongBits(publishTimestampMicros);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32));
result = prime * result + ((targets == null) ? 0 : targets.hashCode());
result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode());
result = prime * result + (int) (ntRecieveTimestampMicros ^ (ntRecieveTimestampMicros >>> 32));
return result;
}
@@ -173,27 +215,34 @@ public class PhotonPipelineResult implements ProtobufSerializable {
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineResult other = (PhotonPipelineResult) obj;
if (!targets.equals(other.targets)) return false;
if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis))
return false;
if (Double.doubleToLongBits(timestampSeconds)
!= Double.doubleToLongBits(other.timestampSeconds)) return false;
if (captureTimestampMicros != other.captureTimestampMicros) return false;
if (Double.doubleToLongBits(publishTimestampMicros)
!= Double.doubleToLongBits(other.publishTimestampMicros)) return false;
if (sequenceID != other.sequenceID) 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;
return true;
}
@Override
public String toString() {
return "PhotonPipelineResult [targets="
return "PhotonPipelineResult [captureTimestamp="
+ captureTimestampMicros
+ ", publishTimestamp="
+ publishTimestampMicros
+ ", sequenceID="
+ sequenceID
+ ", targets="
+ targets
+ ", latencyMillis="
+ latencyMillis
+ ", timestampSeconds="
+ timestampSeconds
+ ", multiTagResult="
+ multiTagResult
+ ", ntRecieveTimestamp="
+ ntRecieveTimestampMicros
+ "]";
}
@@ -206,7 +255,9 @@ public class PhotonPipelineResult implements ProtobufSerializable {
@Override
public void pack(Packet packet, PhotonPipelineResult value) {
packet.encode(value.latencyMillis);
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);
@@ -214,7 +265,9 @@ public class PhotonPipelineResult implements ProtobufSerializable {
@Override
public PhotonPipelineResult unpack(Packet packet) {
var latency = packet.decodeDouble();
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++) {
@@ -222,7 +275,7 @@ public class PhotonPipelineResult implements ProtobufSerializable {
}
var result = MultiTargetPNPResult.serde.unpack(packet);
return new PhotonPipelineResult(latency, targets, result);
return new PhotonPipelineResult(seq, cap, pub, targets, result);
}
}

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

@@ -49,7 +49,9 @@ public class PhotonPipelineResultProto
@Override
public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) {
return new PhotonPipelineResult(
msg.getLatencyMs(),
msg.getSequenceId(),
msg.getCaptureTimestampMicros(),
msg.getNtPublishTimestampMicros(),
PhotonTrackedTarget.proto.unpack(msg.getTargets()),
MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()));
}
@@ -59,6 +61,8 @@ public class PhotonPipelineResultProto
PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets());
MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.getMultiTagResult());
msg.setLatencyMs(value.getLatencyMillis());
msg.setSequenceId(value.getSequenceID());
msg.setCaptureTimestampMicros(value.getCaptureTimestampMicros());
msg.setNtPublishTimestampMicros(value.getPublishTimestampMicros());
}
}