diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 14eee8f190a..141844e362e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -21,6 +21,7 @@ import java.util.Map; import java.util.NoSuchElementException; import java.util.Objects; +import java.util.Optional; /** * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder @@ -125,6 +126,16 @@ public Pose2d getEstimatedPosition() { return m_odometry.getPoseMeters(); } + /** + * Return the pose at a given timestamp, if the buffer is not empty. + * + * @param timestampSeconds The pose's timestamp in seconds. + * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). + */ + public Optional sampleAt(double timestampSeconds) { + return m_poseBuffer.getSample(timestampSeconds).map(record -> record.poseMeters); + } + /** * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate * while still accounting for measurement noise. diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 6e38d8c65fb..4acd560b9b6 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -4,6 +4,8 @@ #pragma once +#include + #include #include #include @@ -89,6 +91,15 @@ class WPILIB_DLLEXPORT PoseEstimator { */ Pose2d GetEstimatedPosition() const; + /** + * Return the pose at a given timestamp, if the buffer is not empty. + * + * @param timestampSeconds The pose's timestamp. + * @return The pose at the given timestamp (or std::nullopt if the buffer is + * empty). + */ + std::optional SampleAt(units::second_t timestamp) const; + /** * Adds a vision measurement to the Kalman Filter. This will correct * the odometry pose estimate while still accounting for measurement noise. diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc index 79d71bcc566..43942cfbae2 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc @@ -57,6 +57,19 @@ Pose2d PoseEstimator::GetEstimatedPosition() return m_odometry.GetPose(); } +template +std::optional PoseEstimator::SampleAt( + units::second_t timestamp) const { + // TODO Replace with std::optional::transform() in C++23 + std::optional::InterpolationRecord> + sample = m_poseBuffer.Sample(timestamp); + if (sample) { + return sample->pose; + } else { + return std::nullopt; + } +} + template void PoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h index 94d28180d8f..bfb1b548976 100644 --- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -100,7 +100,7 @@ class TimeInterpolatableBuffer { * * @param time The time at which to sample the buffer. */ - std::optional Sample(units::second_t time) { + std::optional Sample(units::second_t time) const { if (m_pastSnapshots.empty()) { return {}; } 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 08c5a156cf1..b019c608116 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 @@ -18,6 +18,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import java.util.List; +import java.util.Optional; import java.util.Random; import java.util.TreeMap; import java.util.function.Function; @@ -304,4 +305,54 @@ void testDiscardsStaleVisionMeasurements() { estimator.getEstimatedPosition().getRotation().getRadians(), "Incorrect Final Theta"); } + + @Test + void testSampleAt() { + var kinematics = new DifferentialDriveKinematics(1); + var estimator = + new DifferentialDrivePoseEstimator( + kinematics, + new Rotation2d(), + 0, + 0, + new Pose2d(), + VecBuilder.fill(1, 1, 1), + VecBuilder.fill(1, 1, 1)); + + // Returns empty when null + assertEquals(Optional.empty(), estimator.sampleAt(1)); + + // Add odometry measurements, but don't fill up the buffer + // Add a tiny tolerance for the upper bound because of floating point rounding error + for (double time = 1; time <= 2 + 1e-9; time += 0.02) { + estimator.updateWithTime(time, new Rotation2d(), time, time); + } + + // Sample at an added time + assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02)); + // Sample between updates (test interpolation) + assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01)); + // Sampling before the oldest value returns the oldest value + assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5)); + // Sampling after the newest value returns the newest value + assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5)); + + // Add a vision measurement after the odometry measurements (while keeping all of the old + // odometry measurements) + estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2); + + // Make sure nothing changed (except the newest value) + assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5)); + + // Add a vision measurement before the odometry measurements that's still in the buffer + estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9); + + // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) + assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5)); + } } 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 ea08a5f1d90..c48012f7f95 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 @@ -19,6 +19,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import java.util.List; +import java.util.Optional; import java.util.Random; import java.util.TreeMap; import java.util.function.Function; @@ -321,4 +322,59 @@ void testDiscardsOldVisionMeasurements() { estimator.getEstimatedPosition().getRotation().getRadians(), "Incorrect Final Theta"); } + + @Test + void testSampleAt() { + 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(1, 1, 1), + VecBuilder.fill(1, 1, 1)); + + // Returns empty when null + assertEquals(Optional.empty(), estimator.sampleAt(1)); + + // Add odometry measurements, but don't fill up the buffer + // Add a tiny tolerance for the upper bound because of floating point rounding error + for (double time = 1; time <= 2 + 1e-9; time += 0.02) { + var wheelPositions = new MecanumDriveWheelPositions(time, time, time, time); + estimator.updateWithTime(time, new Rotation2d(), wheelPositions); + } + + // Sample at an added time + assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02)); + // Sample between updates (test interpolation) + assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01)); + // Sampling before the oldest value returns the oldest value + assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5)); + // Sampling after the newest value returns the newest value + assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5)); + + // Add a vision measurement after the odometry measurements (while keeping all of the old + // odometry measurements) + estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2); + + // Make sure nothing changed (except the newest value) + assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5)); + + // Add a vision measurement before the odometry measurements that's still in the buffer + estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9); + + // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) + assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5)); + } } 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 f827a102527..7101fa554a8 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 @@ -19,6 +19,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import java.util.List; +import java.util.Optional; import java.util.Random; import java.util.TreeMap; import java.util.function.Function; @@ -355,4 +356,70 @@ void testDiscardsOldVisionMeasurements() { estimator.getEstimatedPosition().getRotation().getRadians(), "Incorrect Final Theta"); } + + @Test + void testSampleAt() { + 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(1, 1, 1), + VecBuilder.fill(1, 1, 1)); + + // Returns empty when null + assertEquals(Optional.empty(), estimator.sampleAt(1)); + + // Add odometry measurements, but don't fill up the buffer + // Add a tiny tolerance for the upper bound because of floating point rounding error + for (double time = 1; time <= 2 + 1e-9; time += 0.02) { + var wheelPositions = + new SwerveModulePosition[] { + new SwerveModulePosition(time, new Rotation2d()), + new SwerveModulePosition(time, new Rotation2d()), + new SwerveModulePosition(time, new Rotation2d()), + new SwerveModulePosition(time, new Rotation2d()) + }; + estimator.updateWithTime(time, new Rotation2d(), wheelPositions); + } + + // Sample at an added time + assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02)); + // Sample between updates (test interpolation) + assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01)); + // Sampling before the oldest value returns the oldest value + assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5)); + // Sampling after the newest value returns the newest value + assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5)); + + // Add a vision measurement after the odometry measurements (while keeping all of the old + // odometry measurements) + estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2); + + // Make sure nothing changed (except the newest value) + assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5)); + + // Add a vision measurement before the odometry measurements that's still in the buffer + estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9); + + // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) + assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5)); + } } diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 3103068344d..ca88d11bd18 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -293,3 +293,62 @@ TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { estimator.GetEstimatedPosition().Rotation().Radians().value(), 1e-6); } + +TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) { + frc::DifferentialDriveKinematics kinematics{1_m}; + frc::DifferentialDrivePoseEstimator estimator{ + kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, + {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}}; + + // Returns empty when null + EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s)); + + // Add odometry measurements, but don't fill up the buffer + // Add a tiny tolerance for the upper bound because of floating point rounding + // error + for (double time = 1; time <= 2 + 1e-9; time += 0.02) { + estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{}, + units::meter_t{time}, units::meter_t{time}); + } + + // Sample at an added time + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + // Sample between updates (test interpolation) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + // Sampling before the oldest value returns the oldest value + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + // Sampling after the newest value returns the newest value + EXPECT_EQ(std::make_optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(2.5_s)); + + // Add a vision measurement after the odometry measurements (while keeping all + // of the old odometry measurements) + estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}}, + 2.2_s); + + // Make sure nothing changed (except the newest value) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + + // Add a vision measurement before the odometry measurements that's still in + // the buffer + estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}}, + 0.9_s); + + // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(2.5_s)); +} diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index 8b3a586892d..14d45dd04de 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -295,3 +295,67 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { estimator.GetEstimatedPosition().Rotation().Radians().value(), 1e-6); } + +TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) { + 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{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}}; + + // Returns empty when null + EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s)); + + // Add odometry measurements, but don't fill up the buffer + // Add a tiny tolerance for the upper bound because of floating point rounding + // error + for (double time = 1; time <= 2 + 1e-9; time += 0.02) { + frc::MecanumDriveWheelPositions wheelPositions{ + units::meter_t{time}, units::meter_t{time}, units::meter_t{time}, + units::meter_t{time}}; + estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{}, + wheelPositions); + } + + // Sample at an added time + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + // Sample between updates (test interpolation) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + // Sampling before the oldest value returns the oldest value + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + // Sampling after the newest value returns the newest value + EXPECT_EQ(std::make_optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(2.5_s)); + + // Add a vision measurement after the odometry measurements (while keeping all + // of the old odometry measurements) + estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}}, + 2.2_s); + + // Make sure nothing changed (except the newest value) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + + // Add a vision measurement before the odometry measurements that's still in + // the buffer + estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}}, + 0.9_s); + + // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(2.5_s)); +} diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index f1024efbc92..77e3d6bbe4f 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -311,3 +311,74 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { estimator.GetEstimatedPosition().Rotation().Radians().value(), 1e-6); } + +TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) { + 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::SwerveDrivePoseEstimator estimator{ + kinematics, + frc::Rotation2d{}, + {frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, + frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}, + frc::Pose2d{}, + {1.0, 1.0, 1.0}, + {1.0, 1.0, 1.0}}; + + // Returns empty when null + EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s)); + + // Add odometry measurements, but don't fill up the buffer + // Add a tiny tolerance for the upper bound because of floating point rounding + // error + for (double time = 1; time <= 2 + 1e-9; time += 0.02) { + wpi::array wheelPositions{ + {frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}, + frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}, + frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}, + frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}}}; + estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{}, + wheelPositions); + } + + // Sample at an added time + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + // Sample between updates (test interpolation) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + // Sampling before the oldest value returns the oldest value + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + // Sampling after the newest value returns the newest value + EXPECT_EQ(std::make_optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(2.5_s)); + + // Add a vision measurement after the odometry measurements (while keeping all + // of the old odometry measurements) + estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}}, + 2.2_s); + + // Make sure nothing changed (except the newest value) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + + // Add a vision measurement before the odometry measurements that's still in + // the buffer + estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}}, + 0.9_s); + + // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) + EXPECT_EQ(std::make_optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(1.02_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(1.01_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(0.5_s)); + EXPECT_EQ(std::make_optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}), + estimator.SampleAt(2.5_s)); +}