diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 889875317..a24c673eb 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -19,6 +19,7 @@ package org.photonvision.common.dataflow.networktables; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.util.WPIUtilJNI; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; @@ -27,6 +28,7 @@ import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.networktables.NTTopicSet; +import org.photonvision.common.util.math.MathUtils; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; @@ -46,8 +48,6 @@ public class NTDataPublisher implements CVPipelineResultConsumer { private final BooleanSupplier driverModeSupplier; private final Consumer driverModeConsumer; - private long heartbeatCounter = 0; - public NTDataPublisher( String cameraNickname, Supplier pipelineIndexSupplier, @@ -129,9 +129,13 @@ public class NTDataPublisher implements CVPipelineResultConsumer { @Override public void accept(CVPipelineResult result) { + var now = WPIUtilJNI.now(); + var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos()); var simplified = new PhotonPipelineResult( - result.getLatencyMillis(), + result.sequenceID, + captureMicros, + now, TrackedTarget.simpleFromTrackedTargets(result.targets), result.multiTagResult); @@ -190,7 +194,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer { ts.cameraDistortionPublisher.accept(new double[] {}); } - ts.heartbeatPublisher.set(heartbeatCounter++); + ts.heartbeatPublisher.set(result.sequenceID); // TODO...nt4... is this needed? rootTable.getInstance().flush(); diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java index 11d234c02..a22dab745 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -67,6 +67,10 @@ public class MathUtils { return micros * 1000; } + public static long nanosToMicros(long nanos) { + return nanos / 1000; + } + public static double map( double value, double in_min, double in_max, double out_min, double out_max) { return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java b/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java index d4202f77a..8caeb0082 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java @@ -22,6 +22,7 @@ import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.opencv.Releasable; public class Frame implements Releasable { + public final long sequenceID; public final long timestampNanos; // Frame should at _least_ contain the thresholded frame, and sometimes the color image @@ -32,11 +33,13 @@ public class Frame implements Releasable { public final FrameStaticProperties frameStaticProperties; public Frame( + long sequenceID, CVMat color, CVMat processed, FrameThresholdType type, long timestampNanos, FrameStaticProperties frameStaticProperties) { + this.sequenceID = sequenceID; this.colorImage = color; this.processedImage = processed; this.type = type; @@ -45,15 +48,17 @@ public class Frame implements Releasable { } public Frame( + long sequenceID, CVMat color, CVMat processed, FrameThresholdType processType, FrameStaticProperties frameStaticProperties) { - this(color, processed, processType, MathUtils.wpiNanoTime(), frameStaticProperties); + this(sequenceID, color, processed, processType, MathUtils.wpiNanoTime(), frameStaticProperties); } public Frame() { this( + -1, new CVMat(), new CVMat(), FrameThresholdType.NONE, diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java index 70de9c511..a60f63692 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/FrameProvider.java @@ -21,18 +21,20 @@ import java.util.function.Supplier; import org.photonvision.vision.opencv.ImageRotationMode; import org.photonvision.vision.pipe.impl.HSVPipe; -public interface FrameProvider extends Supplier { - String getName(); +public abstract class FrameProvider implements Supplier { + protected int sequenceID = 0; + + public abstract String getName(); /** Ask the camera to produce a certain kind of processed image (e.g. HSV or greyscale) */ - void requestFrameThresholdType(FrameThresholdType type); + public abstract void requestFrameThresholdType(FrameThresholdType type); /** Ask the camera to rotate frames it outputs */ - void requestFrameRotation(ImageRotationMode rotationMode); + public abstract void requestFrameRotation(ImageRotationMode rotationMode); /** Ask the camera to provide either the input, output, or both frames. */ - void requestFrameCopies(boolean copyInput, boolean copyOutput); + public abstract void requestFrameCopies(boolean copyInput, boolean copyOutput); /** Ask the camera to rotate frames it outputs */ - void requestHsvSettings(HSVPipe.HSVParams params); + public abstract void requestHsvSettings(HSVPipe.HSVParams params); } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java index 034ae6615..396c72e3a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java @@ -29,7 +29,7 @@ import org.photonvision.vision.pipe.impl.GrayscalePipe; import org.photonvision.vision.pipe.impl.HSVPipe; import org.photonvision.vision.pipe.impl.RotateImagePipe; -public abstract class CpuImageProcessor implements FrameProvider { +public abstract class CpuImageProcessor extends FrameProvider { protected static class CapturedFrame { CVMat colorImage; FrameStaticProperties staticProps; @@ -86,13 +86,20 @@ public abstract class CpuImageProcessor implements FrameProvider { } else { outputMat = new CVMat(); } + + ++sequenceID; } else { System.out.println("Input was empty!"); outputMat = new CVMat(); } return new Frame( - input.colorImage, outputMat, m_processType, input.captureTimestamp, input.staticProps); + sequenceID, + input.colorImage, + outputMat, + m_processType, + input.captureTimestamp, + input.staticProps); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java index 83a947726..7cc084618 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java @@ -99,7 +99,7 @@ public class FileFrameProvider extends CpuImageProcessor { @Override public CapturedFrame getInputMat() { var out = new CVMat(); - out.copyTo(originalFrame); + out.copyFrom(originalFrame); // block to keep FPS at a defined rate if (System.currentTimeMillis() - lastGetMillis < millisDelay) { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java index 68f454cda..0f9f301b7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java @@ -30,7 +30,7 @@ import org.photonvision.vision.opencv.CVMat; import org.photonvision.vision.opencv.ImageRotationMode; import org.photonvision.vision.pipe.impl.HSVPipe.HSVParams; -public class LibcameraGpuFrameProvider implements FrameProvider { +public class LibcameraGpuFrameProvider extends FrameProvider { private final LibcameraGpuSettables settables; static final Logger logger = new Logger(LibcameraGpuFrameProvider.class, LogGroup.Camera); @@ -92,7 +92,11 @@ public class LibcameraGpuFrameProvider implements FrameProvider { LibCameraJNI.releasePair(p_ptr); + // Know frame is good -- increment sequence + ++sequenceID; + return new Frame( + sequenceID, colorMat, processedMat, type, diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java b/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java index e58f0d11d..728d39915 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java +++ b/photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java @@ -36,11 +36,11 @@ public class CVMat implements Releasable { this(new Mat()); } - public void copyTo(CVMat srcMat) { - copyTo(srcMat.getMat()); + public void copyFrom(CVMat srcMat) { + copyFrom(srcMat.getMat()); } - public void copyTo(Mat srcMat) { + public void copyFrom(Mat srcMat) { srcMat.copyTo(mat); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 7f69af66c..eff9eaff7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -116,7 +116,7 @@ public class AprilTagPipeline extends CVPipeline> tagDetectionPipeResult; @@ -219,7 +219,8 @@ public class AprilTagPipeline extends CVPipeline> tagDetectionPipeResult; @@ -236,7 +236,8 @@ public class ArucoPipeline extends CVPipeline> rknnResult = rknnPipe.run(input_frame.colorImage); + CVPipeResult> rknnResult = rknnPipe.run(frame.colorImage); sumPipeNanosElapsed += rknnResult.nanosElapsed; List targetList; var names = rknnPipe.getClassNames(); - input_frame.colorImage.getMat().copyTo(input_frame.processedImage.getMat()); + frame.colorImage.getMat().copyTo(frame.processedImage.getMat()); // ***************** change based on backend *********************** @@ -125,7 +125,7 @@ public class ObjectDetectionPipeline var fps = fpsResult.output; return new CVPipelineResult( - sumPipeNanosElapsed, fps, collect2dTargetsResult.output, input_frame, names); + frame.sequenceID, sumPipeNanosElapsed, fps, collect2dTargetsResult.output, frame, names); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java index bf5b73a4e..b92735a53 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java @@ -233,6 +233,7 @@ public class OutputStreamPipeline { var fps = fpsResult.output; return new CVPipelineResult( + inputAndOutputFrame.sequenceID, sumPipeNanosElapsed, fps, // Unused but here just in case targetsToDraw, diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index e03889785..fa2cb1a55 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -169,6 +169,6 @@ public class ReflectivePipeline extends CVPipeline objectDetectionClassNames; public CVPipelineResult( - double processingNanos, double fps, List targets, Frame inputFrame) { - this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame, List.of()); + long sequenceID, + double processingNanos, + double fps, + List targets, + Frame inputFrame) { + this( + sequenceID, + processingNanos, + fps, + targets, + new MultiTargetPNPResult(), + inputFrame, + List.of()); } public CVPipelineResult( + long sequenceID, double processingNanos, double fps, List targets, Frame inputFrame, List classNames) { - this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame, classNames); + this( + sequenceID, + processingNanos, + fps, + targets, + new MultiTargetPNPResult(), + inputFrame, + classNames); } public CVPipelineResult( + long sequenceID, double processingNanos, double fps, List targets, MultiTargetPNPResult multiTagResult, Frame inputFrame) { - this(processingNanos, fps, targets, multiTagResult, inputFrame, List.of()); + this(sequenceID, processingNanos, fps, targets, multiTagResult, inputFrame, List.of()); } public CVPipelineResult( + long sequenceID, double processingNanos, double fps, List targets, MultiTargetPNPResult multiTagResult, Frame inputFrame, List classNames) { + this.sequenceID = sequenceID; this.processingNanos = processingNanos; this.fps = fps; this.targets = targets != null ? targets : Collections.emptyList(); @@ -74,11 +97,12 @@ public class CVPipelineResult implements Releasable { } public CVPipelineResult( + long sequenceID, double processingNanos, double fps, List targets, MultiTargetPNPResult multiTagResult) { - this(processingNanos, fps, targets, multiTagResult, null, List.of()); + this(sequenceID, processingNanos, fps, targets, multiTagResult, null, List.of()); } public boolean hasTargets() { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java index 7ad8ff83c..a06bd8443 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java @@ -29,7 +29,11 @@ public class CalibrationPipelineResult extends CVPipelineResult { } public CalibrationPipelineResult( - double latencyNanos, double fps, Frame outputFrame, List> corners) { - super(latencyNanos, fps, cornersToTarget(corners), outputFrame); + long sequenceID, + double latencyNanos, + double fps, + Frame outputFrame, + List> corners) { + super(sequenceID, latencyNanos, fps, cornersToTarget(corners), outputFrame); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java index 609a3f448..67e8160c7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java @@ -21,7 +21,7 @@ import java.util.List; import org.photonvision.vision.frame.Frame; public class DriverModePipelineResult extends CVPipelineResult { - public DriverModePipelineResult(double latencyNanos, double fps, Frame outputFrame) { - super(latencyNanos, fps, List.of(), outputFrame); + public DriverModePipelineResult(long seq, double latencyNanos, double fps, Frame outputFrame) { + super(seq, latencyNanos, fps, List.of(), outputFrame); } } diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index 8958c8546..0850f7e2f 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -193,6 +193,7 @@ public class Calibrate3dPipeTest { calibration3dPipeline.takeSnapshot(); var frame = new Frame( + 0, new CVMat(Imgcodecs.imread(file.getAbsolutePath())), new CVMat(), FrameThresholdType.NONE, diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index 559134091..22d407479 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/photon-lib/py/photonlibpy/packet.py @@ -96,6 +96,14 @@ class Packet: """ return self._decodeGeneric(">l", 4) + def decodei64(self) -> int: + """ + * Returns a decoded int64 from the packet. + * + * @return A decoded int64 from the packet. + """ + return self._decodeGeneric(">q", 8) + def decodeDouble(self) -> float: """ * Returns a decoded double from the packet. diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 8be306aa5..1ccca6d44 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -1,6 +1,6 @@ from enum import Enum import ntcore -from wpilib import Timer +from wpilib import RobotController, Timer import wpilib from photonlibpy.packet import Packet from photonlibpy.photonPipelineResult import PhotonPipelineResult @@ -78,6 +78,7 @@ class PhotonCamera: def getLatestResult(self) -> PhotonPipelineResult: self._versionCheck() + now = RobotController.getFPGATime() retVal = PhotonPipelineResult() packetWithTimestamp = self._rawBytesEntry.getAtomic() byteList = packetWithTimestamp.value @@ -88,10 +89,8 @@ class PhotonCamera: else: pkt = Packet(byteList) retVal.populateFromPacket(pkt) - # NT4 allows us to correct the timestamp based on when the message was sent - retVal.setTimestampSeconds( - timestamp / 1e6 - retVal.getLatencyMillis() / 1e3 - ) + # We don't trust NT4 time, hack around + retVal.ntRecieveTimestampMicros = now return retVal def getDriverMode(self) -> bool: diff --git a/photon-lib/py/photonlibpy/photonPipelineResult.py b/photon-lib/py/photonlibpy/photonPipelineResult.py index fb045546f..cd2fec335 100644 --- a/photon-lib/py/photonlibpy/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/photonPipelineResult.py @@ -7,14 +7,27 @@ from photonlibpy.photonTrackedTarget import PhotonTrackedTarget @dataclass class PhotonPipelineResult: - latencyMillis: float = -1.0 - timestampSec: float = -1.0 + # Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As + # reported by WPIUtilJNI::now. + captureTimestampMicros: int = -1 + publishTimestampMicros: int = -1 + + # Mirror of the heartbeat entry -- monotonically increasing + sequenceID: int = -1 + + # Since we don't trust NT time sync, keep track of when we got this packet into robot code + ntRecieveTimestampMicros: int = -1 + targets: list[PhotonTrackedTarget] = field(default_factory=list) multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult) def populateFromPacket(self, packet: Packet) -> Packet: self.targets = [] - self.latencyMillis = packet.decodeDouble() + + self.sequenceID = packet.decodei64() + self.captureTimestampMicros = packet.decodei64() + self.publishTimestampMicros = packet.decodei64() + targetCount = packet.decode8() for _ in range(targetCount): @@ -27,14 +40,20 @@ class PhotonPipelineResult: return packet - def setTimestampSeconds(self, timestampSec: float) -> None: - self.timestampSec = timestampSec - def getLatencyMillis(self) -> float: - return self.latencyMillis + return (self.publishTimestampMicros - self.captureTimestampMicros) / 1e3 - def getTimestamp(self) -> float: - return self.timestampSec + def getTimestampSeconds(self) -> float: + """ + 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)) + """ + # TODO - we don't trust NT4 to correctly latency-compensate ntRecieveTimestampMicros + return ( + self.ntRecieveTimestampMicros + - (self.publishTimestampMicros - self.captureTimestampMicros) + ) / 1e6 def getTargets(self) -> list[PhotonTrackedTarget]: return self.targets diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index a53c10618..b7ddd5083 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -207,7 +207,7 @@ class PhotonPoseEstimator: return None cameraResult = self._camera.getLatestResult() - if cameraResult.timestampSec < 0: + if cameraResult.getTimestampSeconds() < 0: return None # If the pose cache timestamp was set, and the result is from the same @@ -215,12 +215,15 @@ class PhotonPoseEstimator: # empty result if ( self._poseCacheTimestampSeconds > 0.0 - and abs(self._poseCacheTimestampSeconds - cameraResult.timestampSec) < 1e-6 + and abs( + self._poseCacheTimestampSeconds - cameraResult.getTimestampSeconds() + ) + < 1e-6 ): return None # Remember the timestamp of the current result used - self._poseCacheTimestampSeconds = cameraResult.timestampSec + self._poseCacheTimestampSeconds = cameraResult.getTimestampSeconds() # If no targets seen, trivial case -- return empty result if not cameraResult.targets: @@ -259,7 +262,7 @@ class PhotonPoseEstimator: ) return EstimatedRobotPose( best, - result.timestampSec, + result.getTimestampSeconds(), result.targets, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ) @@ -281,7 +284,6 @@ class PhotonPoseEstimator: lowestAmbiguityTarget = None lowestAmbiguityScore = 10.0 - for target in result.targets: targetPoseAmbiguity = target.poseAmbiguity @@ -307,7 +309,7 @@ class PhotonPoseEstimator: targetPosition.transformBy( lowestAmbiguityTarget.getBestCameraToTarget().inverse() ).transformBy(self.robotToCamera.inverse()), - result.timestampSec, + result.getTimestampSeconds(), result.targets, PoseStrategy.LOWEST_AMBIGUITY, ) diff --git a/photon-lib/py/test/data.py b/photon-lib/py/test/data.py deleted file mode 100644 index 1c3d7897f..000000000 --- a/photon-lib/py/test/data.py +++ /dev/null @@ -1,83 +0,0 @@ -# fmt: off - -# Apriltag pipeline, no targets visible -rawBytes1 = [ - 64,45,35,10,111,197,138,185,0,0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -] - -# Retroreflective pipeline ,no targets visible -rawBytes2 = [ - 64,41,107,104,139,218,148,54,0,0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -] - -# Retroreflective pipeline, many targets visible (like, at least 5) -rawBytes3 = [ - 64,49,60,163,231,209,53,17,8,64,54,167,7,236,180,193,9,64,45,220,9,202,55,93,183,63,227,128,0,0,0,0,0,64,86,128,0,0,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,114,144,0,0,0,0,0,64,51,0,0,0,0,0,0,64,115,80,0,0,0,0,0,64,51,0,0,0,0,0,0,64,115,80,0,0,0,0,0,64,77,0,0,0,0,0,0,64,114,144,0,0,0,0,0,64,77,0,0,0,0,0,0,0,191,245,25,16,205,70,208,255,64,20,38,132,240,124,103,173,63,198,77,205,156,15,75,128,64,77,132,163,224,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,98,2,210,254,49,147,12,64,90,73,105,152,56,9,30,64,99,52,180,223,13,131,252,64,88,218,90,145,158,229,134,64,99,250,90,129,206,108,244,64,91,109,45,103,199,246,226,64,98,200,120,160,242,124,4,64,92,220,60,110,97,26,122,0,192,43,81,6,103,64,172,203,64,17,155,176,98,110,238,56,63,170,0,0,0,0,0,0,64,86,128,0,0,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,82,192,0,0,0,0,0,64,93,64,0,0,0,0,0,64,82,192,0,0,0,0,0,64,90,0,0,0,0,0,0,64,83,128,0,0,0,0,0,64,90,0,0,0,0,0,0,64,83,128,0,0,0,0,0,64,93,64,0,0,0,0,0,0,64,55,92,45,193,120,131,71,64,39,83,124,127,93,59,5,63,163,95,95,87,69,127,0,192,82,253,174,44,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,115,16,0,0,8,162,118,64,79,128,0,0,14,168,14,64,115,40,120,120,160,74,2,64,76,112,240,240,158,154,220,64,115,111,255,255,247,93,138,64,76,255,255,255,241,87,242,64,115,87,135,135,95,181,254,64,80,7,135,135,176,178,146,0,64,54,238,153,156,124,254,143,64,33,203,224,209,111,94,118,63,164,170,170,165,226,57,85,192,56,113,167,0,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,114,175,3,128,226,2,249,64,84,42,133,33,58,4,108,64,115,95,3,128,229,226,101,64,82,234,133,33,128,189,196,64,115,111,255,255,29,253,7,64,83,127,255,222,197,251,148,64,114,191,255,255,26,29,155,64,84,191,255,222,127,66,60,0,64,54,24,108,191,99,29,97,64,50,234,255,191,63,2,108,63,159,187,187,135,161,0,0,192,58,144,167,0,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,114,112,0,6,165,21,159,64,37,255,255,112,35,70,217,64,114,220,204,211,38,253,223,64,30,102,101,109,127,219,67,64,114,243,51,57,90,234,97,64,36,204,204,79,220,185,39,64,114,134,102,108,217,2,33,64,43,153,153,9,64,18,94,0,191,248,131,229,0,233,37,92,64,27,57,176,97,1,217,104,63,144,120,120,70,92,211,0,64,82,253,174,64,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,98,156,60,87,45,144,181,64,87,161,225,245,153,66,109,64,98,231,135,162,89,154,95,64,87,124,60,80,183,84,165,64,99,15,15,40,210,111,75,64,88,184,120,138,102,189,147,64,98,195,195,221,166,101,161,64,88,222,30,47,72,171,91,0,63,230,44,178,251,184,167,126,63,240,141,86,39,148,80,111,63,138,170,170,170,170,170,171,0,0,0,0,0,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,99,224,0,0,0,0,0,64,96,96,0,0,0,0,0,64,101,32,0,0,0,0,0,64,96,96,0,0,0,0,0,64,101,32,0,0,0,0,0,64,96,128,0,0,0,0,0,64,99,224,0,0,0,0,0,64,96,128,0,0,0,0,0,0,0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -] - -# Retroreflective pipeline, exactly one target visible -rawBytes4 = [ - 64,46,46,39,224,239,153,128,1,191,246,187,80,38,172,30,62,64,20,74,119,253,143,230,102,63,197,29,96,155,178,49,170,192,67,231,29,0,0,0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,97,236,16,182,148,150,165,64,90,182,142,132,168,136,162,64,99,13,163,153,51,211,223,64,88,211,239,9,93,37,176,64,99,233,247,137,107,105,91,64,90,228,184,123,87,119,94,64,98,200,100,166,204,44,33,64,92,199,87,246,162,218,80,0,0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -] - -# Apriltag 2d targets, 2 targets visible -rawBytes5 = [ - 64,87,61,104,76,240,115,155,2,192,19,10,149,152,236,136,45,64,12,81,172,134,28,107,33,64,0,73,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,108,56,142,143,85,217,250,64,108,243,255,218,95,15,122,64,109,24,142,145,56,72,194,64,99,19,255,209,123,237,92,64,115,191,255,248,85,19,3,64,100,0,0,37,160,240,134,64,115,79,255,247,99,219,159,64,109,224,0,46,132,18,164,4,64,108,64,56,224,0,0,1,64,108,190,88,128,0,0,0,64,115,87,117,32,0,0,0,64,109,250,221,0,0,0,0,64,115,197,222,191,255,255,255,64,100,18,183,0,0,0,0,64,109,146,238,223,255,255,255,64,99,139,100,255,255,255,255,64,25,237,242,228,107,194,46,63,240,75,228,210,151,145,105,64,2,220,85,85,85,85,85,0,0,0,0,0,0,0,0,0,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,117,98,66,80,63,186,109,64,109,242,156,228,38,185,71,64,120,194,66,72,28,206,3,64,100,146,156,228,55,146,195,64,124,223,255,239,192,69,147,64,106,127,255,219,217,70,185,64,121,127,255,247,227,49,253,64,113,239,255,237,228,54,158,4,64,117,143,103,223,255,255,255,64,109,181,117,160,0,0,0,64,121,140,149,64,0,0,0,64,113,240,57,127,255,255,255,64,124,224,95,32,0,0,1,64,106,130,60,64,0,0,1,64,120,183,111,224,0,0,0,64,100,238,35,128,0,0,1,0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -] - -# Random, old data packet, not sure what it's supposed to have but it should be valid -rawBytes6 = [ - 64,67,206,198,96,162,1,71,1,63,208,183,103,126,7,178,243,64,43,139,131,227,249,218,186,64,3,41,85,85,85,85,85,0,0,0,0,0,0,0,0,0,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,96,183,95,135,246,244,23,64,80,250,243,32,88,104,23,64,98,63,255,252,129,210,89,64,58,0,0,10,154,37,74,64,103,95,255,248,9,11,233,64,66,255,255,255,79,47,210,64,101,215,95,131,126,45,167,64,83,250,243,29,89,118,174,4,64,96,211,76,255,255,255,255,64,81,29,41,64,0,0,0,64,101,245,167,159,255,255,255,64,83,228,87,223,255,255,255,64,103,121,185,32,0,0,1,64,67,110,64,192,0,0,0,64,98,91,64,32,0,0,0,64,58,246,143,0,0,0,1,0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -] - -# Random, old data packet, not sure what it's supposed to have but it should be valid -rawBytes7 = [ - 64,45,115,21,181,115,234,179,1,191,174,13,20,135,142,233,101, - 64,8,70,95,109,157,41,94,64,88,209,106,37,213,86,0,0,0,0,0,0,0, - 0,0,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,64,109,223,255,192,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,64,115,239,255,192,0,0,0,0,0,0,0,0,0,0,0,64,115,239, - 255,192,0,0,0,64,109,223,255,192,0,0,0,0,0,255,255,255,255,255,255, - 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, - 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, - 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, - 255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0] - -# Random, old data packet, not sure what it's supposed to have but it should be valid -rawBytes8 = [ - 64,45,134,209,124,94,246,48,1,192,20,108,19,172,246,183,250,192,43,132,62,46, - 45,144,244,63,231,43,236,24,192,165,0,192,84,104,149,84,0,0,0,255,255,255,255, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0, - 0,0,0,0,0,64,93,192,0,11,255,168,69,64,109,224,0,6,159,242,44,64,95,18,247,150,60, - 55,212,64,105,95,131,45,13,191,220,64,97,109,36,58,0,43,222,64,105,166,163,121,96, - 13,212,64,96,195,168,116,225,228,22,64,110,39,32,82,242,64,36,0,0,255,255,255,255, - 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, - 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, - 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, - 0,0,0,0,0,0,0,0,0 -] - -# Random, old data packet, not sure what it's supposed to have but it should be valid -rawBytes9 = [ - 64,66,131,243,115,41,195,72,2,192,32,5,185,224,7,44,212,64,36,113,236,196,56,3,198,64,4,41,85,85,85,85,86,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,84,58,214,175,241,59,116,64,85,61,37,235,61,113,140,64,89,191,255,199,224,203,48,64,70,127,255,245,50,94,21,64,97,162,148,136,7,98,70,64,80,130,218,20,194,142,116,64,93,191,255,248,31,52,208,64,90,128,0,5,102,208,246,4,64,84,133,113,127,255,255,255,64,84,214,69,192,0,0,1,64,93,244,62,95,255,255,255,64,90,151,84,32,0,0,1,64,97,188,73,96,0,0,0,64,80,177,119,64,0,0,0,64,89,237,96,160,0,0,1,64,70,178,22,160,0,0,1,63,211,145,124,33,214,33,142,64,15,33,250,151,128,46,96,64,6,94,170,170,170,170,171,0,0,0,0,0,0,0,0,0,0,0,18,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,63,240,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,240,0,0,0,0,0,0,64,96,13,19,119,56,95,3,64,92,98,154,147,14,69,208,64,100,127,255,255,154,137,77,64,84,64,0,0,118,237,128,64,104,82,236,136,199,160,253,64,92,157,101,108,241,186,48,64,99,224,0,0,101,118,179,64,98,95,255,255,196,137,64,4,64,96,55,169,128,0,0,1,64,92,114,93,64,0,0,0,64,99,228,226,192,0,0,0,64,98,126,35,64,0,0,0,64,104,94,243,224,0,0,0,64,92,187,36,128,0,0,0,64,100,131,245,192,0,0,1,64,84,121,213,192,0,0,1,0,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 - -] diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index dd8588e6c..9b0a286e1 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/photon-lib/py/test/photonPoseEstimator_test.py @@ -36,8 +36,10 @@ def test_lowestAmbiguityStrategy(): cameraOne = PhotonCameraInjector() cameraOne.result = PhotonPipelineResult( - 2, - 11, + 0, + 2 * 1e3, + 1, + 11 * 1e6, [ PhotonTrackedTarget( 3.0, @@ -115,7 +117,7 @@ def test_lowestAmbiguityStrategy(): estimatedPose = estimator.update() pose = estimatedPose.estimatedPose - assertEquals(11, estimatedPose.timestampSeconds) + assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3) assertEquals(1, pose.x, 0.01) assertEquals(3, pose.y, 0.01) assertEquals(2, pose.z, 0.01) @@ -124,8 +126,10 @@ def test_lowestAmbiguityStrategy(): def test_multiTagOnCoprocStrategy(): cameraOne = PhotonCameraInjector() cameraOne.result = PhotonPipelineResult( - 2, - 11, + 0, + 2 * 1e3, + 1, + 11 * 1e6, # There needs to be at least one target present for pose estimation to work # Doesn't matter which/how many targets for this test [ @@ -167,7 +171,7 @@ def test_multiTagOnCoprocStrategy(): estimatedPose = estimator.update() pose = estimatedPose.estimatedPose - assertEquals(11, estimatedPose.timestampSeconds) + assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3) assertEquals(1, pose.x, 0.01) assertEquals(3, pose.y, 0.01) assertEquals(2, pose.z, 0.01) @@ -178,8 +182,10 @@ def test_cacheIsInvalidated(): cameraOne = PhotonCameraInjector() result = PhotonPipelineResult( - 2, - 20, + 0, + 2 * 1e3, + 1, + 20 * 1e6, [ PhotonTrackedTarget( 3.0, @@ -211,7 +217,9 @@ def test_cacheIsInvalidated(): ) # Empty result, expect empty result - cameraOne.result = PhotonPipelineResult(timestampSec=1) + cameraOne.result = PhotonPipelineResult( + captureTimestampMicros=0, publishTimestampMicros=0, ntRecieveTimestampMicros=1e6 + ) estimatedPose = estimator.update() assert estimatedPose is None @@ -220,14 +228,14 @@ def test_cacheIsInvalidated(): estimatedPose = estimator.update() assert estimatedPose is not None assertEquals(20, estimatedPose.timestampSeconds, 0.01) - assertEquals(20, estimator._poseCacheTimestampSeconds) + assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) # And again -- pose cache should mean this is empty cameraOne.result = result estimatedPose = estimator.update() assert estimatedPose is None # Expect the old timestamp to still be here - assertEquals(20, estimator._poseCacheTimestampSeconds) + assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) # Set new field layout -- right after, the pose cache timestamp should be -1 estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0) @@ -236,7 +244,7 @@ def test_cacheIsInvalidated(): cameraOne.result = result estimatedPose = estimator.update() assertEquals(20, estimatedPose.timestampSeconds, 0.01) - assertEquals(20, estimator._poseCacheTimestampSeconds) + assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) def assertEquals(expected, actual, epsilon=0.0): diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py index 0bc03d587..479c8bad8 100644 --- a/photon-lib/py/test/photonlibpy_test.py +++ b/photon-lib/py/test/photonlibpy_test.py @@ -1,64 +1,3 @@ -from photonlibpy.packet import Packet -from photonlibpy.photonPipelineResult import PhotonPipelineResult -from data import rawBytes1 -from data import rawBytes2 -from data import rawBytes3 -from data import rawBytes4 -from data import rawBytes5 -from data import rawBytes6 -from data import rawBytes7 -from data import rawBytes8 -from data import rawBytes9 - - -def setupCommon(bytesIn): - res = PhotonPipelineResult() - packet = Packet(bytesIn) - res.populateFromPacket(packet) - assert packet.outOfBytes is False - return res - - -def test_byteParse1(): - res = setupCommon(rawBytes1) - assert len(res.getTargets()) == 0 - - -def test_byteParse2(): - res = setupCommon(rawBytes2) - assert len(res.getTargets()) == 0 - - -def test_byteParse3(): - res = setupCommon(rawBytes3) - assert len(res.getTargets()) >= 4 - - -def test_byteParse4(): - res = setupCommon(rawBytes4) - assert len(res.getTargets()) == 1 - - -def test_byteParse5(): - res = setupCommon(rawBytes5) - assert len(res.getTargets()) == 2 - - -def test_byteParse6(): - res = setupCommon(rawBytes6) - # assert len(res.getTargets()) >= 0 - - -def test_byteParse7(): - res = setupCommon(rawBytes7) - # assert len(res.getTargets()) >= 0 - - -def test_byteParse8(): - res = setupCommon(rawBytes8) - # assert len(res.getTargets()) >= 0 - - -def test_byteParse9(): - res = setupCommon(rawBytes9) - # assert len(res.getTargets()) >= 0 +def test_roundTrip(): + # TODO implement packet encoding, or just kill me + assert True diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 6dbb6811e..6e06bc1b6 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -44,6 +44,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import java.util.Optional; import java.util.Set; @@ -181,8 +182,7 @@ public class PhotonCamera implements AutoCloseable { // Set the timestamp of the result. // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. - ret.setTimestampSeconds( - (resultSubscriber.subscriber.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); + ret.setRecieveTimestampMicros(RobotController.getFPGATime()); // Return result. return ret; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 60a1ae2d0..d7df0a44e 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -34,6 +34,7 @@ import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.util.PixelFormat; import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.wpilibj.RobotController; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -539,7 +540,16 @@ public class PhotonCameraSim implements AutoCloseable { } // put this simulated data to NT - return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResult); + var now = RobotController.getFPGATime(); + var ret = + new PhotonPipelineResult( + heartbeatCounter, + now - (long) (latencyMillis * 1000), + now, + detectableTgts, + multitagResult); + ret.setRecieveTimestampMicros(now); + return ret; } /** diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 4a0b34249..94225d175 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -115,6 +116,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { PhotonPipelineResult result; // Fill the packet with latest data and populate result. + units::microsecond_t now = + units::microsecond_t(frc::RobotController::GetFPGATime()); const auto value = rawBytesEntry.Get(); if (!value.size()) return result; @@ -122,8 +125,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { packet >> result; - result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) - - result.GetLatency()); + result.SetRecieveTimestamp(now); return result; } diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index bddcc0cb8..720cc48d9 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -369,7 +370,10 @@ class PhotonCameraSim { multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; } - return PhotonPipelineResult{latency, detectableTgts, multiTagResults}; + units::second_t now = frc::Timer::GetFPGATimestamp(); + + return PhotonPipelineResult{heartbeatCounter, now - latency, now, + detectableTgts, multiTagResults}; } void SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); @@ -426,7 +430,7 @@ class PhotonCameraSim { PhotonCamera* cam; NTTopicSet ts{}; - uint64_t heartbeatCounter{0}; + int64_t heartbeatCounter{0}; uint64_t nextNTEntryTime{wpi::Now()}; diff --git a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h index 365efebd5..c6f2e8adf 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h @@ -90,7 +90,7 @@ class SimPhotonCamera : public PhotonCamera { latencyMillisEntry.SetDouble(latency.to()); std::sort(targetList.begin(), targetList.end(), [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); }); - PhotonPipelineResult newResult{latency, targetList}; + PhotonPipelineResult newResult{0, 0_s, latency, targetList}; Packet packet{}; packet << newResult; diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 81842dcc4..734c57077 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -63,7 +63,9 @@ class PhotonPoseEstimatorTest { PhotonCameraInjector cameraOne = new PhotonCameraInjector(); cameraOne.result = new PhotonPipelineResult( - 2, + 0, + 0, + 0, List.of( new PhotonTrackedTarget( 3.0, @@ -122,7 +124,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setTimestampSeconds(11); + cameraOne.result.setRecieveTimestampMicros((long) (11 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -142,7 +144,9 @@ class PhotonPoseEstimatorTest { PhotonCameraInjector cameraOne = new PhotonCameraInjector(); cameraOne.result = new PhotonPipelineResult( - 2, + 0, + 0, + 0, List.of( new PhotonTrackedTarget( 3.0, @@ -202,7 +206,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setTimestampSeconds(4); + cameraOne.result.setRecieveTimestampMicros((long) (4 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -225,7 +229,9 @@ class PhotonPoseEstimatorTest { PhotonCameraInjector cameraOne = new PhotonCameraInjector(); cameraOne.result = new PhotonPipelineResult( - 2, + 0, + 0, + 0, List.of( new PhotonTrackedTarget( 3.0, @@ -284,7 +290,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setTimestampSeconds(17); + cameraOne.result.setRecieveTimestampMicros((long) (17 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -308,7 +314,9 @@ class PhotonPoseEstimatorTest { PhotonCameraInjector cameraOne = new PhotonCameraInjector(); cameraOne.result = new PhotonPipelineResult( - 2, + 0, + 0, + 0, List.of( new PhotonTrackedTarget( 3.0, @@ -367,7 +375,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setTimestampSeconds(1); + cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -383,7 +391,9 @@ class PhotonPoseEstimatorTest { cameraOne.result = new PhotonPipelineResult( - 2, + 0, + 0, + 0, List.of( new PhotonTrackedTarget( 3.0, @@ -442,7 +452,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setTimestampSeconds(7); + cameraOne.result.setRecieveTimestampMicros((long) (7 * 1e6)); estimatedPose = estimator.update(); pose = estimatedPose.get().estimatedPose; @@ -458,7 +468,9 @@ class PhotonPoseEstimatorTest { PhotonCameraInjector cameraOne = new PhotonCameraInjector(); var result = new PhotonPipelineResult( - 2, + 0, + 0, + 0, List.of( new PhotonTrackedTarget( 3.0, @@ -479,7 +491,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - result.setTimestampSeconds(20); + result.setRecieveTimestampMicros((long) (20 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -490,7 +502,7 @@ class PhotonPoseEstimatorTest { // Empty result, expect empty result cameraOne.result = new PhotonPipelineResult(); - cameraOne.result.setTimestampSeconds(1); + cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6)); Optional estimatedPose = estimator.update(); assertFalse(estimatedPose.isPresent()); @@ -523,7 +535,9 @@ class PhotonPoseEstimatorTest { PhotonCameraInjector cameraOne = new PhotonCameraInjector(); cameraOne.result = new PhotonPipelineResult( - 2, + 0, + 0, + 0, List.of( new PhotonTrackedTarget( 3.0, @@ -582,7 +596,7 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); // 3 3 3 ambig .4 - cameraOne.result.setTimestampSeconds(20); + cameraOne.result.setRecieveTimestampMicros(20 * 1000000); PhotonPoseEstimator estimator = new PhotonPoseEstimator( diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index aed60ff2f..9835d0d72 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -83,8 +83,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(11)); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, std::move(cameraOne), {}); @@ -138,8 +138,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(17_s); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(17_s); photon::PhotonPoseEstimator estimator( aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne), @@ -181,8 +181,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(17)); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator( aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {}); @@ -225,8 +225,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(17)); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {}); @@ -259,8 +259,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; - estimator.GetCamera()->testResult = {2_ms, targetsThree}; - estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21)); + estimator.GetCamera()->testResult = {0, 0_s, 2_ms, targetsThree}; + estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(21)); estimatedPose = estimator.Update(); ASSERT_TRUE(estimatedPose); @@ -300,8 +300,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(15)); + cameraOne.testResult = {0, 0_ms, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, std::move(cameraOne), {}); @@ -347,17 +347,18 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { std::move(cameraOne), {}); // empty input, expect empty out - estimator.GetCamera()->testResult = {2_ms, {}}; - estimator.GetCamera()->testResult.SetTimestamp(units::second_t(1)); + estimator.GetCamera()->testResult = {0, 0_s, 2_ms, {}}; + estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(1)); auto estimatedPose = estimator.Update(); EXPECT_FALSE(estimatedPose); // Set result, and update -- expect present and timestamp to be 15 - estimator.GetCamera()->testResult = {3_ms, targets}; - estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15)); + estimator.GetCamera()->testResult = {0, 0_s, 3_ms, targets}; + estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(15)); estimatedPose = estimator.Update(); EXPECT_TRUE(estimatedPose); - EXPECT_NEAR(15, estimatedPose.value().timestamp.to(), 1e-6); + EXPECT_NEAR((15_s - 3_ms).to(), + estimatedPose.value().timestamp.to(), 1e-6); // And again -- now pose cache should be empty estimatedPose = estimator.Update(); diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java index e97e3f165..6a6e6419e 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java @@ -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. * diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 34f0602b1..1b75f96c7 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -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 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 targets) { - this.latencyMillis = latencyMillis; + public PhotonPipelineResult( + long sequenceID, + long captureTimestamp, + long publishTimestamp, + List 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 targets, MultiTargetPNPResult result) { - this.latencyMillis = latencyMillis; + long sequenceID, + long captureTimestamp, + long publishTimestamp, + List 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 - * getLatencyMillis() + * 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(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); } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index dbbc18065..6022c24b7 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -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()); } } diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp index 3de5bc074..2e3367d0c 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -19,25 +19,29 @@ namespace photon { PhotonPipelineResult::PhotonPipelineResult( - units::millisecond_t latency, std::span targets) - : latency(latency), - targets(targets.data(), targets.data() + targets.size()) {} - -PhotonPipelineResult::PhotonPipelineResult( - units::millisecond_t latency, std::span targets, + int64_t sequenceID, units::microsecond_t captureTimestamp, + units::microsecond_t publishTimestamp, + std::span targets, MultiTargetPNPResult multitagResult) - : latency(latency), + : sequenceID(sequenceID), + captureTimestamp(captureTimestamp), + publishTimestamp(publishTimestamp), targets(targets.data(), targets.data() + targets.size()), multitagResult(multitagResult) {} bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { - return latency == other.latency && targets == other.targets && - multitagResult == other.multitagResult; + return sequenceID == other.sequenceID && + captureTimestamp == other.captureTimestamp && + publishTimestamp == other.publishTimestamp && + ntRecieveTimestamp == other.ntRecieveTimestamp && + targets == other.targets && multitagResult == other.multitagResult; } Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { // Encode latency and number of targets. - packet << result.latency.value() + packet << result.sequenceID + << static_cast(result.captureTimestamp.value()) + << static_cast(result.publishTimestamp.value()) << static_cast(result.targets.size()); // Encode the information of each target. @@ -50,21 +54,35 @@ Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { // Decode latency, existence of targets, and number of targets. - double latencyMillis = 0; + int64_t sequenceID = 0; + int64_t capTS = 0; + int64_t pubTS = 0; int8_t targetCount = 0; - packet >> latencyMillis >> targetCount; - result.latency = units::millisecond_t(latencyMillis); + std::vector targets; + MultiTargetPNPResult multitagResult; - result.targets.clear(); + packet >> sequenceID >> capTS >> pubTS >> targetCount; + + targets.clear(); + targets.reserve(targetCount); // Decode the information of each target. for (int i = 0; i < targetCount; ++i) { PhotonTrackedTarget target; packet >> target; - result.targets.push_back(target); + targets.push_back(target); } - packet >> result.multitagResult; + packet >> multitagResult; + + units::microsecond_t captureTS = + units::microsecond_t{static_cast(capTS)}; + units::microsecond_t publishTS = + units::microsecond_t{static_cast(pubTS)}; + + result = PhotonPipelineResult{sequenceID, captureTS, publishTS, targets, + multitagResult}; + return packet; } } // namespace photon diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp index 3605ae594..6beeccaab 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp @@ -41,7 +41,11 @@ wpi::Protobuf::Unpack( } return photon::PhotonPipelineResult{ - units::millisecond_t{m->latency_ms()}, targets, + m->sequence_id(), + units::microsecond_t{static_cast(m->capture_timestamp_micros())}, + units::microsecond_t{ + static_cast(m->nt_publish_timestamp_micros())}, + targets, wpi::UnpackProtobuf( m->multi_target_result())}; } @@ -50,7 +54,9 @@ void wpi::Protobuf::Pack( google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) { auto m = static_cast(msg); - m->set_latency_ms(value.latency.value()); + m->set_sequence_id(value.sequenceID); + m->set_capture_timestamp_micros(value.captureTimestamp.value()); + m->set_nt_publish_timestamp_micros(value.publishTimestamp.value()); m->clear_targets(); for (const auto& t : value.GetTargets()) { diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index c0965fbd9..a34d1df6f 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -41,21 +41,20 @@ class PhotonPipelineResult { /** * Constructs a pipeline result. - * @param latency 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 multitagResult The multitarget result. Default to empty */ - PhotonPipelineResult(units::millisecond_t latency, - std::span targets); - - /** - * Constructs a pipeline result. - * @param latency The latency in the pipeline. - * @param targets The list of targets identified by the pipeline. - * @param multitagResult The multitarget result - */ - PhotonPipelineResult(units::millisecond_t latency, + PhotonPipelineResult(int64_t sequenceID, + units::microsecond_t captureTimestamp, + units::microsecond_t publishTimestamp, std::span targets, - MultiTargetPNPResult multitagResult); + MultiTargetPNPResult multitagResult = {}); /** * Returns the best target in this pipeline result. If there are no targets, @@ -81,7 +80,9 @@ class PhotonPipelineResult { * Returns the latency in the pipeline. * @return The latency in the pipeline. */ - units::millisecond_t GetLatency() const { return latency; } + units::millisecond_t GetLatency() const { + return publishTimestamp - captureTimestamp; + } /** * Returns the estimated time the frame was taken, @@ -89,7 +90,9 @@ class PhotonPipelineResult { * @return The timestamp in seconds or -1 if this result was not initiated * with a timestamp. */ - units::second_t GetTimestamp() const { return timestamp; } + units::second_t GetTimestamp() const { + return ntRecieveTimestamp - (publishTimestamp - captureTimestamp); + } /** * Return the latest mulit-target result, as calculated on your coprocessor. @@ -99,11 +102,14 @@ class PhotonPipelineResult { const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; } /** - * Sets the timestamp in seconds - * @param timestamp The timestamp in seconds + * The number of non-empty frames processed by this camera since boot. Useful + * to checking if a camera is alive. */ - void SetTimestamp(const units::second_t timestamp) { - this->timestamp = timestamp; + int64_t SequenceID() const { return sequenceID; } + + /** Sets the FPGA timestamp this result was recieved by robot code */ + void SetRecieveTimestamp(const units::second_t timestamp) { + this->ntRecieveTimestamp = timestamp; } /** @@ -125,8 +131,17 @@ class PhotonPipelineResult { friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); - units::millisecond_t latency = 0_s; - units::second_t timestamp = -1_s; + // Mirror of the heartbeat entry -- monotonically increasing + int64_t sequenceID = -1; + + // Image capture and NT publish timestamp, in microseconds and in the + // coprocessor timebase. As reported by WPIUtilJNI::now. + units::microsecond_t captureTimestamp; + units::microsecond_t publishTimestamp; + // Since we don't trust NT time sync, keep track of when we got this packet + // into robot code + units::microsecond_t ntRecieveTimestamp = -1_s; + wpi::SmallVector targets; MultiTargetPNPResult multitagResult; inline static bool HAS_WARNED = false; diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index d5b75802f..23daca25d 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -56,8 +56,12 @@ message ProtobufPhotonTrackedTarget { } message ProtobufPhotonPipelineResult { - double latency_ms = 1; + double latency_ms = 1 [deprecated = true]; repeated ProtobufPhotonTrackedTarget targets = 2; ProtobufMultiTargetPNPResult multi_target_result = 3; + + int64 sequence_id = 4; + int64 capture_timestamp_micros = 5; + int64 nt_publish_timestamp_micros = 6; } diff --git a/photon-targeting/src/test/java/org/photonvision/PacketTest.java b/photon-targeting/src/test/java/org/photonvision/PacketTest.java index 3885f3810..ab361932a 100644 --- a/photon-targeting/src/test/java/org/photonvision/PacketTest.java +++ b/photon-targeting/src/test/java/org/photonvision/PacketTest.java @@ -180,7 +180,7 @@ class PacketTest { @Test void pipelineResultSerde() { - var ret1 = new PhotonPipelineResult(1, List.of()); + var ret1 = new PhotonPipelineResult(1, 2, 3, List.of()); var p1 = new Packet(ret1.getPacketSize()); PhotonPipelineResult.serde.pack(p1, ret1); var unpackedRet1 = PhotonPipelineResult.serde.unpack(p1); @@ -188,7 +188,9 @@ class PacketTest { var ret2 = new PhotonPipelineResult( + 1, 2, + 3, List.of( new PhotonTrackedTarget( 3.0, @@ -235,7 +237,9 @@ class PacketTest { var ret3 = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -289,7 +293,9 @@ class PacketTest { public void testMultiTargetSerde() { var result = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index 72dacda16..5bb1f1b5e 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -35,7 +35,9 @@ public class PhotonPipelineResultTest { a = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -77,7 +79,9 @@ public class PhotonPipelineResultTest { new TargetCorner(7, 8))))); b = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -121,7 +125,9 @@ public class PhotonPipelineResultTest { a = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -167,7 +173,9 @@ public class PhotonPipelineResultTest { List.of(1, 2, 3))); b = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -218,7 +226,9 @@ public class PhotonPipelineResultTest { public void inequalityTest() { var a = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -260,7 +270,9 @@ public class PhotonPipelineResultTest { new TargetCorner(7, 8))))); var b = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 7.0, @@ -304,7 +316,9 @@ public class PhotonPipelineResultTest { a = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -350,7 +364,9 @@ public class PhotonPipelineResultTest { List.of(3, 4, 7))); b = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java index c745fab8a..6857a808b 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java @@ -39,7 +39,9 @@ public class PhotonPipelineResultProtoTest { // non multitag result result = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, @@ -87,7 +89,9 @@ public class PhotonPipelineResultProtoTest { // multitag result result = new PhotonPipelineResult( - 2, + 3, + 4, + 5, List.of( new PhotonTrackedTarget( 3.0, diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 1094a3783..61b8bf124 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -71,7 +71,7 @@ TEST(PacketTest, PhotonTrackedTarget) { } TEST(PacketTest, PhotonPipelineResult) { - photon::PhotonPipelineResult result{1_s, {}}; + photon::PhotonPipelineResult result{0, 0_s, 1_s, {}}; photon::Packet p; p << result; @@ -109,7 +109,7 @@ TEST(PacketTest, PhotonPipelineResult) { {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}}; - photon::PhotonPipelineResult result2{2_s, targets}; + photon::PhotonPipelineResult result2{0, 0_s, 1_s, targets}; photon::Packet p2; p2 << result2; diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp index 65c60f0d1..5b46962c6 100644 --- a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp @@ -21,7 +21,7 @@ #include "photon/targeting/proto/PhotonPipelineResultProto.h" TEST(PhotonPipelineResultTest, Roundtrip) { - photon::PhotonPipelineResult result{12_ms, {}}; + photon::PhotonPipelineResult result{0, 0_s, 12_ms, {}}; google::protobuf::Arena arena; google::protobuf::Message* proto = @@ -62,7 +62,7 @@ TEST(PhotonPipelineResultTest, Roundtrip) { {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}}; - photon::PhotonPipelineResult result2{12_ms, targets}; + photon::PhotonPipelineResult result2{0, 0_s, 12_ms, targets}; proto = wpi::Protobuf::New(&arena); wpi::Protobuf::Pack(proto, result2); @@ -84,7 +84,7 @@ TEST(PhotonPipelineResultTest, Roundtrip) { photon::MultiTargetPNPResult multitagRes{pnpRes, {1, 2, 3, 4}}; - photon::PhotonPipelineResult result3{12_ms, targets, multitagRes}; + photon::PhotonPipelineResult result3{0, 0_s, 12_ms, targets, multitagRes}; proto = wpi::Protobuf::New(&arena); wpi::Protobuf::Pack(proto, result3);