diff --git a/photon-lib/py/buildAndTest.sh b/photon-lib/py/buildAndTest.sh
index 0916b41327..f8c71654f1 100755
--- a/photon-lib/py/buildAndTest.sh
+++ b/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
diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py
index 3c735981d2..00cb0fbd2a 100644
--- a/photon-lib/py/photonlibpy/photonPoseEstimator.py
+++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py
@@ -229,7 +229,7 @@ def _invalidatePoseCache(self) -> None:
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(
diff --git a/photon-lib/py/test/__init__.py b/photon-lib/py/test/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py
index 3c0dc67d41..4937b626ab 100644
--- a/photon-lib/py/test/photonPoseEstimator_test.py
+++ b/photon-lib/py/test/photonPoseEstimator_test.py
@@ -15,6 +15,8 @@
## along with this program. If not, see .
###############################################################################
+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),
- )
-
- estimator = PhotonPoseEstimator(
- aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
+ metadata=timestamps.toPhotonPipelineMetadata(),
)
-
- # 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):
diff --git a/photon-lib/py/test/testUtil.py b/photon-lib/py/test/testUtil.py
new file mode 100644
index 0000000000..d2be20ebee
--- /dev/null
+++ b/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,
+ )
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
index 6cd76c7e06..a9330f6102 100644
--- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -42,11 +42,7 @@
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 @@ private void invalidatePoseCache() {
}
private void checkUpdate(Object oldObj, Object newObj) {
- if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) {
+ if (!Objects.equals(oldObj, newObj)) {
invalidatePoseCache();
}
}
diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
index c95d452bc3..41d17df998 100644
--- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
+++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -28,6 +28,7 @@
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.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 @@ void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
- 20000000,
- 1100000,
+ 20_000_000,
+ 1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
@@ -624,6 +627,9 @@ void cacheIsInvalidated() {
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 @@ void cacheIsInvalidated() {
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
diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
index c95fa57ed0..a4922c61d2 100644
--- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
+++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
@@ -412,12 +412,41 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_NEAR((15_s - 3_ms).to(),
estimatedPose.value().timestamp.to(), 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(),
+ estimatedPose.value().timestamp.to(), 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(),
+ estimatedPose.value().timestamp.to(), 1e-6);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {