Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11: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

32
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -498,10 +498,22 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Add a vision measurement with a different translation
estimator.AddVisionMeasurement(
wpi::math::Pose3d(3_m, 0_m, 0_m, wpi::math::Rotation3d{}),
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().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(wpi::math::Rotation3d{0_deg, 0_deg, 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(0, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -517,7 +529,7 @@ TEST(SwerveDrivePoseEstimator3dTest, 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(0, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -525,6 +537,20 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Add a vision measurement with a different rotation
estimator.AddVisionMeasurement(
wpi::math::Pose3d(2.5_m, 1_m, 0_m,
wpi::math::Rotation3d{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(0, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(wpi::math::Translation3d{-1_m, -1_m, -1_m});
@@ -533,7 +559,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose

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