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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2026-02-15 00:51:21 -08:00
parent 36c489247c 77dfad97c6
commit af865f8020
98 changed files with 3031 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -464,10 +464,20 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Add a vision measurement with a different translation
estimator.AddVisionMeasurement(
wpi::math::Pose2d{3_m, 0_m, wpi::math::Rotation2d{}},
wpi::math::MathSharedStore::GetTimestamp());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(wpi::math::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
@@ -481,19 +491,30 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Add a vision measurement with a different rotation
estimator.AddVisionMeasurement(
wpi::math::Pose2d{2.5_m, 1_m, wpi::math::Rotation2d{180_deg}},
wpi::math::MathSharedStore::GetTimestamp());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(wpi::math::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose