Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add 3D odometry and pose estimation (#7119)

Browse Source
This commit is contained in:
Joseph Eng
2024-11-16 07:56:14 -08:00
committed by GitHub
parent aa7dd258c4
commit 2acf111f56
49 changed files with 6716 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -376,53 +376,60 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
{1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m},
frc::Pose2d{1_m, 0_m, frc::Rotation2d{}});
EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, {2_m, 2_m, 2_m, 2_m});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, 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(frc::Rotation2d{90_deg});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test orientation
estimator.Update(frc::Rotation2d{}, {3_m, 3_m, 3_m, 3_m});
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose
estimator.ResetPose(frc::Pose2d{});
EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
}