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] 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

54
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.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 <utility>
@@ -354,3 +355,56 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}),
estimator.SampleAt(2.5_s));
}
TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, 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());
// Test orientation and wheel positions
estimator.Update(frc::Rotation2d{}, 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());
// 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
estimator.Update(frc::Rotation2d{}, 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());
// 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());
}