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

Use ReadQueue for PhotonCamera timestamps (#1316)

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

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -83,12 +83,16 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(11));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -138,13 +142,17 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(17_s);
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
{{0_m, 0_m, 4_m}, {}});
auto estimatedPose = estimator.Update();
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -181,14 +189,19 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -225,14 +238,19 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
std::move(cameraOne), {});
{});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -259,10 +277,14 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
estimator.GetCamera()->testResult = {0, 0_s, 2_ms, targetsThree};
estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(21));
cameraOne.testResult = {{0, 0_s, 2_ms, targetsThree}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
@@ -300,12 +322,17 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_ms, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(15));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -344,23 +371,35 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
cameraOne.test = true;
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
{});
// empty input, expect empty out
estimator.GetCamera()->testResult = {0, 0_s, 2_ms, {}};
estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(1));
auto estimatedPose = estimator.Update();
cameraOne.testResult = {{0, 0_s, 2_ms, {}}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
estimator.GetCamera()->testResult = {0, 0_s, 3_ms, targets};
estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(15));
estimatedPose = estimator.Update();
cameraOne.testResult = {{0, 0_s, 3_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- now pose cache should be empty
estimatedPose = estimator.Update();
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
}