[wpimath] Fix C++ pose estimator poseEstimate initialization (wpilibs…

…uite#7249)
bzlmodRio · Oct 22, 2024 · 6745fc7 · 6745fc7
1 parent 40af8db
commit 6745fc7
Show file tree

Hide file tree

Showing 7 changed files with 42 additions and 9 deletions.
diff --git 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<DifferentialDriveWheelSpeeds,
                     DifferentialDriveWheelPositions>(
           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
@@ -26,4 +26,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     const wpi::array<double, 3>& visionMeasurementStdDevs)
     : PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
           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
@@ -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<WheelSpeeds, WheelPositions>& odometry,
                 const wpi::array<double, 3>& stateStdDevs,
                 const wpi::array<double, 3>& 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
@@ -85,7 +85,9 @@ class SwerveDrivePoseEstimator
       : PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
                       wpi::array<SwerveModulePosition, NumModules>>(
             kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
-        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
+        m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
+    this->ResetPose(initialPose);
+  }
 
  private:
   SwerveDriveOdometry<NumModules> m_odometryImpl;

diff --git 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
@@ -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
@@ -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{}};