[photon-lib] Invalidate pose cache when setting referencePose (#2040)

photon-lib/py/buildAndTest.sh

@@ -1,7 +1,13 @@
+#!/usr/bin/env bash
+
+set -euo pipefail
+cd -- "$(dirname -- "$0")"
+
 # Uninstall if it already was installed
 python3 -m pip uninstall -y photonlibpy
 
 # Build wheel
+python3 -m pip install wheel
 python3 setup.py bdist_wheel
 
 # Install whatever wheel was made

photon-lib/py/photonlibpy/photonPoseEstimator.py

@@ -229,7 +229,7 @@ class PhotonPoseEstimator:
         self._poseCacheTimestampSeconds = -1.0
 
     def _checkUpdate(self, oldObj, newObj) -> None:
-        if oldObj != newObj and oldObj is not None and oldObj is not newObj:
+        if oldObj != newObj:
             self._invalidatePoseCache()
 
     def addHeadingData(

photon-lib/py/test/photonPoseEstimator_test.py

@@ -15,6 +15,8 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
+from test import testUtil
+
 import wpimath.units
 from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
 from photonlibpy.estimation import TargetModel
@@ -191,7 +193,7 @@ def test_pnpDistanceTrigSolve():
     assert bestTarget.fiducialId == 0
     assert result.ntReceiveTimestampMicros > 0
     # Make test independent of the FPGA time.
-    result.ntReceiveTimestampMicros = fakeTimestampSecs * 1e6
+    result.ntReceiveTimestampMicros = int(fakeTimestampSecs * 1e6)
 
     estimator.addHeadingData(
         result.getTimestampSeconds(), realPose.rotation().toRotation2d()
@@ -217,7 +219,7 @@ def test_pnpDistanceTrigSolve():
     assert bestTarget.fiducialId == 0
     assert result.ntReceiveTimestampMicros > 0
     # Make test independent of the FPGA time.
-    result.ntReceiveTimestampMicros = fakeTimestampSecs * 1e6
+    result.ntReceiveTimestampMicros = int(fakeTimestampSecs * 1e6)
 
     estimator.addHeadingData(
         result.getTimestampSeconds(), realPose.rotation().toRotation2d()
@@ -289,8 +291,36 @@ def test_multiTagOnCoprocStrategy():
 def test_cacheIsInvalidated():
     aprilTags = fakeAprilTagFieldLayout()
     cameraOne = PhotonCameraInjector()
+
+    estimator = PhotonPoseEstimator(
+        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
+    )
+
+    # Initial state, expect no timestamp.
+    assertEquals(-1, estimator._poseCacheTimestampSeconds)
+
+    # First result is 17s after epoch start.
+    timestamps = testUtil.PipelineTimestamps(captureTimestampMicros=17_000_000)
+    latencySecs = timestamps.pipelineLatencySecs()
+
+    # No targets, expect empty result
+    cameraOne.result = PhotonPipelineResult(
+        timestamps.receiveTimestampMicros(),
+        metadata=timestamps.toPhotonPipelineMetadata(),
+    )
+    estimatedPose = estimator.update()
+
+    assert estimatedPose is None
+    assertEquals(
+        timestamps.receiveTimestampMicros() * 1e-6 - latencySecs,
+        estimator._poseCacheTimestampSeconds,
+        1e-3,
+    )
+
+    # Set actual result
+    timestamps.incrementTimeMicros(2_500_000)
     result = PhotonPipelineResult(
-        int(20 * 1e6),
+        timestamps.receiveTimestampMicros(),
         [
             PhotonTrackedTarget(
                 3.0,
@@ -315,31 +345,21 @@ def test_cacheIsInvalidated():
                 0.7,
             )
         ],
-        metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
+        metadata=timestamps.toPhotonPipelineMetadata(),
     )
-
-    estimator = PhotonPoseEstimator(
-        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
-    )
-
-    # Empty result, expect empty result
-    cameraOne.result = PhotonPipelineResult(0)
-    estimatedPose = estimator.update()
-    assert estimatedPose is None
-
-    # Set actual result
     cameraOne.result = result
     estimatedPose = estimator.update()
     assert estimatedPose is not None
-    assertEquals(20, estimatedPose.timestampSeconds, 0.01)
-    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    expectedTimestamp = timestamps.receiveTimestampMicros() * 1e-6 - latencySecs
+    assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
+    assertEquals(expectedTimestamp, 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 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
 
     # Set new field layout -- right after, the pose cache timestamp should be -1
     estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
@@ -350,8 +370,14 @@ def test_cacheIsInvalidated():
 
     assert estimatedPose is not None
 
-    assertEquals(20, estimatedPose.timestampSeconds, 0.01)
-    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
+    assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
+
+    # Setting a value from None to a non-None should invalidate the cache.
+    assert estimator.referencePose is None
+    estimator.referencePose = Pose3d(3, 3, 3, Rotation3d())
+
+    assertEquals(-1, estimator._poseCacheTimestampSeconds)
 
 
 def assertEquals(expected, actual, epsilon=0.0):

photon-lib/py/test/testUtil.py

@@ -0,0 +1,65 @@
+"""Test utilities."""
+
+from photonlibpy.targeting import PhotonPipelineMetadata
+
+
+class InvalidTestDataException(ValueError):
+    pass
+
+
+class PipelineTimestamps:
+    """Helper class to ensure timestamps are positive."""
+
+    def __init__(
+        self,
+        *,
+        captureTimestampMicros: int,
+        pipelineLatencyMicros=2_000,
+        receiveLatencyMicros=1_000,
+    ):
+        if captureTimestampMicros < 0:
+            raise InvalidTestDataException("captureTimestampMicros cannot be negative")
+        if pipelineLatencyMicros <= 0:
+            raise InvalidTestDataException("pipelineLatencyMicros must be positive")
+        if receiveLatencyMicros < 0:
+            raise InvalidTestDataException("receiveLatencyMicros cannot be negative")
+        self._captureTimestampMicros = captureTimestampMicros
+        self._pipelineLatencyMicros = pipelineLatencyMicros
+        self._receiveLatencyMicros = receiveLatencyMicros
+        self._sequenceID = 0
+
+    @property
+    def captureTimestampMicros(self) -> int:
+        return self._captureTimestampMicros
+
+    @captureTimestampMicros.setter
+    def captureTimestampMicros(self, micros: int) -> None:
+        if micros < 0:
+            raise InvalidTestDataException("captureTimestampMicros cannot be negative")
+        if micros < self._captureTimestampMicros:
+            raise InvalidTestDataException("time cannot go backwards")
+        self._captureTimestampMicros = micros
+        self._sequenceID += 1
+
+    @property
+    def pipelineLatencyMicros(self) -> int:
+        return self._pipelineLatencyMicros
+
+    def pipelineLatencySecs(self) -> float:
+        return self.pipelineLatencyMicros * 1e-6
+
+    def incrementTimeMicros(self, micros: int) -> None:
+        self.captureTimestampMicros += micros
+
+    def publishTimestampMicros(self) -> int:
+        return self._captureTimestampMicros + self.pipelineLatencyMicros
+
+    def receiveTimestampMicros(self) -> int:
+        return self.publishTimestampMicros() + self._receiveLatencyMicros
+
+    def toPhotonPipelineMetadata(self) -> PhotonPipelineMetadata:
+        return PhotonPipelineMetadata(
+            captureTimestampMicros=self.captureTimestampMicros,
+            publishTimestampMicros=self.publishTimestampMicros(),
+            sequenceID=self._sequenceID,
+        )

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

@@ -42,11 +42,7 @@ import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.numbers.N8;
 import edu.wpi.first.wpilibj.DriverStation;
-import java.util.ArrayList;
-import java.util.HashSet;
-import java.util.List;
-import java.util.Optional;
-import java.util.Set;
+import java.util.*;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.targeting.PhotonPipelineResult;
@@ -175,7 +171,7 @@ public class PhotonPoseEstimator {
     }
 
     private void checkUpdate(Object oldObj, Object newObj) {
-        if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) {
+        if (!Objects.equals(oldObj, newObj)) {
             invalidatePoseCache();
         }
     }

photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java

@@ -28,6 +28,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
@@ -38,11 +39,13 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import java.io.IOException;
@@ -592,8 +595,8 @@ class PhotonPoseEstimatorTest {
         var result =
                 new PhotonPipelineResult(
                         0,
-                        20000000,
-                        1100000,
+                        20_000_000,
+                        1_100_000,
                         1024,
                         List.of(
                                 new PhotonTrackedTarget(
@@ -624,6 +627,9 @@ class PhotonPoseEstimatorTest {
                         PoseStrategy.AVERAGE_BEST_TARGETS,
                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
 
+        // Initial state, expect no timestamp
+        assertEquals(-1, estimator.poseCacheTimestampSeconds);
+
         // Empty result, expect empty result
         cameraOne.result = new PhotonPipelineResult();
         cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
@@ -652,6 +658,12 @@ class PhotonPoseEstimatorTest {
         estimatedPose = estimator.update(cameraOne.result);
         assertEquals(20, estimatedPose.get().timestampSeconds, .01);
         assertEquals(20, estimator.poseCacheTimestampSeconds);
+
+        // Setting a value from None to a non-None should invalidate the cache
+        assertNull(estimator.getReferencePose());
+        assertEquals(20, estimator.poseCacheTimestampSeconds);
+        estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
+        assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
     }
 
     @Test

photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp

@@ -412,12 +412,41 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
   EXPECT_NEAR((15_s - 3_ms).to<double>(),
               estimatedPose.value().timestamp.to<double>(), 1e-6);
 
-  // And again -- now pose cache should be empty
+  // And again -- pose cache should result in returning std::nullopt
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
     estimatedPose = estimator.Update(result);
   }
 
   EXPECT_FALSE(estimatedPose);
+
+  // If the camera produces a result that is > 1 micro second later,
+  // the pose cache should not be hit.
+  cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  EXPECT_NEAR((16_s - 3_ms).to<double>(),
+              estimatedPose.value().timestamp.to<double>(), 1e-6);
+
+  // And again -- pose cache should result in returning std::nullopt
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  EXPECT_FALSE(estimatedPose);
+
+  // Setting ReferencePose should also clear the cache
+  estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
+                                         units::meter_t(3), frc::Rotation3d()));
+
+  for (const auto& result : cameraOne.GetAllUnreadResults()) {
+    estimatedPose = estimator.Update(result);
+  }
+
+  ASSERT_TRUE(estimatedPose);
+  EXPECT_NEAR((16_s - 3_ms).to<double>(),
+              estimatedPose.value().timestamp.to<double>(), 1e-6);
 }
 
 TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {