From 0eb0a4e3c58c0ac643c9da9bfc24c18936a05078 Mon Sep 17 00:00:00 2001 From: Tim Winters Date: Mon, 5 Feb 2024 09:50:36 -0500 Subject: [PATCH] Store the last pose on update (#1207) * Store the last pose on update * Don't clear lastPose if pose isn't calculated --------- Co-authored-by: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com> --- .../src/main/java/org/photonvision/PhotonPoseEstimator.java | 4 ++-- photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index f93c49914..6b217c9b7 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -408,8 +408,8 @@ public class PhotonPoseEstimator { return Optional.empty(); } - if (estimatedPose.isEmpty()) { - lastPose = null; + if (estimatedPose.isPresent()) { + lastPose = estimatedPose.get().estimatedPose; } return estimatedPose; diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 30bc6de2d..d14394b99 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -186,6 +186,9 @@ std::optional PhotonPoseEstimator::Update( ret = std::nullopt; } + if (ret) { + lastPose = ret.value().estimatedPose; + } return ret; }