[wpimath] Remove swerve wrappers for odometry and pose estimation, move wheel positions operations to kinematics (#6673)

wpimath/src/main/native/include/frc/estimator/PoseEstimator.h

@@ -36,7 +36,7 @@ namespace frc {
  * @tparam WheelSpeeds Wheel speeds type.
  * @tparam WheelPositions Wheel positions type.
  */
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 class WPILIB_DLLEXPORT PoseEstimator {
  public:
   /**

wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc

@@ -8,7 +8,7 @@
 
 namespace frc {
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
     Kinematics<WheelSpeeds, WheelPositions>& kinematics,
     Odometry<WheelSpeeds, WheelPositions>& odometry,
@@ -22,7 +22,7 @@ PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
   SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
     const wpi::array<double, 3>& visionMeasurementStdDevs) {
   wpi::array<double, 3> r{wpi::empty_array};
@@ -42,7 +42,7 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
   }
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
     const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
     const Pose2d& pose) {
@@ -51,13 +51,13 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
   m_poseBuffer.Clear();
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
     const {
   return m_odometry.GetPose();
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
     units::second_t timestamp) const {
   // TODO Replace with std::optional::transform() in C++23
@@ -70,7 +70,7 @@ std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
   }
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
   // Step 0: If this measurement is old enough to be outside the pose buffer's
@@ -128,14 +128,14 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
   }
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
     const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
   return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                         wheelPositions);
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
     units::second_t currentTime, const Rotation2d& gyroAngle,
     const WheelPositions& wheelPositions) {
@@ -149,7 +149,7 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
   return GetEstimatedPosition();
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
 PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
     Kinematics<WheelSpeeds, WheelPositions>& kinematics,
@@ -160,8 +160,8 @@ PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
     return endValue;
   } else {
     // Find the new wheel distance measurements.
-    WheelPositions wheels_lerp =
-        this->wheelPositions.Interpolate(endValue.wheelPositions, i);
+    WheelPositions wheels_lerp = kinematics.Interpolate(
+        this->wheelPositions, endValue.wheelPositions, i);
 
     // Find the new gyro angle.
     auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -14,7 +14,6 @@
 #include "frc/geometry/Rotation2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
-#include "frc/kinematics/SwerveDriveWheelPositions.h"
 #include "units/time.h"
 
 namespace frc {
@@ -32,8 +31,8 @@ namespace frc {
  */
 template <size_t NumModules>
 class SwerveDrivePoseEstimator
-    : public PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
-                           SwerveDriveWheelPositions<NumModules>> {
+    : public PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
+                           wpi::array<SwerveModulePosition, NumModules>> {
  public:
   /**
    * Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -83,69 +82,11 @@ class SwerveDrivePoseEstimator
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs)
-      : PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
-                      SwerveDriveWheelPositions<NumModules>>(
+      : PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
+                      wpi::array<SwerveModulePosition, NumModules>>(
             kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
         m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
 
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset in the user's robot code.
-   * The library automatically takes care of offsetting the gyro angle.
-   *
-   * @param gyroAngle       The angle reported by the gyroscope.
-   * @param modulePositions The current distance and rotation measurements of
-   *                        the swerve modules.
-   * @param pose            The position on the field that your robot is at.
-   */
-  void ResetPosition(
-      const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-      const Pose2d& pose) {
-    PoseEstimator<
-        SwerveDriveWheelSpeeds<NumModules>,
-        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
-                                                              {modulePositions},
-                                                              pose);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param gyroAngle       The current gyro angle.
-   * @param modulePositions The current distance and rotation measurements of
-   *                        the swerve modules.
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d Update(
-      const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return PoseEstimator<
-        SwerveDriveWheelSpeeds<NumModules>,
-        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
-                                                       {modulePositions});
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param currentTime     Time at which this method was called, in seconds.
-   * @param gyroAngle       The current gyro angle.
-   * @param modulePositions The current distance traveled and rotations of
-   *                        the swerve modules.
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d UpdateWithTime(
-      units::second_t currentTime, const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
-                         SwerveDriveWheelPositions<NumModules>>::
-        UpdateWithTime(currentTime, gyroAngle, {modulePositions});
-  }
-
  private:
   SwerveDriveOdometry<NumModules> m_odometryImpl;
 };