add getSample

Author: mwitcpalek

build.gradle

@@ -6,7 +6,7 @@ plugins {
     id "maven-publish"
     id "org.jetbrains.kotlin.jvm" version "1.9.0" //1.7.21
     id "com.google.devtools.ksp" version "1.9.0-1.0.13" //1.7.21-1.0.8
-    id "edu.wpi.first.GradleRIO" version "2025.1.1"
+    id "edu.wpi.first.GradleRIO" version "2025.2.1"
     id "com.diffplug.spotless" version "6.12.1"
 }
 

src/main/java/org/strykeforce/swerve/PoseEstimatorOdometryStrategy.java

@@ -165,6 +165,10 @@ public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition... modulePositio
     return odometry.update(gyroAngle, modulePositions);
   }
 
+  public Pose2d getSample(double timestampSeconds) {
+    return odometry.sampleAt(timestampSeconds).get();
+  }
+
   // @Override
   // public Pose2d updateWithTime(
   //     double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {