cpp

Signed-off-by: Jade Turner <[email protected]>
PhotonVision · Jan 15, 2025 · 179e315 · 179e315
1 parent 70de697
commit 179e315
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 15 deletions.
diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp
@@ -36,15 +36,6 @@ void Robot::RobotPeriodic() {
   drivetrain.Periodic();
   vision.Periodic();
 
-  auto visionEst = vision.GetEstimatedGlobalPose();
-  if (visionEst.has_value()) {
-    auto est = visionEst.value();
-    auto estPose = est.estimatedPose.ToPose2d();
-    auto estStdDevs = vision.GetEstimationStdDevs(estPose);
-    drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp,
-                                    estStdDevs);
-  }
-
   drivetrain.Log();
 }
 

diff --git a/photonlib-cpp-examples/poseest/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h
@@ -31,6 +31,7 @@
 #include <photon/simulation/VisionTargetSim.h>
 #include <photon/targeting/PhotonPipelineResult.h>
 
+#include <functional>
 #include <limits>
 #include <memory>
 
@@ -41,7 +42,10 @@
 
 class Vision {
  public:
-  Vision() {
+  Vision(std::function<void(frc::Pose2d, units::second_t,
+                            Eigen::Matrix<double, 3, 1>)>
+             visionConsumer)
+      : visionConsumer{visionConsumer} {
     photonEstimator.SetMultiTagFallbackStrategy(
         photon::PoseStrategy::LOWEST_AMBIGUITY);
 
@@ -68,9 +72,7 @@ class Vision {
 
   photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
 
-  std::optional<photon::EstimatedRobotPose> GetEstimatedGlobalPose() {
-    std::optional<photon::EstimatedRobotPose> visionEst;
-
+  void Periodic() {
     // Run each new pipeline result through our pose estimator
     for (const auto& result : camera.GetAllUnreadResults()) {
       // cache result and update pose estimator
@@ -87,9 +89,13 @@ class Vision {
           GetSimDebugField().GetObject("VisionEstimation")->SetPoses({});
         }
       }
-    }
 
-    return visionEst;
+      if (visionEst) {
+        visionConsumer(
+            visionEst->estimatedPose.ToPose2d(), visionEst->estimate,
+            GetEstimationStdDevs(visionEst->estimatedPose.ToPose2d()));
+      }
+    }
   }
 
   Eigen::Matrix<double, 3, 1> GetEstimationStdDevs(frc::Pose2d estimatedPose) {
@@ -149,4 +155,6 @@ class Vision {
 
   // The most recent result, cached for calculating std devs
   photon::PhotonPipelineResult m_latestResult;
+  std::function<void(frc::Pose2d, units::second_t, Eigen::Matrix<double, 3, 1>)>
+      visionConsumer;
 };