From 61040443fa16386a84162c42a11b48c6ef5b24e8 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Wed, 27 Mar 2024 21:47:37 -0400 Subject: [PATCH] fix cpp --- .../simulation/PhotonCameraSim.java | 7 ++-- .../main/native/cpp/photon/PhotonCamera.cpp | 4 ++- .../photon/simulation/PhotonCameraSim.h | 8 +++-- .../photon/simulation/SimCameraProperties.h | 12 +++---- .../photon/simulation/SimPhotonCamera.h | 2 +- .../photonvision/PhotonPoseEstimatorTest.java | 2 +- .../native/cpp/PhotonPoseEstimatorTest.cpp | 35 ++++++++++--------- .../photon/targeting/PhotonPipelineResult.cpp | 16 +++++---- .../proto/PhotonPipelineResultProto.cpp | 3 +- .../photon/targeting/PhotonPipelineResult.h | 13 +++---- .../src/test/native/cpp/PacketTest.cpp | 2 +- 11 files changed, 59 insertions(+), 45 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index bad3d4db35..d7df0a44ef 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -541,10 +541,13 @@ public PhotonPipelineResult process( // put this simulated data to NT var now = RobotController.getFPGATime(); - int seq = 0; var ret = new PhotonPipelineResult( - seq, now - (long) (latencyMillis * 1000), now, detectableTgts, multitagResult); + heartbeatCounter, + now - (long) (latencyMillis * 1000), + now, + detectableTgts, + multitagResult); ret.setRecieveTimestampMicros(now); return ret; } diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index a8f6e47c71..94225d175e 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -115,7 +116,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { PhotonPipelineResult result; // Fill the packet with latest data and populate result. - units::microsecond_t now = frc::RobotController::GetFPGATime(); + units::microsecond_t now = + units::microsecond_t(frc::RobotController::GetFPGATime()); const auto value = rawBytesEntry.Get(); if (!value.size()) return result; diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index adcd9558c6..196bd26ff9 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -369,7 +370,10 @@ class PhotonCameraSim { multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; } - return PhotonPipelineResult{latency, detectableTgts, multiTagResults}; + units::second_t now = frc::Timer::GetFPGATimestamp(); + + return PhotonPipelineResult{heartbeatCounter, now - latency, now, + detectableTgts, multiTagResults}; } void SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); @@ -426,7 +430,7 @@ class PhotonCameraSim { PhotonCamera* cam; NTTopicSet ts{}; - uint64_t heartbeatCounter{0}; + int64_t heartbeatCounter{0}; uint64_t nextNTEntryTime{wpi::Now()}; diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h index 4632ef5d49..f3a5d73677 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -96,12 +96,12 @@ class SimCameraProperties { units::radian_t{std::numbers::pi / 2.0}}) .Radians()}}, frc::Translation3d{ - 1_m, - frc::Rotation3d{0_rad, - (GetPixelPitch(0) + frc::Rotation2d{units::radian_t{ - std::numbers::pi / 2.0}}) - .Radians(), - 0_rad}}, + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(0) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, frc::Translation3d{ 1_m, frc::Rotation3d{0_rad, (GetPixelPitch(height) + diff --git a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h index 365efebd59..c6f2e8adfc 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h @@ -90,7 +90,7 @@ class SimPhotonCamera : public PhotonCamera { latencyMillisEntry.SetDouble(latency.to()); std::sort(targetList.begin(), targetList.end(), [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); }); - PhotonPipelineResult newResult{latency, targetList}; + PhotonPipelineResult newResult{0, 0_s, latency, targetList}; Packet packet{}; packet << newResult; diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 72e94da48c..734c570777 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -596,7 +596,7 @@ void averageBestPoses() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); // 3 3 3 ambig .4 - cameraOne.result.setRecieveTimestampMicros(20); + cameraOne.result.setRecieveTimestampMicros(20 * 1000000); PhotonPoseEstimator estimator = new PhotonPoseEstimator( diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index aed60ff2fc..9835d0d723 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -83,8 +83,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(11)); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, std::move(cameraOne), {}); @@ -138,8 +138,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(17_s); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(17_s); photon::PhotonPoseEstimator estimator( aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne), @@ -181,8 +181,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(17)); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator( aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {}); @@ -225,8 +225,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(17)); + cameraOne.testResult = {0, 0_s, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {}); @@ -259,8 +259,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; - estimator.GetCamera()->testResult = {2_ms, targetsThree}; - estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21)); + estimator.GetCamera()->testResult = {0, 0_s, 2_ms, targetsThree}; + estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(21)); estimatedPose = estimator.Update(); ASSERT_TRUE(estimatedPose); @@ -300,8 +300,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {2_ms, targets}; - cameraOne.testResult.SetTimestamp(units::second_t(15)); + cameraOne.testResult = {0, 0_ms, 2_ms, targets}; + cameraOne.testResult.SetRecieveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, std::move(cameraOne), {}); @@ -347,17 +347,18 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { std::move(cameraOne), {}); // empty input, expect empty out - estimator.GetCamera()->testResult = {2_ms, {}}; - estimator.GetCamera()->testResult.SetTimestamp(units::second_t(1)); + estimator.GetCamera()->testResult = {0, 0_s, 2_ms, {}}; + estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(1)); auto estimatedPose = estimator.Update(); EXPECT_FALSE(estimatedPose); // Set result, and update -- expect present and timestamp to be 15 - estimator.GetCamera()->testResult = {3_ms, targets}; - estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15)); + estimator.GetCamera()->testResult = {0, 0_s, 3_ms, targets}; + estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(15)); estimatedPose = estimator.Update(); EXPECT_TRUE(estimatedPose); - EXPECT_NEAR(15, estimatedPose.value().timestamp.to(), 1e-6); + EXPECT_NEAR((15_s - 3_ms).to(), + estimatedPose.value().timestamp.to(), 1e-6); // And again -- now pose cache should be empty estimatedPose = estimator.Update(); diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp index 972cfb9f24..2e3367d0c6 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -39,8 +39,9 @@ bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { // Encode latency and number of targets. - packet << result.sequenceID << result.captureTimestamp.value() - << result.publishTimestamp.value() + packet << result.sequenceID + << static_cast(result.captureTimestamp.value()) + << static_cast(result.publishTimestamp.value()) << static_cast(result.targets.size()); // Encode the information of each target. @@ -74,12 +75,13 @@ Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { packet >> multitagResult; - units::microsecond_t captureTS = units::microsecond_t{static_cast(capTS)}; - units::microsecond_t publishTS = units::microsecond_t{static_cast(pubTS)}; + units::microsecond_t captureTS = + units::microsecond_t{static_cast(capTS)}; + units::microsecond_t publishTS = + units::microsecond_t{static_cast(pubTS)}; - result = PhotonPipelineResult{ - sequenceID, captureTS, publishTS, - targets, multitagResult}; + result = PhotonPipelineResult{sequenceID, captureTS, publishTS, targets, + multitagResult}; return packet; } diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp index c2b8f20313..6beeccaabc 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp @@ -43,7 +43,8 @@ wpi::Protobuf::Unpack( return photon::PhotonPipelineResult{ m->sequence_id(), units::microsecond_t{static_cast(m->capture_timestamp_micros())}, - units::microsecond_t{static_cast(m->nt_publish_timestamp_micros())}, + units::microsecond_t{ + static_cast(m->nt_publish_timestamp_micros())}, targets, wpi::UnpackProtobuf( m->multi_target_result())}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index c188f42657..a34d1df6fb 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -50,10 +50,11 @@ class PhotonPipelineResult { * @param targets The list of targets identified by the pipeline. * @param multitagResult The multitarget result. Default to empty */ - PhotonPipelineResult(int64_t sequenceID, units::microsecond_t captureTimestamp, - units::microsecond_t publishTimestamp, - std::span targets, - MultiTargetPNPResult multitagResult = {}); + PhotonPipelineResult(int64_t sequenceID, + units::microsecond_t captureTimestamp, + units::microsecond_t publishTimestamp, + std::span targets, + MultiTargetPNPResult multitagResult = {}); /** * Returns the best target in this pipeline result. If there are no targets, @@ -104,7 +105,7 @@ class PhotonPipelineResult { * The number of non-empty frames processed by this camera since boot. Useful * to checking if a camera is alive. */ - const int64_t SequenceID() const { return sequenceID; } + int64_t SequenceID() const { return sequenceID; } /** Sets the FPGA timestamp this result was recieved by robot code */ void SetRecieveTimestamp(const units::second_t timestamp) { @@ -139,7 +140,7 @@ class PhotonPipelineResult { units::microsecond_t publishTimestamp; // Since we don't trust NT time sync, keep track of when we got this packet // into robot code - units::microsecond_t ntRecieveTimestamp; + units::microsecond_t ntRecieveTimestamp = -1_s; wpi::SmallVector targets; MultiTargetPNPResult multitagResult; diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 14e18599ac..61b8bf1240 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -109,7 +109,7 @@ TEST(PacketTest, PhotonPipelineResult) { {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}}; - photon::PhotonPipelineResult result2{0, 0_s, 2_s, targets}; + photon::PhotonPipelineResult result2{0, 0_s, 1_s, targets}; photon::Packet p2; p2 << result2;