diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 8b5148cf21..cdf1b8610c 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -68,7 +68,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, rootTable->GetRawTopic("rawBytes") .Subscribe( "rawBytes", {}, - {.pollStorage = 1, .periodic = 0.01, .sendAll = true})), + {.pollStorage = 20, .periodic = 0.01, .sendAll = true})), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber( @@ -102,7 +102,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} -PhotonPipelineResult PhotonCamera::GetLatestResult() { +std::vector PhotonCamera::GetAllUnreadResult() { if (test) { return testResult; } @@ -114,25 +114,27 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { packet.Clear(); // Create the new result; - PhotonPipelineResult result; + std::vector ret; - // Fill the packet with latest data and populate result. const auto changes = rawBytesEntry.ReadQueue(); - if (!changes.size()) { - return result; - } - const nt::Timestamped> value = changes[0]; - if (!value.value.size() || value.time == 0) { - return result; - } + ret.reserve(changes.size()); - photon::Packet packet{value.value}; + for (const nt::Timestamped>& value : changes) { + if (!value.value.size() || value.time == 0) { + continue; + } - packet >> result; + // Fill the packet with latest data and populate result. + photon::Packet packet{value.value}; - result.SetTimestamp(units::microsecond_t(value.time) - result.GetLatency()); + PhotonPipelineResult result; + packet >> result; + result.SetTimestamp(units::microsecond_t(value.time) - result.GetLatency()); + + ret.emplace_back(std::move(result)); + } - return result; + return ret; } void PhotonCamera::SetDriverMode(bool driverMode) { diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index d14394b999..745f35a4eb 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -112,8 +112,16 @@ std::optional PhotonPoseEstimator::Update() { ""); return std::nullopt; } - auto result = camera->GetLatestResult(); - return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs()); + + auto results = camera->GetAllUnreadResult(); + + std::optional ret; + + for (const auto& result : results) { + ret = Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs()); + } + + return ret; } std::optional PhotonPoseEstimator::Update( diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index ef839c0688..0b10023ec6 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -51,7 +51,7 @@ PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera, } bool PhotonCameraSim::CanSeeTargetPose(const frc::Pose3d& camPose, - const VisionTargetSim& target) { + const VisionTargetSim& target) { CameraTargetRelation rel{camPose, target.GetPose()}; return ((units::math::abs(rel.camToTargYaw.Degrees()) < prop.GetHorizFOV().Degrees() / 2.0) && @@ -94,9 +94,9 @@ std::optional PhotonCameraSim::ConsumeNextEntryTime() { return std::nullopt; } } -PhotonPipelineResult PhotonCameraSim::Process(units::second_t latency, - const frc::Pose3d& cameraPose, - std::vector targets) { +PhotonPipelineResult PhotonCameraSim::Process( + units::second_t latency, const frc::Pose3d& cameraPose, + std::vector targets) { std::sort(targets.begin(), targets.end(), [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) { units::meter_t dist1 = @@ -332,7 +332,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result, - uint64_t recieveTimestamp) { + uint64_t recieveTimestamp) { ts.latencyMillisEntry.Set( result.GetLatency().convert().to(), recieveTimestamp); diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index d9cf2574e6..e99a331e09 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -74,13 +75,24 @@ class PhotonCamera { PhotonCamera(PhotonCamera&&) = default; - virtual ~PhotonCamera() = default; + ~PhotonCamera() = default; /** - * Returns the latest pipeline result. - * @return The latest pipeline result. + * The list of pipeline results sent by PhotonVision since the last call to + * GetAllUnreadResult(). Calling this function clears the internal FIFO queue, + * and multiple calls to GetAllUnreadResult() will return different + * (potentially empty) result arrays. Be careful to call this exactly ONCE per + * loop of your robot code! FIFO depth is limited to 20 changes, so make sure + * to call this frequently enough to avoid old results being discarded, too! */ - virtual PhotonPipelineResult GetLatestResult(); + std::vector GetAllUnreadResult(); + + [[deprecated("Replace with GetAllUnreadResult")]] + inline const PhotonPipelineResult GetLatestResult() { + const auto ret = GetAllUnreadResult(); + if (!ret.size()) return PhotonPipelineResult(); + return ret.back(); + } /** * Toggles driver mode. @@ -159,7 +171,7 @@ class PhotonCamera { // For use in tests bool test = false; - PhotonPipelineResult testResult; + std::vector testResult; protected: std::shared_ptr mainTable; diff --git a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h deleted file mode 100644 index 365efebd59..0000000000 --- a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h +++ /dev/null @@ -1,137 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "photon/PhotonCamera.h" -#include "photon/PhotonTargetSortMode.h" -#include "photon/dataflow/structures/Packet.h" -#include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" -#include "photon/targeting/PhotonPipelineResult.h" -#include "photon/targeting/PhotonTrackedTarget.h" - -namespace photon { -class SimPhotonCamera : public PhotonCamera { - public: - SimPhotonCamera(nt::NetworkTableInstance instance, - const std::string& cameraName) - : PhotonCamera(instance, cameraName) { - latencyMillisEntry = rootTable->GetEntry("latencyMillis"); - hasTargetEntry = rootTable->GetEntry("hasTargetEntry"); - targetPitchEntry = rootTable->GetEntry("targetPitchEntry"); - targetYawEntry = rootTable->GetEntry("targetYawEntry"); - targetAreaEntry = rootTable->GetEntry("targetAreaEntry"); - targetSkewEntry = rootTable->GetEntry("targetSkewEntry"); - targetPoseEntry = rootTable->GetEntry("targetPoseEntry"); - rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes"); - versionEntry = instance.GetTable("photonvision")->GetEntry("version"); - // versionEntry.SetString(PhotonVersion.versionString); - } - - explicit SimPhotonCamera(const std::string& cameraName) - : SimPhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} - - virtual ~SimPhotonCamera() = default; - - /** - * Simulate one processed frame of vision data, putting one result to NT. - * - * @param latency Latency of the provided frame - * @param targetList List of targets detected - */ - void SubmitProcessedFrame(units::millisecond_t latency, - std::vector targetList) { - SubmitProcessedFrame(latency, PhotonTargetSortMode::LeftMost(), targetList); - } - - /** - * Simulate one processed frame of vision data, putting one result to NT. - * - * @param latency Latency of the provided frame - * @param sortMode Order in which to sort targets - * @param targetList List of targets detected - */ - void SubmitProcessedFrame( - units::millisecond_t latency, - std::function - sortMode, - std::vector targetList) { - latencyMillisEntry.SetDouble(latency.to()); - std::sort(targetList.begin(), targetList.end(), - [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); }); - PhotonPipelineResult newResult{latency, targetList}; - Packet packet{}; - packet << newResult; - - rawBytesPublisher.Set( - std::span{packet.GetData().data(), packet.GetDataSize()}); - - bool hasTargets = newResult.HasTargets(); - hasTargetEntry.SetBoolean(hasTargets); - if (!hasTargets) { - targetPitchEntry.SetDouble(0.0); - targetYawEntry.SetDouble(0.0); - targetAreaEntry.SetDouble(0.0); - targetPoseEntry.SetDoubleArray( - std::vector{0.0, 0.0, 0.0, 0, 0, 0, 0}); - targetSkewEntry.SetDouble(0.0); - } else { - PhotonTrackedTarget bestTarget = newResult.GetBestTarget(); - targetPitchEntry.SetDouble(bestTarget.GetPitch()); - targetYawEntry.SetDouble(bestTarget.GetYaw()); - targetAreaEntry.SetDouble(bestTarget.GetArea()); - targetSkewEntry.SetDouble(bestTarget.GetSkew()); - - frc::Transform3d transform = bestTarget.GetBestCameraToTarget(); - targetPoseEntry.SetDoubleArray(std::vector{ - transform.X().to(), transform.Y().to(), - transform.Z().to(), transform.Rotation().GetQuaternion().W(), - transform.Rotation().GetQuaternion().X(), - transform.Rotation().GetQuaternion().Y(), - transform.Rotation().GetQuaternion().Z()}); - } - } - - private: - nt::NetworkTableEntry latencyMillisEntry; - nt::NetworkTableEntry hasTargetEntry; - nt::NetworkTableEntry targetPitchEntry; - nt::NetworkTableEntry targetYawEntry; - nt::NetworkTableEntry targetAreaEntry; - nt::NetworkTableEntry targetSkewEntry; - nt::NetworkTableEntry targetPoseEntry; - nt::NetworkTableEntry versionEntry; - nt::RawPublisher rawBytesPublisher; -}; -} // namespace photon diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index aed60ff2fc..d3910aa88a 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 = {{2_ms, targets}}; + cameraOne.testResult[0].SetTimestamp(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 = {{2_ms, targets}}; + cameraOne.testResult[0].SetTimestamp(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 = {{2_ms, targets}}; + cameraOne.testResult[0].SetTimestamp(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 = {{2_ms, targets}}; + cameraOne.testResult[0].SetTimestamp(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 = {{2_ms, targetsThree}}; + estimator.GetCamera()->testResult[0].SetTimestamp(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 = {{2_ms, targets}}; + cameraOne.testResult[0].SetTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, std::move(cameraOne), {}); @@ -347,14 +347,14 @@ 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 = {{2_ms, {}}}; + estimator.GetCamera()->testResult[0].SetTimestamp(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 = {{3_ms, targets}}; + estimator.GetCamera()->testResult[0].SetTimestamp(units::second_t(15)); estimatedPose = estimator.Update(); EXPECT_TRUE(estimatedPose); EXPECT_NEAR(15, estimatedPose.value().timestamp.to(), 1e-6);