diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp index 75c72d2163..3ce3663d8a 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp +++ b/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(); } diff --git a/photonlib-cpp-examples/poseest/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h index af3713389e..ca025cc376 100644 --- 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 #include +#include #include #include @@ -41,7 +42,10 @@ class Vision { public: - Vision() { + Vision(std::function)> + visionConsumer) + : visionConsumer{visionConsumer} { photonEstimator.SetMultiTagFallbackStrategy( photon::PoseStrategy::LOWEST_AMBIGUITY); @@ -68,9 +72,7 @@ class Vision { photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; } - std::optional GetEstimatedGlobalPose() { - std::optional 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 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)> + visionConsumer; }; diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java index d47e3056ee..95fac0e8ab 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java +++ b/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 diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java index 81e505394d..8105d91a95 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java +++ b/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 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. - * - *

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 getEstimatedGlobalPose() { + public void periodic() { Optional visionEst = Optional.empty(); for (var change : camera.getAllUnreadResults()) { visionEst = photonEstimator.update(change); @@ -109,8 +101,15 @@ public Optional 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 estimationStdDevs); + } }