Make Vision pose estimation examples use all vision measurements

photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp

@@ -34,15 +34,7 @@ void Robot::RobotInit() {}
 void Robot::RobotPeriodic() {
   launcher.periodic();
   drivetrain.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);
-  }
+  vision.Periodic();
 
   drivetrain.Log();
 }

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->timestamp,
+            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;
 };

photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java

@@ -46,7 +46,7 @@ public class Robot extends TimedRobot {
     @Override
     public void robotInit() {
         drivetrain = new SwerveDrive();
-        vision = new Vision();
+        vision = new Vision(drivetrain::addVisionMeasurement);
 
         controller = new XboxController(0);
 
@@ -61,16 +61,8 @@ public void robotPeriodic() {
         // Update drivetrain subsystem
         drivetrain.periodic();
 
-        // Correct pose estimate with vision measurements
-        var visionEst = vision.getEstimatedGlobalPose();
-        visionEst.ifPresent(
-                est -> {
-                    // Change our trust in the measurement based on the tags we can see
-                    var estStdDevs = vision.getEstimationStdDevs();
-
-                    drivetrain.addVisionMeasurement(
-                            est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
-                });
+        // Update vision
+        vision.periodic();
 
         // Test/Example only!
         // Apply an offset to pose estimator to test vision correction

photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java

@@ -48,12 +48,14 @@ public class Vision {
     private final PhotonCamera camera;
     private final PhotonPoseEstimator photonEstimator;
     private Matrix<N3, N1> curStdDevs;
+    private EstimateConsumer consumer;
 
     // Simulation
     private PhotonCameraSim cameraSim;
     private VisionSystemSim visionSim;
 
-    public Vision() {
+    public Vision(EstimateConsumer consumer) {
+        this.consumer = consumer;
         camera = new PhotonCamera(kCameraName);
 
         photonEstimator =
@@ -83,17 +85,7 @@ public Vision() {
         }
     }
 
-    /**
-     * The latest estimated robot pose on the field from vision data. This may be empty. This should
-     * only be called once per loop.
-     *
-     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
-     * {@link getEstimationStdDevs}
-     *
-     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
-     *     used for estimation.
-     */
-    public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+    public void periodic() {
         Optional<EstimatedRobotPose> visionEst = Optional.empty();
         for (var change : camera.getAllUnreadResults()) {
             visionEst = photonEstimator.update(change);
@@ -109,8 +101,15 @@ public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
                             getSimDebugField().getObject("VisionEstimation").setPoses();
                         });
             }
+
+            visionEst.ifPresent(
+                    est -> {
+                        // Change our trust in the measurement based on the tags we can see
+                        var estStdDevs = getEstimationStdDevs();
+
+                        consumer.accept(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
+                    });
         }
-        return visionEst;
     }
 
     /**
@@ -188,4 +187,9 @@ public Field2d getSimDebugField() {
         if (!Robot.isSimulation()) return null;
         return visionSim.getDebugField();
     }
+
+    @FunctionalInterface
+    public static interface EstimateConsumer {
+        public void accept(Pose2d pose, double timestamp, Matrix<N3, N1> estimationStdDevs);
+    }
 }