Use ReadQueue for PhotonCamera timestamps (PhotonVision#1316)

This removes the extra GetLastChange call to keep everything properly
atomic.

Closes PhotonVision#1303

photon-lib/build.gradle

@@ -53,6 +53,8 @@ model {
             nativeUtils.useRequiredLibrary(it, "wpilib_shared")
             nativeUtils.useRequiredLibrary(it, "apriltag_shared")
             nativeUtils.useRequiredLibrary(it, "opencv_shared")
+            nativeUtils.useRequiredLibrary(it, "cscore_shared")
+            nativeUtils.useRequiredLibrary(it, "cameraserver_shared")
         }
     }
     testSuites {

photon-lib/py/photonlibpy/photonCamera.py

@@ -1,4 +1,5 @@
 from enum import Enum
+from typing import List
 import ntcore
 from wpilib import RobotController, Timer
 import wpilib
@@ -75,6 +76,39 @@ def __init__(self, cameraName: str):
         self._prevHeartbeat = 0
         self._prevHeartbeatChangeTime = Timer.getFPGATimestamp()
 
+    def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
+        """
+        The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
+        Calling this function clears the internal FIFO queue, and multiple calls to
+        getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
+        call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
+        make sure to call this frequently enough to avoid old results being discarded, too!
+        """
+
+        self._versionCheck()
+
+        changes = self._rawBytesEntry.readQueue()
+
+        ret = []
+
+        for change in changes:
+            byteList = change.value
+            timestamp = change.time
+
+            if len(byteList) < 1:
+                pass
+            else:
+                newResult = PhotonPipelineResult()
+                pkt = Packet(byteList)
+                newResult.populateFromPacket(pkt)
+                # NT4 allows us to correct the timestamp based on when the message was sent
+                newResult.setTimestampSeconds(
+                    timestamp / 1e6 - newResult.getLatencyMillis() / 1e3
+                )
+                ret.append(newResult)
+
+        return ret
+
     def getLatestResult(self) -> PhotonPipelineResult:
         self._versionCheck()
 

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -44,8 +44,8 @@
 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.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 import java.util.Optional;
@@ -137,10 +137,12 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
                 cameraTable
                         .getRawTopic("rawBytes")
                         .subscribe(
-                                "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
-        resultSubscriber =
-                new PacketSubscriber<>(
-                        rawBytesEntry, PhotonPipelineResult.serde, new PhotonPipelineResult());
+                                "rawBytes",
+                                new byte[] {},
+                                PubSubOption.periodic(0.01),
+                                PubSubOption.sendAll(true),
+                                PubSubOption.pollStorage(20));
+        resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.serde);
         driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
         driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
         inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
@@ -176,21 +178,52 @@ public PhotonCamera(String cameraName) {
     }
 
     /**
-     * Returns the latest pipeline result.
+     * The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
+     * Calling this function clears the internal FIFO queue, and multiple calls to
+     * getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
+     * call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
+     * make sure to call this frequently enough to avoid old results being discarded, too!
+     */
+    public List<PhotonPipelineResult> getAllUnreadResults() {
+        List<PhotonPipelineResult> ret = new ArrayList<>();
+
+        var changes = resultSubscriber.getAllChanges();
+
+        // TODO: NT4 timestamps are still not to be trusted. But it's the best we can do until we can
+        // make time sync more reliable.
+        for (var c : changes) {
+            var result = c.value;
+            result.setRecieveTimestampMicros(c.timestamp);
+            ret.add(result);
+        }
+
+        return ret;
+    }
+
+    /**
+     * Returns the latest pipeline result. This is simply the most recent result recieved via NT.
+     * Calling this multiple times will always return the most recent result.
      *
-     * @return The latest pipeline result.
+     * <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss
+     * results, or provide duplicate ones!
      */
