Use queue for multithread odometry updates

src/main/java/frc/robot/Constants.java

@@ -128,7 +128,7 @@ public static final class SwerveConstants {
 
     public static final double maxModuleSpeed = Units.feetToMeters(14.5);
 
-    public static final int odometryUpdateFrequency = 50; // 50 Hz
+    public static final int odometryUpdateFrequency = 100; // 100 Hz
   }
 
   public static final class AutoConstants {

src/main/java/frc/robot/RobotContainer.java

@@ -262,7 +262,7 @@ private boolean errorsPresent() {
   }
 
   public void updateSwerveOdometry() {
-    swerve.updateOdometry();
+    swerve.updateOdometryQueue();
   }
 
   /**

src/main/java/frc/robot/subsystems/Swerve.java

@@ -21,6 +21,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N3;
@@ -53,6 +54,7 @@
 import frc.robot.util.LimelightHelpers;
 import frc.robot.util.LimelightHelpers.LimelightTarget_Fiducial;
 import java.util.ArrayList;
+import java.util.Collections;
 import java.util.List;
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
@@ -102,6 +104,11 @@ public class Swerve extends VirtualSubsystem {
   public static final Lock odometryLock = new ReentrantLock();
   private SwerveDrivePoseEstimator poseEstimator;
 
+  private List<SwerveDriveWheelPositions> wheelPositionsQueue =
+      Collections.synchronizedList(new ArrayList<>(4));
+  private List<Rotation2d> gyroAnglesQueue = Collections.synchronizedList(new ArrayList<>(4));
+  private List<Double> timestampsQueue = Collections.synchronizedList(new ArrayList<>(4));
+
   private double limelightLastDetectedTime = 0.0;
 
   private final SysIdRoutine sysIdRoutine =
@@ -305,7 +312,6 @@ public void lockModules() {
   }
 
   public void zeroYaw() {
-    odometryLock.lock();
     Pose2d originalOdometryPosition = poseEstimator.getEstimatedPosition();
 
     setHeading(Rotation2d.fromDegrees(0.0));
@@ -314,7 +320,6 @@ public void zeroYaw() {
         getHeading(),
         getModulePositions(),
         new Pose2d(originalOdometryPosition.getTranslation(), AllianceUtil.getZeroRotation()));
-    odometryLock.unlock();
   }
 
   public void setHeading(Rotation2d rotation) {
@@ -330,10 +335,7 @@ public ChassisSpeeds getChassisSpeeds() {
   }
 
   public Pose2d getPose() {
-    odometryLock.lock();
-    Pose2d pose = poseEstimator.getEstimatedPosition();
-    odometryLock.unlock();
-    return pose;
+    return poseEstimator.getEstimatedPosition();
   }
 
   public void resetPose(Pose2d pose) {
@@ -371,10 +373,30 @@ public SwerveModuleState[] getDesiredStates() {
     };
   }
 
-  public void updateOdometry() {
+  public void updateOdometryQueue() {
     odometryLock.lock();
-    poseEstimator.update(getHeading(), getModulePositions());
-    odometryLock.unlock();
+    try {
+      wheelPositionsQueue.add(new SwerveDriveWheelPositions(getModulePositions()));
+      gyroAnglesQueue.add(getHeading());
+      timestampsQueue.add(Timer.getFPGATimestamp());
+    } finally {
+      odometryLock.unlock();
+    }
+  }
+
+  public void updateOdometry() {
+    int newEntries =
+        Math.min(
+            wheelPositionsQueue.size(), Math.min(gyroAnglesQueue.size(), timestampsQueue.size()));
+
+    for (int i = 0; i < newEntries; i++) {
+      poseEstimator.updateWithTime(
+          timestampsQueue.get(i), gyroAnglesQueue.get(i), wheelPositionsQueue.get(i));
+    }
+
+    wheelPositionsQueue.clear();
+    gyroAnglesQueue.clear();
+    timestampsQueue.clear();
   }
 
   private Vector<N3> getLLStandardDeviations(
@@ -483,10 +505,15 @@ public void periodic() {
     backRightModule.periodic();
 
     odometryLock.lock();
+    try {
+      updateOdometry();
+    } finally {
+      odometryLock.unlock();
+    }
+
     updateVisionPoseEstimates();
 
     field.setRobotPose(poseEstimator.getEstimatedPosition());
-    odometryLock.unlock();
   }
 
   @Override