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

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 {};
     }