[wpimath] Fix exception for empty pose buffer in pose estimators (#5106)

Fixes #5100.
2026-06-28 02:11:43 +00:00 · 2023-02-16 22:00:21 -08:00
parent 9eef79d638
commit 74dea9f05e
6 changed files with 27 additions and 9 deletions
--- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
+++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -172,8 +172,9 @@ class SwerveDrivePoseEstimator {
                            units::second_t timestamp) {
    // Step 0: If this measurement is old enough to be outside the pose buffer's
    // timespan, skip.
-    if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
-        timestamp) {
+    if (!m_poseBuffer.GetInternalBuffer().empty() &&
+        m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration >
+            timestamp) {
      return;
    }