diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 942cd1c9f..9f196d74d 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -212,6 +212,19 @@ public class PhotonPoseEstimator { } PhotonPipelineResult cameraResult = camera.getLatestResult(); + + return update(cameraResult); + } + + /** + * Updates the estimated position of the robot. Returns empty if there are no cameras set or no + * targets were found from the cameras. + * + * @param cameraResult The latest pipeline result from the camera + * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and + * pipeline results used to create the estimate + */ + public Optional update(PhotonPipelineResult cameraResult) { if (!cameraResult.hasTargets()) { return Optional.empty(); } diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index 3b4f27807..ecdc03eb1 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -55,7 +55,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, std::optional PhotonPoseEstimator::Update() { auto result = camera.GetLatestResult(); + return Update(result); +} +std::optional PhotonPoseEstimator::Update( + const PhotonPipelineResult& result) { if (!result.HasTargets()) { return std::nullopt; } diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h index 6407732ee..10b89f499 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h @@ -155,6 +155,11 @@ class PhotonPoseEstimator { */ std::optional Update(); + /** + * Update the pose estimator. + */ + std::optional Update(const PhotonPipelineResult& result); + inline PhotonCamera& GetCamera() { return camera; } private: