Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Fixes #5100.
This commit is contained in:
Tyler Veness
2023-02-16 22:00:21 -08:00
committed by GitHub
parent 9eef79d638
commit 74dea9f05e
6 changed files with 27 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -96,8 +96,9 @@ void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, 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;
}

5
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -107,8 +107,9 @@ void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, 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;
}