From 70de697fc92cfe1a004bc73dcd9cf29602993df4 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sun, 12 Jan 2025 00:35:04 +0800 Subject: [PATCH 1/3] Make Vision pose estimation examples use all vision measurements Resolves https://github.com/PhotonVision/photonvision/issues/1634 Signed-off-by: Jade Turner --- .../poseest/src/main/cpp/Robot.cpp | 1 + .../src/main/java/frc/robot/Robot.java | 14 ++------- .../src/main/java/frc/robot/Vision.java | 30 +++++++++++-------- 3 files changed, 21 insertions(+), 24 deletions(-) diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp index 75c72d2163..5d89f15e79 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp @@ -34,6 +34,7 @@ void Robot::RobotInit() {} void Robot::RobotPeriodic() { launcher.periodic(); drivetrain.Periodic(); + vision.Periodic(); auto visionEst = vision.GetEstimatedGlobalPose(); if (visionEst.has_value()) { 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); + } } From 179e31599fecbf56390bc935a1d2dfee39cc4253 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Wed, 15 Jan 2025 11:31:46 +0800 Subject: [PATCH 2/3] cpp Signed-off-by: Jade Turner --- .../poseest/src/main/cpp/Robot.cpp | 9 --------- .../poseest/src/main/include/Vision.h | 20 +++++++++++++------ 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp index 5d89f15e79..3ce3663d8a 100644 --- 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 index af3713389e..1f8660a2b9 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->estimate, + 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; }; From a8c129135912af034242591f6fa2cf0c3dbd1ad0 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Wed, 15 Jan 2025 11:48:50 +0800 Subject: [PATCH 3/3] oops Signed-off-by: Jade Turner --- photonlib-cpp-examples/poseest/src/main/include/Vision.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photonlib-cpp-examples/poseest/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h index 1f8660a2b9..ca025cc376 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Vision.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Vision.h @@ -92,7 +92,7 @@ class Vision { if (visionEst) { visionConsumer( - visionEst->estimatedPose.ToPose2d(), visionEst->estimate, + visionEst->estimatedPose.ToPose2d(), visionEst->timestamp, GetEstimationStdDevs(visionEst->estimatedPose.ToPose2d())); } }