Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51: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/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -399,9 +399,10 @@ TEST(SwerveDrivePoseEstimatorTest, 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
{
@@ -412,9 +413,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
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
{
@@ -423,17 +425,19 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
modulePosition, modulePosition});
}
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
{
@@ -442,23 +446,26 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
modulePosition, modulePosition});
}
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());
}