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] Fix pose estimator performance (#4111)

Browse Source 
Fixes #4087.
This commit is contained in:
Tyler Veness
2022-04-26 18:43:59 -07:00
committed by GitHub
parent 51bc893bc5
commit d926dd1610
12 changed files with 159 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
View File

@@ -14,12 +14,12 @@ TEST(TimeInterpolatableBufferTest, Interpolation) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
buffer.AddSample(0_s, frc::Rotation2d(0_rad));
EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(0_rad));
EXPECT_TRUE(buffer.Sample(0_s).value() == frc::Rotation2d(0_rad));
buffer.AddSample(1_s, frc::Rotation2d(1_rad));
EXPECT_TRUE(buffer.Sample(0.5_s) == frc::Rotation2d(0.5_rad));
EXPECT_TRUE(buffer.Sample(1_s) == frc::Rotation2d(1_rad));
EXPECT_TRUE(buffer.Sample(0.5_s).value() == frc::Rotation2d(0.5_rad));
EXPECT_TRUE(buffer.Sample(1_s).value() == frc::Rotation2d(1_rad));
buffer.AddSample(3_s, frc::Rotation2d(2_rad));
EXPECT_TRUE(buffer.Sample(2_s) == frc::Rotation2d(1.5_rad));
EXPECT_TRUE(buffer.Sample(2_s).value() == frc::Rotation2d(1.5_rad));
buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
@@ -30,9 +30,8 @@ TEST(TimeInterpolatableBufferTest, Pose2d) {
// We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
frc::Pose2d sample = buffer.Sample(0.5_s);
EXPECT_TRUE(std::abs(sample.X().to<double>() - (1 - 1 / std::sqrt(2))) <
0.01);
EXPECT_TRUE(std::abs(sample.Y().to<double>() - (1 / std::sqrt(2))) < 0.01);
EXPECT_TRUE(std::abs(sample.Rotation().Degrees().to<double>() - 45) < 0.01);
frc::Pose2d sample = buffer.Sample(0.5_s).value();
EXPECT_TRUE(std::abs(sample.X().value() - (1 - 1 / std::sqrt(2))) < 0.01);
EXPECT_TRUE(std::abs(sample.Y().value() - (1 / std::sqrt(2))) < 0.01);
EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45) < 0.01);
}