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 C++ pose estimator poseEstimate initialization (#7249)

Browse Source
This commit is contained in:
Joseph Eng
2024-10-21 22:29:04 -07:00
committed by GitHub
parent 40af8db28a
commit 6745fc7c2f
7 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -394,10 +394,15 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
frc::Rotation2d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose2d{},
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{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());
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};