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

7
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -19,6 +19,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
/**
@@ -190,7 +191,11 @@ public class DifferentialDrivePoseEstimator {
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
try {
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
} catch (NoSuchElementException ex) {
return;
}

7
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -20,6 +20,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
/**
@@ -178,7 +179,11 @@ public class MecanumDrivePoseEstimator {
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
try {
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
} catch (NoSuchElementException ex) {
return;
}

7
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -21,6 +21,7 @@ import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Arrays;
import java.util.Map;
import java.util.NoSuchElementException;
import java.util.Objects;
/**
@@ -180,7 +181,11 @@ public class SwerveDrivePoseEstimator {
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
try {
if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) {
return;
}
} catch (NoSuchElementException ex) {
return;
}