From 6745fc7c2fdb1e22370635477fd0173ee5fd075c Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 21 Oct 2024 22:29:04 -0700 Subject: [PATCH] [wpimath] Fix C++ pose estimator poseEstimate initialization (#7249) --- .../estimator/DifferentialDrivePoseEstimator.cpp | 4 +++- .../cpp/estimator/MecanumDrivePoseEstimator.cpp | 4 +++- .../native/include/frc/estimator/PoseEstimator.h | 5 ++++- .../frc/estimator/SwerveDrivePoseEstimator.h | 4 +++- .../DifferentialDrivePoseEstimatorTest.cpp | 14 ++++++++++++-- .../estimator/MecanumDrivePoseEstimatorTest.cpp | 13 +++++++++++-- .../cpp/estimator/SwerveDrivePoseEstimatorTest.cpp | 7 ++++++- 7 files changed, 42 insertions(+), 9 deletions(-) diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index bf844a12aa8..6ddc916a69e 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -24,4 +24,6 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( : PoseEstimator( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), - m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} + m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} { + ResetPose(m_odometryImpl.GetPose()); +} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 7c75e71ffee..5db1df74c06 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -26,4 +26,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const wpi::array& visionMeasurementStdDevs) : PoseEstimator( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), - m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} + m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) { + ResetPose(m_odometryImpl.GetPose()); +} diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index 506bb69d945..c5981e7bfb8 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -45,6 +45,9 @@ class WPILIB_DLLEXPORT PoseEstimator { /** * Constructs a PoseEstimator. * + * @warning The initial pose estimate will always be the default pose, + * regardless of the odometry's current pose. + * * @param kinematics A correctly-configured kinematics object for your * drivetrain. * @param odometry A correctly-configured odometry object for your drivetrain. @@ -60,7 +63,7 @@ class WPILIB_DLLEXPORT PoseEstimator { Odometry& odometry, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) { + : m_odometry(odometry) { for (size_t i = 0; i < 3; ++i) { m_q[i] = stateStdDevs[i] * stateStdDevs[i]; } diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 8aeec4e5b3b..7205771e50b 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -85,7 +85,9 @@ class SwerveDrivePoseEstimator : PoseEstimator, wpi::array>( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), - m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} + m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} { + this->ResetPose(initialPose); + } private: SwerveDriveOdometry m_odometryImpl; diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 3ae22482492..029902b58cd 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -359,8 +359,18 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) { 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}}; + kinematics, + frc::Rotation2d{}, + 0_m, + 0_m, + 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 estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m, diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index ef2fe8919fd..f1f7be98a17 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -368,8 +368,17 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) { 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}}; + kinematics, + frc::Rotation2d{}, + frc::MecanumDriveWheelPositions{}, + 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 estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m}, diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index ec19d387f56..d62200176c0 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -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{}};