Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix pose estimator reset methods (#7225)

Browse Source
This commit is contained in:
Joseph Eng
2024-10-18 16:06:32 -07:00
committed by GitHub
parent ee22482f4a
commit 2054d0f57e
8 changed files with 474 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project.
#include <limits>
#include <numbers>
#include <random>
#include <tuple>
#include <vector>
@@ -383,3 +384,76 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(SwerveDrivePoseEstimatorTest, TestReset) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose2d{},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};
estimator.ResetPosition(
frc::Rotation2d{},
{modulePosition, modulePosition, modulePosition, modulePosition},
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());
// Test orientation and wheel positions
{
frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
modulePosition, modulePosition});
}
EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_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());
// Test orientation
{
frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}};
estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition,
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());
// 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());
// 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());
}