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

[wpimath] Discard stale pose estimates (#5045)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2023-02-04 01:04:30 -05:00
committed by GitHub
parent fe5d226a19
commit 4079eabe9b
14 changed files with 283 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -266,4 +266,41 @@ class DifferentialDrivePoseEstimatorTest {
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
}
}
@Test
void testDiscardsStaleVisionMeasurements() {
var kinematics = new DifferentialDriveKinematics(1);
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
new Rotation2d(),
0,
0,
new Pose2d(),
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
double time = 0;
// Add enough measurements to fill up the buffer
for (; time < 4; time += 0.02) {
estimator.updateWithTime(time, new Rotation2d(), 0, 0);
}
var odometryPose = estimator.getEstimatedPosition();
// Apply a vision measurement made 3 seconds ago
// This test passes if this does not cause a ConcurrentModificationException.
estimator.addVisionMeasurement(
new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
1,
VecBuilder.fill(0.1, 0.1, 0.1));
assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
assertEquals(
odometryPose.getRotation().getRadians(),
estimator.getEstimatedPosition().getRotation().getRadians(),
"Incorrect Final Theta");
}
}

41
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -279,4 +279,45 @@ class MecanumDrivePoseEstimatorTest {
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
}
}
@Test
void testDiscardsOldVisionMeasurements() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1),
new Translation2d(-1, 1),
new Translation2d(1, -1),
new Translation2d(-1, -1));
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
new Rotation2d(),
new MecanumDriveWheelPositions(),
new Pose2d(),
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
double time = 0;
// Add enough measurements to fill up the buffer
for (; time < 4; time += 0.02) {
estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions());
}
var odometryPose = estimator.getEstimatedPosition();
// Apply a vision measurement made 3 seconds ago
// This test passes if this does not cause a ConcurrentModificationException.
estimator.addVisionMeasurement(
new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
1,
VecBuilder.fill(0.1, 0.1, 0.1));
assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
assertEquals(
odometryPose.getRotation().getRadians(),
estimator.getEstimatedPosition().getRotation().getRadians(),
"Incorrect Final Theta");
}
}

54
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -300,4 +300,58 @@ class SwerveDrivePoseEstimatorTest {
assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
}
}
@Test
void testDiscardsOldVisionMeasurements() {
var kinematics =
new SwerveDriveKinematics(
new Translation2d(1, 1),
new Translation2d(-1, 1),
new Translation2d(1, -1),
new Translation2d(-1, -1));
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
new Rotation2d(),
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
},
new Pose2d(),
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
double time = 0;
// Add enough measurements to fill up the buffer
for (; time < 4; time += 0.02) {
estimator.updateWithTime(
time,
new Rotation2d(),
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
}
var odometryPose = estimator.getEstimatedPosition();
// Apply a vision measurement made 3 seconds ago
// This test passes if this does not cause a ConcurrentModificationException.
estimator.addVisionMeasurement(
new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)),
1,
VecBuilder.fill(0.1, 0.1, 0.1));
assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X");
assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y");
assertEquals(
odometryPose.getRotation().getRadians(),
estimator.getEstimatedPosition().getRotation().getRadians(),
"Incorrect Final Theta");
}
}