Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Kevin Cooney
2025-08-07 22:14:44 -07:00
committed by GitHub
parent a930852bee
commit ab854e91e5
8 changed files with 164 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -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) {