Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +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");
}
}

28
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -263,3 +263,31 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}
TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the bufer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m);
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition().Rotation().Radians().value(),
1e-6);
}

31
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -262,3 +262,34 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}
TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::MecanumDriveKinematics kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the bufer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
estimator.UpdateWithTime(time, frc::Rotation2d{},
frc::MecanumDriveWheelPositions{});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition().Rotation().Radians().value(),
1e-6);
}

35
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -275,3 +275,38 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}
TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the bufer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition().Rotation().Radians().value(),
1e-6);
}