From 2054d0f57e2167db029e7eeb3c8fc1c76d7ba4e1 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Fri, 18 Oct 2024 16:06:32 -0700 Subject: [PATCH] [wpimath] Fix pose estimator reset methods (#7225) --- .../first/math/estimator/PoseEstimator.java | 6 + .../include/frc/estimator/PoseEstimator.h | 6 + .../DifferentialDrivePoseEstimatorTest.java | 80 +++++++++++++ .../MecanumDrivePoseEstimatorTest.java | 87 ++++++++++++++ .../SwerveDrivePoseEstimatorTest.java | 111 ++++++++++++++++++ .../DifferentialDrivePoseEstimatorTest.cpp | 54 +++++++++ .../MecanumDrivePoseEstimatorTest.cpp | 56 +++++++++ .../SwerveDrivePoseEstimatorTest.cpp | 74 ++++++++++++ 8 files changed, 474 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 2674e2ceae..1f00cb7b18 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -134,6 +134,8 @@ public class PoseEstimator { public void resetPose(Pose2d pose) { m_odometry.resetPose(pose); m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); } /** @@ -144,6 +146,8 @@ public class PoseEstimator { public void resetTranslation(Translation2d translation) { m_odometry.resetTranslation(translation); m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); } /** @@ -154,6 +158,8 @@ public class PoseEstimator { public void resetRotation(Rotation2d rotation) { m_odometry.resetRotation(rotation); m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); } /** diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 1f4a88be2b..506bb69d94 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -124,6 +124,8 @@ class WPILIB_DLLEXPORT PoseEstimator { void ResetPose(const Pose2d& pose) { m_odometry.ResetPose(pose); m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose(); } /** @@ -134,6 +136,8 @@ class WPILIB_DLLEXPORT PoseEstimator { void ResetTranslation(const Translation2d& translation) { m_odometry.ResetTranslation(translation); m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose(); } /** @@ -144,6 +148,8 @@ class WPILIB_DLLEXPORT PoseEstimator { void ResetRotation(const Rotation2d& rotation) { m_odometry.ResetRotation(rotation); m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose(); } /** diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java index 221c98be03..09ebfeda98 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.estimator; +import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -25,6 +26,8 @@ import java.util.function.Function; import org.junit.jupiter.api.Test; class DifferentialDrivePoseEstimatorTest { + private static final double kEpsilon = 1e-9; + @Test void testAccuracy() { var kinematics = new DifferentialDriveKinematics(1); @@ -355,4 +358,81 @@ class DifferentialDrivePoseEstimatorTest { assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5)); assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5)); } + + @Test + void testReset() { + var kinematics = new DifferentialDriveKinematics(1); + var estimator = + new DifferentialDrivePoseEstimator( + kinematics, + Rotation2d.kZero, + 0, + 0, + Pose2d.kZero, + VecBuilder.fill(1, 1, 1), + VecBuilder.fill(1, 1, 1)); + + // Test reset position + estimator.resetPosition(Rotation2d.kZero, 1, 1, new Pose2d(1, 0, Rotation2d.kZero)); + + assertAll( + () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + + // Test orientation and wheel positions + estimator.update(Rotation2d.kZero, 2, 2); + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + + // Test reset rotation + estimator.resetRotation(Rotation2d.kCCW_Pi_2); + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test orientation + estimator.update(Rotation2d.kZero, 3, 3); + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test reset translation + estimator.resetTranslation(new Translation2d(-1, -1)); + + assertAll( + () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test reset pose + estimator.resetPose(Pose2d.kZero); + + assertAll( + () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java index 0dba2bbe67..ca81e7d866 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.estimator; +import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -26,6 +27,8 @@ import java.util.function.Function; import org.junit.jupiter.api.Test; class MecanumDrivePoseEstimatorTest { + private static final double kEpsilon = 1e-9; + @Test void testAccuracyFacingTrajectory() { var kinematics = @@ -377,4 +380,88 @@ class MecanumDrivePoseEstimatorTest { assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5)); assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5)); } + + @Test + void testReset() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new MecanumDrivePoseEstimator( + kinematics, + Rotation2d.kZero, + new MecanumDriveWheelPositions(), + Pose2d.kZero, + VecBuilder.fill(1, 1, 1), + VecBuilder.fill(1, 1, 1)); + + // Test reset position + estimator.resetPosition( + Rotation2d.kZero, + new MecanumDriveWheelPositions(1, 1, 1, 1), + new Pose2d(1, 0, Rotation2d.kZero)); + + assertAll( + () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + + // Test orientation and wheel positions + estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(2, 2, 2, 2)); + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + + // Test reset rotation + estimator.resetRotation(Rotation2d.kCCW_Pi_2); + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test orientation + estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(3, 3, 3, 3)); + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test reset translation + estimator.resetTranslation(new Translation2d(-1, -1)); + + assertAll( + () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test reset pose + estimator.resetPose(Pose2d.kZero); + + assertAll( + () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index 54398902f7..2b259a6a89 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -4,6 +4,7 @@ package edu.wpi.first.math.estimator; +import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -26,6 +27,8 @@ import java.util.function.Function; import org.junit.jupiter.api.Test; class SwerveDrivePoseEstimatorTest { + private static final double kEpsilon = 1e-9; + @Test void testAccuracyFacingTrajectory() { var kinematics = @@ -422,4 +425,112 @@ class SwerveDrivePoseEstimatorTest { assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5)); assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5)); } + + @Test + void testReset() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new SwerveDrivePoseEstimator( + kinematics, + Rotation2d.kZero, + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, + Pose2d.kZero, + VecBuilder.fill(1, 1, 1), + VecBuilder.fill(1, 1, 1)); + + // Test reset position + { + var modulePosition = new SwerveModulePosition(1, Rotation2d.kZero); + estimator.resetPosition( + Rotation2d.kZero, + new SwerveModulePosition[] { + modulePosition, modulePosition, modulePosition, modulePosition + }, + new Pose2d(1, 0, Rotation2d.kZero)); + } + + assertAll( + () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + + // Test orientation and wheel positions + { + var modulePosition = new SwerveModulePosition(2, Rotation2d.kZero); + estimator.update( + Rotation2d.kZero, + new SwerveModulePosition[] { + modulePosition, modulePosition, modulePosition, modulePosition + }); + } + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + + // Test reset rotation + estimator.resetRotation(Rotation2d.kCCW_Pi_2); + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test orientation + { + var modulePosition = new SwerveModulePosition(3, Rotation2d.kZero); + estimator.update( + Rotation2d.kZero, + new SwerveModulePosition[] { + modulePosition, modulePosition, modulePosition, modulePosition + }); + } + + assertAll( + () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test reset translation + estimator.resetTranslation(new Translation2d(-1, -1)); + + assertAll( + () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals( + Math.PI / 2, + estimator.getEstimatedPosition().getRotation().getRadians(), + kEpsilon)); + + // Test reset pose + estimator.resetPose(Pose2d.kZero); + + assertAll( + () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> + assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + } } diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index f6d229ec65..3ae2248249 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #include #include @@ -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()); +} diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index ae2f573874..ef2fe8919f 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #include #include @@ -361,3 +362,58 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) { EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}), estimator.SampleAt(2.5_s)); } + +TEST(MecanumDrivePoseEstimatorTest, TestReset) { + frc::MecanumDriveKinematics 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::MecanumDrivePoseEstimator estimator{ + kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{}, + 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, 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, 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, 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()); +} diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 1193824698..ec19d387f5 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #include #include @@ -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()); +}