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

[wpimath] Add PoseEstimator.sampleAt() (#6426)

Browse Source
This commit is contained in:
Joseph Eng
2024-04-27 21:03:37 -07:00
committed by GitHub
parent 962bf7ff10
commit cdfa2ece6f
10 changed files with 404 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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<frc::SwerveModulePosition, 4> 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::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
estimator.SampleAt(1.02_s));
// Sample between updates (test interpolation)
EXPECT_EQ(std::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::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::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::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::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::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}),
estimator.SampleAt(1.02_s));
EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}),
estimator.SampleAt(1.01_s));
EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}),
estimator.SampleAt(0.5_s));
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}