Use readqueue for results and change ownership

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 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

@@ -45,6 +45,8 @@
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.ArrayList;
+import java.util.List;
 import java.util.Optional;
 import java.util.Set;
 import org.photonvision.common.hardware.VisionLEDMode;
@@ -131,10 +133,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);
@@ -170,22 +174,50 @@ 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();
+
+        for (var c : changes) {
+            var result = c.value;
+            result.setTimestampSeconds((c.timestamp / 1e6) - (result.getLatencyMillis() / 1e3));
+            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.setTimestampSeconds(
-                (resultSubscriber.subscriber.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);
+        // TODO: NT4 time sync is Not To Be Trusted, we should do something else?
+        result.setTimestampSeconds((ret.timestamp / 1e6) - (result.getLatencyMillis() / 1e3));
 
-        // 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.kAprilTag16h5;
     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,22 @@ 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.
+     *   <li>Strategy is multi-tag on coprocessor
      * </ul>
      *
      * @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());
     }
 
     /**

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

@@ -66,7 +66,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(
@@ -102,15 +104,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;
 
@@ -128,6 +130,37 @@ 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;
+    result.SetTimestamp(units::microsecond_t(value.time) - result.GetLatency());
+  }
+
+  return ret;
+}
+
 void PhotonCamera::SetDriverMode(bool driverMode) {
   driverModePublisher.Set(driverMode);
 }

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

@@ -66,22 +66,6 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
                                          frc::Transform3d robotToCamera)
     : aprilTags(tags),
       strategy(strat),
-      camera(nullptr),
-      m_robotToCamera(robotToCamera),
-      lastPose(frc::Pose3d()),
-      referencePose(frc::Pose3d()),
-      poseCacheTimestamp(-1_s) {
-  HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
-             InstanceCount);
-  InstanceCount++;
-}
-
-PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
-                                         PoseStrategy strat, PhotonCamera&& cam,
-                                         frc::Transform3d robotToCamera)
-    : aprilTags(tags),
-      strategy(strat),
-      camera(std::make_shared<PhotonCamera>(std::move(cam))),
       m_robotToCamera(robotToCamera),
       lastPose(frc::Pose3d()),
       referencePose(frc::Pose3d()),
@@ -106,25 +90,6 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
   multiTagFallbackStrategy = strategy;
 }
 
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
-  if (!camera) {
-    FRC_ReportError(frc::warn::Warning, "[PhotonPoseEstimator] Missing camera!",
-                    "");
-    return std::nullopt;
-  }
-  auto result = camera->GetLatestResult();
-  return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
-}
-
-std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
-    const PhotonPipelineResult& result) {
-  // If camera is null, best we can do is pass null calibration data
-  if (!camera) {
-    return Update(result, std::nullopt, std::nullopt, this->strategy);
-  }
-  return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
-}
-
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     const PhotonPipelineResult& result, std::optional<cv::Mat> cameraMatrixData,
     std::optional<cv::Mat> cameraDistCoeffs) {
@@ -399,6 +364,10 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
   }
 
   if (!camMat || !distCoeffs) {
+    FRC_ReportError(frc::warn::Warning,
+                    "No camera calibration data provided to "
+                    "PhotonPoseEstimator::MultiTagOnRioStrategy!",
+                    "");
     return Update(result, std::nullopt, std::nullopt,
                   this->multiTagFallbackStrategy);
   }