+    @Deprecated(since = "2024", forRemoval = true)
     public PhotonPipelineResult getLatestResult() {
         verifyVersion();
 
         var ret = resultSubscriber.get();
 
-        // Set the timestamp of the result.
+        if (ret.timestamp == 0) return new PhotonPipelineResult();
+
+        var result = ret.value;
+
+        // Set the timestamp of the result. Since PacketSubscriber doesn't realize that the result
+        // contains a thing with time knowledge, set it here.
         // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
-        ret.setRecieveTimestampMicros(RobotController.getFPGATime());
+        // TODO: NT4 time sync is Not To Be Trusted, we should do something else?
+        result.setRecieveTimestampMicros(ret.timestamp);
 
-        // Return result.
-        return ret;
+        return result;
     }
 
     /**

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -90,7 +90,6 @@ public enum PoseStrategy {
     private TargetModel tagModel = TargetModel.kAprilTag36h11;
     private PoseStrategy primaryStrategy;
     private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
-    private final PhotonCamera camera;
     private Transform3d robotToCamera;
 
     private Pose3d lastPose;
@@ -107,31 +106,21 @@ public enum PoseStrategy {
      *     Coordinate System</a>. Note that setting the origin of this layout object will affect the
      *     results from this class.
      * @param strategy The strategy it should use to determine the best pose.
-     * @param camera PhotonCamera
      * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
      *     robot ➔ camera) in the <a href=
      *     "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
      *     Coordinate System</a>.
      */
     public PhotonPoseEstimator(
-            AprilTagFieldLayout fieldTags,
-            PoseStrategy strategy,
-            PhotonCamera camera,
-            Transform3d robotToCamera) {
+            AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
         this.fieldTags = fieldTags;
         this.primaryStrategy = strategy;
-        this.camera = camera;
         this.robotToCamera = robotToCamera;
 
         HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
         InstanceCount++;
     }
 
-    public PhotonPoseEstimator(
-            AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
-        this(fieldTags, strategy, null, robotToCamera);
-    }
-
     /** Invalidates the pose cache. */
     private void invalidatePoseCache() {
         poseCacheTimestampSeconds = -1;
@@ -288,45 +277,24 @@ public void setRobotToCameraTransform(Transform3d robotToCamera) {
     }
 
     /**
-     * Poll data from the configured cameras and update the estimated position of the robot. Returns
-     * empty if:
-     *
-     * <ul>
-     *   <li>New data has not been received since the last call to {@code update()}.
-     *   <li>No targets were found from the camera
-     *   <li>There is no camera set
-     * </ul>
-     *
-     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
-     */
-    public Optional<EstimatedRobotPose> update() {
-        if (camera == null) {
-            DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false);
-            return Optional.empty();
-        }
-
-        PhotonPipelineResult cameraResult = camera.getLatestResult();
-
-        return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
-    }
-
-    /**
-     * Updates the estimated position of the robot. Returns empty if:
+     * Updates the estimated position of the robot, assuming no camera calibration is required for the
+     * selected strategy. Returns empty if:
      *
      * <ul>
      *   <li>The timestamp of the provided pipeline result is the same as in the previous call to
      *       {@code update()}.
      *   <li>No targets were found in the pipeline results.
      * </ul>
      *
+     * Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not
+     * provided
+     *
      * @param cameraResult The latest pipeline result from the camera
      * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
      *     create the estimate.
      */
     public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
-        if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty());
-        return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
+        return update(cameraResult, Optional.empty(), Optional.empty());
     }
 
     /**
@@ -338,10 +306,10 @@ public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
      *   <li>No targets were found in the pipeline results.
      * </ul>
      *
-     * @param cameraMatrix Camera calibration data that can be used in the case of no assigned
-     *     PhotonCamera.
-     * @param distCoeffs Camera calibration data that can be used in the case of no assigned
-     *     PhotonCamera
+     * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
+     *     otherwise
+     * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
+     *     otherwise
      * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
      *     create the estimate.
      */
