[wpimath] Add PoseEstimator.sampleAt() (#6426)

wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java

@@ -21,6 +21,7 @@ import edu.wpi.first.math.numbers.N3;
 import java.util.Map;
 import java.util.NoSuchElementException;
 import java.util.Objects;
+import java.util.Optional;
 
 /**
  * This class wraps {@link Odometry} to fuse latency-compensated vision measurements with encoder
@@ -125,6 +126,16 @@ public class PoseEstimator<T extends WheelPositions<T>> {
     return m_odometry.getPoseMeters();
   }
 
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestampSeconds The pose's timestamp in seconds.
+   * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+   */
+  public Optional<Pose2d> sampleAt(double timestampSeconds) {
+    return m_poseBuffer.getSample(timestampSeconds).map(record -> record.poseMeters);
+  }
+
   /**
    * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
    * while still accounting for measurement noise.

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

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
@@ -89,6 +91,15 @@ class WPILIB_DLLEXPORT PoseEstimator {
    */
   Pose2d GetEstimatedPosition() const;
 
+  /**
+   * Return the pose at a given timestamp, if the buffer is not empty.
+   *
+   * @param timestamp The pose's timestamp.
+   * @return The pose at the given timestamp (or std::nullopt if the buffer is
+   * empty).
+   */
+  std::optional<Pose2d> SampleAt(units::second_t timestamp) const;
+
   /**
    * Adds a vision measurement to the Kalman Filter. This will correct
    * the odometry pose estimate while still accounting for measurement noise.

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

@@ -57,6 +57,19 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
   return m_odometry.GetPose();
 }
 
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
+    units::second_t timestamp) const {
+  // TODO Replace with std::optional::transform() in C++23
+  std::optional<PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord>
+      sample = m_poseBuffer.Sample(timestamp);
+  if (sample) {
+    return sample->pose;
+  } else {
+    return std::nullopt;
+  }
+}
+
 template <typename WheelSpeeds, WheelPositions WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {

wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h

@@ -100,7 +100,7 @@ class TimeInterpolatableBuffer {
    *
    * @param time The time at which to sample the buffer.
    */
-  std::optional<T> Sample(units::second_t time) {
+  std::optional<T> Sample(units::second_t time) const {
     if (m_pastSnapshots.empty()) {
       return {};
     }