Resolve TODO comments in MultiPhotonPoseEstimator (#144)

- Update `MultiPhotonPoseEstimator` to use
  `resetHeadingData(double, Rotation2d)`
- Update `PhotonVisionPosePublisher` to implement `AutoCloseable`

Author: kcooney

vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java

@@ -226,7 +226,7 @@ private SimCameraProperties createSimProperties() {
     public void close() {
       photonCamera.close();
       cameraPosePublisher.close();
-      // TODO: Update PhotonVisionPosePublisher to support close() and call it here
+      robotPosePublisher.close();
     }
   }
 
@@ -355,11 +355,8 @@ public void resetHeadingData(double timestampSeconds, Rotation2d heading) {
   }
 
   public void resetHeadingData(double timestampSeconds, Rotation3d heading) {
-    // TODO: Use PhotonPoseEstimator.resetHeadingData(double, Rotation2d) once we use a version of
-    // PhotonVision that includes it (see  https://github.com/PhotonVision/photonvision/pull/2013).
     for (PhotonCameraWrapper<C> cameraWrapper : cameraWrappers) {
-      cameraWrapper.estimator.resetHeadingData(timestampSeconds, heading.toRotation2d());
-      cameraWrapper.estimator.addHeadingData(timestampSeconds, heading);
+      cameraWrapper.estimator.resetHeadingData(timestampSeconds, heading);
     }
   }
 

vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java

@@ -41,7 +41,7 @@
  *
  * @since 2.0.0
  */
-public final class PhotonVisionPosePublisher {
+public final class PhotonVisionPosePublisher implements AutoCloseable {
   /**
    * How much time we expect to pass between receiving pose estimates from PhotonVision when an
    * AprilTag is visible. Empirically, this is 0.1 seconds.
@@ -105,6 +105,15 @@ public void publish(List<EstimatedRobotPose> poseEstimates) {
     aprilTagPosePublisher.publish(aprilTagPoses);
   }
 
+  /**
+   * @since 2.1.0
+   */
+  @Override
+  public void close() {
+    robotPosePublisher.close();
+    aprilTagPosePublisher.close();
+  }
+
   /** Gets the robot pose from the EstimatedRobotPose and converts it to a timestamped value. */
   private TimestampedValue<Pose3d> getRobotPoseFromEstimatedRobotPose(
       EstimatedRobotPose estimatedRobotPose) {

vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java

@@ -1,5 +1,5 @@
 /*
-Copyright 2025 Prospect Robotics SWENext Club
+Copyright 2025-2026 Prospect Robotics SWENext Club
 
 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
@@ -32,7 +32,7 @@
  * data that takes longer to produce than the frequency of the robot's event loop (for example,
  * estimated robot pose data produced by a camera).
  */
-final class TimestampedStructPublisher<S> {
+final class TimestampedStructPublisher<S> implements AutoCloseable {
   private static final long MICROS_PER_SECOND = 1_000_000;
 
   /**
@@ -106,6 +106,11 @@ public void publish(List<TimestampedValue<S>> timestampedValues) {
     }
   }
 
+  @Override
+  public void close() {
+    publisher.close();
+  }
+
   private long currentTimeMicros() {
     return (long) (fpgaTimestampSupplier.get() * MICROS_PER_SECOND);
   }