@@ -378,7 +346,7 @@ private Optional<EstimatedRobotPose> update(
             Optional<Matrix<N3, N3>> cameraMatrix,
             Optional<Matrix<N8, N1>> distCoeffs,
             PoseStrategy strat) {
-        Optional<EstimatedRobotPose> estimatedPose;
+        Optional<EstimatedRobotPose> estimatedPose = Optional.empty();
         switch (strat) {
             case LOWEST_AMBIGUITY:
                 estimatedPose = lowestAmbiguityStrategy(cameraResult);
@@ -397,10 +365,20 @@ private Optional<EstimatedRobotPose> update(
                 estimatedPose = averageBestTargetsStrategy(cameraResult);
                 break;
             case MULTI_TAG_PNP_ON_RIO:
-                estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
+                if (cameraMatrix.isEmpty()) {
+                    DriverStation.reportWarning(
+                            "Camera matrix is empty for multi-tag-on-rio",
+                            Thread.currentThread().getStackTrace());
+                } else if (distCoeffs.isEmpty()) {
+                    DriverStation.reportWarning(
+                            "Camera matrix is empty for multi-tag-on-rio",
+                            Thread.currentThread().getStackTrace());
+                } else {
+                    estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
+                }
                 break;
             case MULTI_TAG_PNP_ON_COPROCESSOR:
-                estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
+                estimatedPose = multiTagOnCoprocStrategy(cameraResult);
                 break;
             default:
                 DriverStation.reportError(
@@ -415,10 +393,7 @@ private Optional<EstimatedRobotPose> update(
         return estimatedPose;
     }
 
-    private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
-            PhotonPipelineResult result,
-            Optional<Matrix<N3, N3>> cameraMatrixOpt,
-            Optional<Matrix<N8, N1>> distCoeffsOpt) {
+    private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
         if (result.getMultiTagResult().estimatedPose.isPresent) {
             var best_tf = result.getMultiTagResult().estimatedPose.best;
             var best =
@@ -433,7 +408,8 @@ private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
                             result.getTargets(),
                             PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
         } else {
-            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+            // We can nver fall back on another multitag strategy
+            return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
         }
     }
 

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -81,7 +81,7 @@ public class PhotonCameraSim implements AutoCloseable {
     private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
 
     private final AprilTagFieldLayout tagLayout =
-            AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+            AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
     // video stream simulation
     private final CvSource videoSimRaw;

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -74,7 +74,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
       rootTable(mainTable->GetSubTable(cameraName)),
       rawBytesEntry(
           rootTable->GetRawTopic("rawBytes")
-              .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})),
+              .Subscribe(
+                  "rawBytes", {},
+                  {.pollStorage = 20, .periodic = 0.01, .sendAll = true})),
       inputSaveImgEntry(
           rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
       inputSaveImgSubscriber(
@@ -110,15 +112,15 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
   if (test) {
-    return testResult;
+    if (testResult.size())
+      return testResult.back();
+    else
+      return PhotonPipelineResult{};
   }
 
   // Prints warning if not connected
   VerifyVersion();
 
-  // Clear the current packet.
-  packet.Clear();
-
   // Create the new result;
   PhotonPipelineResult result;
 
@@ -137,6 +139,40 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   return result;
 }
 
+std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
+  if (test) {
+    return testResult;
+  }
+
+  // Prints warning if not connected
+  VerifyVersion();
+
+  const auto changes = rawBytesEntry.ReadQueue();
+
+  // Create the new result list -- these will be updated in-place
+  std::vector<PhotonPipelineResult> ret(changes.size());
+
+  for (size_t i = 0; i < changes.size(); i++) {
+    const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
+
+    if (!value.value.size() || value.time == 0) {
+      continue;
+    }
+
+    // Fill the packet with latest data and populate result.
+    photon::Packet packet{value.value};
+
+    PhotonPipelineResult& result = ret[i];
+    packet >> result;
+    // TODO: NT4 timestamps are still not to be trusted. But it's the best we
+    // can do until we can make time sync more reliable.
+    result.SetRecieveTimestamp(units::microsecond_t(value.time) -
+                               result.GetLatency());
+  }
+
+  return ret;
+}
+
 void PhotonCamera::SetDriverMode(bool driverMode) {
   driverModePublisher.Set(driverMode);
 }