diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index b5fe591f2b..ada6fc72d0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -212,7 +212,16 @@ public class DifferentialDrivePoseEstimator { sample.get().rightMeters, sample.get().poseMeters.exp(scaledTwist)); - // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the + // Step 6: Record the current pose to allow multiple measurements from the same timestamp + m_poseBuffer.addSample( + timestampSeconds, + new InterpolationRecord( + getEstimatedPosition(), + sample.get().gyroAngle, + sample.get().leftMeters, + sample.get().rightMeters)); + + // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the // pose buffer and correct odometry. for (Map.Entry entry : m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 448b8d3fde..7c8b0a7c41 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -199,7 +199,13 @@ public class MecanumDrivePoseEstimator { sample.get().wheelPositions, sample.get().poseMeters.exp(scaledTwist)); - // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the + // Step 6: Record the current pose to allow multiple measurements from the same timestamp + m_poseBuffer.addSample( + timestampSeconds, + new InterpolationRecord( + getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions)); + + // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the // pose buffer and correct odometry. for (Map.Entry entry : m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 57451a0329..3a57ff09dd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -201,7 +201,13 @@ public class SwerveDrivePoseEstimator { sample.get().modulePositions, sample.get().poseMeters.exp(scaledTwist)); - // Step 6: Replay odometry inputs between sample time and latest recorded sample to update the + // Step 6: Record the current pose to allow multiple measurements from the same timestamp + m_poseBuffer.addSample( + timestampSeconds, + new InterpolationRecord( + getEstimatedPosition(), sample.get().gyroAngle, sample.get().modulePositions)); + + // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the // pose buffer and correct odometry. for (Map.Entry entry : m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 39e0d8cb65..27d745cdfd 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -121,7 +121,13 @@ void DifferentialDrivePoseEstimator::AddVisionMeasurement( sample.value().gyroAngle, sample.value().leftDistance, sample.value().rightDistance, sample.value().pose.Exp(scaledTwist)); - // Step 6: Replay odometry inputs between sample time and latest recorded + // Step 6: Record the current pose to allow multiple measurements from the + // same timestamp + m_poseBuffer.AddSample( + timestamp, {GetEstimatedPosition(), sample.value().gyroAngle, + sample.value().leftDistance, sample.value().rightDistance}); + + // Step 7: Replay odometry inputs between sample time and latest recorded // sample to update the pose buffer and correct odometry. auto internal_buf = m_poseBuffer.GetInternalBuffer(); diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 9642e08f98..a7fa523f2c 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -132,7 +132,13 @@ void frc::MecanumDrivePoseEstimator::AddVisionMeasurement( sample.value().wheelPositions, sample.value().pose.Exp(scaledTwist)); - // Step 6: Replay odometry inputs between sample time and latest recorded + // Step 6: Record the current pose to allow multiple measurements from the + // same timestamp + m_poseBuffer.AddSample(timestamp, + {GetEstimatedPosition(), sample.value().gyroAngle, + sample.value().wheelPositions}); + + // Step 7: Replay odometry inputs between sample time and latest recorded // sample to update the pose buffer and correct odometry. auto internal_buf = m_poseBuffer.GetInternalBuffer(); diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index d07bafe5ed..ea282fdb51 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -194,10 +194,16 @@ class SwerveDrivePoseEstimator { // Step 5: Reset Odometry to state at sample with vision adjustment. m_odometry.ResetPosition(sample.value().gyroAngle, - sample.value().modulePostions, + sample.value().modulePositions, sample.value().pose.Exp(scaledTwist)); - // Step 6: Replay odometry inputs between sample time and latest recorded + // Step 6: Record the current pose to allow multiple measurements from the + // same timestamp + m_poseBuffer.AddSample(timestamp, + {GetEstimatedPosition(), sample.value().gyroAngle, + sample.value().modulePositions}); + + // Step 7: Replay odometry inputs between sample time and latest recorded // sample to update the pose buffer and correct odometry. auto internal_buf = m_poseBuffer.GetInternalBuffer(); @@ -207,7 +213,7 @@ class SwerveDrivePoseEstimator { for (auto entry = upper_bound; entry != internal_buf.end(); entry++) { UpdateWithTime(entry->first, entry->second.gyroAngle, - entry->second.modulePostions); + entry->second.modulePositions); } } @@ -300,7 +306,7 @@ class SwerveDrivePoseEstimator { Rotation2d gyroAngle; // The distances traveled and rotations meaured at each module. - wpi::array modulePostions; + wpi::array modulePositions; /** * Checks equality between this InterpolationRecord and another object. @@ -344,14 +350,14 @@ class SwerveDrivePoseEstimator { for (size_t i = 0; i < NumModules; i++) { modulePositions[i].distance = - wpi::Lerp(this->modulePostions[i].distance, - endValue.modulePostions[i].distance, i); + wpi::Lerp(this->modulePositions[i].distance, + endValue.modulePositions[i].distance, i); modulePositions[i].angle = - wpi::Lerp(this->modulePostions[i].angle, - endValue.modulePostions[i].angle, i); + wpi::Lerp(this->modulePositions[i].angle, + endValue.modulePositions[i].angle, i); modulesDelta[i].distance = - modulePositions[i].distance - this->modulePostions[i].distance; + modulePositions[i].distance - this->modulePositions[i].distance; modulesDelta[i].angle = modulePositions[i].angle; } diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h index 771fe846b0..fbe7948d44 100644 --- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -68,11 +68,24 @@ class TimeInterpolatableBuffer { if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) { m_pastSnapshots.emplace_back(time, sample); } else { - m_pastSnapshots.insert( - std::upper_bound( - m_pastSnapshots.begin(), m_pastSnapshots.end(), time, - [](auto t, const auto& pair) { return t < pair.first; }), - std::pair(time, sample)); + auto first_after = std::upper_bound( + m_pastSnapshots.begin(), m_pastSnapshots.end(), time, + [](auto t, const auto& pair) { return t < pair.first; }); + + auto last_not_greater_than = first_after - 1; + + if (last_not_greater_than == m_pastSnapshots.begin() || + last_not_greater_than->first < time) { + // Two cases handled together: + // 1. All entries come after the sample + // 2. Some entries come before the sample, but none are recorded with + // the same time + m_pastSnapshots.insert(first_after, std::pair(time, sample)); + } else { + // Final case: + // 3. An entry exists with the same recorded time. + last_not_greater_than->second = sample; + } } while (time - m_pastSnapshots[0].first > m_historySize) { m_pastSnapshots.erase(m_pastSnapshots.begin()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java index c3906d4de0..84ba7473b6 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java @@ -227,4 +227,55 @@ class DifferentialDrivePoseEstimatorTest { assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } + + @Test + void testSimultaneousVisionMeasurements() { + // This tests for multiple vision measurements appled at the same time. The expected behavior + // is that all measurements affect the estimated pose. The alternative result is that only one + // vision measurement affects the outcome. If that were the case, after 1000 measurements, the + // estimated pose would converge to that measurement. + var kinematics = new DifferentialDriveKinematics(1); + + var estimator = + new DifferentialDrivePoseEstimator( + kinematics, + new Rotation2d(), + 0, + 0, + new Pose2d(1, 2, Rotation2d.fromDegrees(270)), + VecBuilder.fill(0.02, 0.02, 0.01), + VecBuilder.fill(0.1, 0.1, 0.1)); + + estimator.updateWithTime(0, new Rotation2d(), 0, 0); + + var visionMeasurements = + new Pose2d[] { + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new Pose2d(3, 1, Rotation2d.fromDegrees(90)), + new Pose2d(2, 4, Rotation2d.fromRadians(180)), + }; + + for (int i = 0; i < 1000; i++) { + for (var measurement : visionMeasurements) { + estimator.addVisionMeasurement(measurement, 0); + } + } + + for (var measurement : visionMeasurements) { + var errorLog = + "Estimator converged to one vision measurement: " + + estimator.getEstimatedPosition().toString() + + " -> " + + measurement.toString(); + + var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); + var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); + var dtheta = + Math.abs( + measurement.getRotation().getDegrees() + - estimator.getEstimatedPosition().getRotation().getDegrees()); + + assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog); + } + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java index 02d2d52c42..fcaebb7eba 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java @@ -236,4 +236,59 @@ class MecanumDrivePoseEstimatorTest { assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } + + @Test + void testSimultaneousVisionMeasurements() { + // This tests for multiple vision measurements appled at the same time. The expected behavior + // is that all measurements affect the estimated pose. The alternative result is that only one + // vision measurement affects the outcome. If that were the case, after 1000 measurements, the + // estimated pose would converge to that measurement. + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var estimator = + new MecanumDrivePoseEstimator( + kinematics, + new Rotation2d(), + wheelPositions, + new Pose2d(1, 2, Rotation2d.fromDegrees(270)), + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.45, 0.45, 0.1)); + + estimator.updateWithTime(0, new Rotation2d(), wheelPositions); + + var visionMeasurements = + new Pose2d[] { + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new Pose2d(3, 1, Rotation2d.fromDegrees(90)), + new Pose2d(2, 4, Rotation2d.fromRadians(180)), + }; + + for (int i = 0; i < 1000; i++) { + for (var measurement : visionMeasurements) { + estimator.addVisionMeasurement(measurement, 0); + } + } + + for (var measurement : visionMeasurements) { + var errorLog = + "Estimator converged to one vision measurement: " + + estimator.getEstimatedPosition().toString() + + " -> " + + measurement.toString(); + + var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); + var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); + var dtheta = + Math.abs( + measurement.getRotation().getDegrees() + - estimator.getEstimatedPosition().getRotation().getDegrees()); + + assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog); + } + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index fde2c39e9f..49679d4a3b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -263,4 +263,64 @@ class SwerveDrivePoseEstimatorTest { assertEquals(0.0, maxError, 0.2, "Incorrect max error"); } } + + @Test + void testSimultaneousVisionMeasurements() { + // This tests for multiple vision measurements appled at the same time. The expected behavior + // is that all measurements affect the estimated pose. The alternative result is that only one + // vision measurement affects the outcome. If that were the case, after 1000 measurements, the + // estimated pose would converge to that measurement. + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + + var fl = new SwerveModulePosition(); + var fr = new SwerveModulePosition(); + var bl = new SwerveModulePosition(); + var br = new SwerveModulePosition(); + + var estimator = + new SwerveDrivePoseEstimator( + kinematics, + new Rotation2d(), + new SwerveModulePosition[] {fl, fr, bl, br}, + new Pose2d(1, 2, Rotation2d.fromDegrees(270)), + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9)); + + estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br}); + + var visionMeasurements = + new Pose2d[] { + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new Pose2d(3, 1, Rotation2d.fromDegrees(90)), + new Pose2d(2, 4, Rotation2d.fromRadians(180)), + }; + + for (int i = 0; i < 1000; i++) { + for (var measurement : visionMeasurements) { + estimator.addVisionMeasurement(measurement, 0); + } + } + + for (var measurement : visionMeasurements) { + var errorLog = + "Estimator converged to one vision measurement: " + + estimator.getEstimatedPosition().toString() + + " -> " + + measurement.toString(); + + var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); + var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); + var dtheta = + Math.abs( + measurement.getRotation().getDegrees() + - estimator.getEstimatedPosition().getRotation().getDegrees()); + + assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog); + } + } } diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 1d623ca591..4c91c137be 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -207,3 +207,59 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) { } } } + +TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { + // This tests for multiple vision measurements appled at the same time. + // The expected behavior is that all measurements affect the estimated pose. + // The alternative result is that only one vision measurement affects the + // outcome. If that were the case, after 1000 measurements, the estimated + // pose would converge to that measurement. + frc::DifferentialDriveKinematics kinematics{1.0_m}; + + frc::DifferentialDrivePoseEstimator estimator{ + kinematics, + frc::Rotation2d{}, + 0_m, + 0_m, + frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, + {0.02, 0.02, 0.01}, + {0.1, 0.1, 0.1}}; + + estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m); + + for (int i = 0; i < 1000; i++) { + estimator.AddVisionMeasurement( + frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } +} diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index 13dc5aa091..79c2cb859e 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -206,3 +206,59 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) { } } } + +TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { + // This tests for multiple vision measurements appled at the same time. + // The expected behavior is that all measurements affect the estimated pose. + // The alternative result is that only one vision measurement affects the + // outcome. If that were the case, after 1000 measurements, the estimated + // pose would converge to that measurement. + 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::MecanumDriveWheelPositions wheelPositions; + + frc::MecanumDrivePoseEstimator estimator{ + kinematics, frc::Rotation2d{}, + wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, + {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}}; + + estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions); + + for (int i = 0; i < 1000; i++) { + estimator.AddVisionMeasurement( + frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } +} diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 554ca5943e..6d5c61f0ca 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -8,6 +8,7 @@ #include #include +#include #include "frc/estimator/SwerveDrivePoseEstimator.h" #include "frc/geometry/Pose2d.h" @@ -215,3 +216,62 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) { } } } + +TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { + // This tests for multiple vision measurements appled at the same time. + // The expected behavior is that all measurements affect the estimated pose. + // The alternative result is that only one vision measurement affects the + // outcome. If that were the case, after 1000 measurements, the estimated + // pose would converge to that measurement. + 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{1_m, 2_m, frc::Rotation2d{270_deg}}, + {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; + + estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br}); + + for (int i = 0; i < 1000; i++) { + estimator.AddVisionMeasurement( + frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); + auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); + auto dtheta = units::math::abs( + estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } +}