Rename private methods and locals; extract locals; add comments

Author: kcooney

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

@@ -13,9 +13,9 @@
 import java.util.List;
 import java.util.Optional;
 import java.util.function.Supplier;
-import java.util.stream.Stream;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 /**
  * Publishes timestamped pose estimates from a camera.
@@ -76,32 +76,39 @@ public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilT
    * @param poseEstimates The estimated locations (with the blue driver station as the origin).
    */
   public void publish(List<EstimatedRobotPose> poseEstimates) {
-    robotPosePublisher.publish(
+    // Publish all the estimated robot positions.
+    List<TimestampedValue<Pose3d>> robotPoses =
+        poseEstimates.stream().map(this::getRobotPoseFromEstimatedRobotPose).toList();
+    robotPosePublisher.publish(robotPoses);
+
+    // Publish the location of the AprilTag used for the above estimated positions.
+    List<TimestampedValue<Pose3d>> aprilTagPoses =
         poseEstimates.stream()
-            .map(PhotonVisionPosePublisher::toRobotPoseTimestampedValue)
-            .toList());
-    aprilTagPosePublisher.publish(
-        poseEstimates.stream().flatMap(this::toAprilTagPoseTimestampedValue).toList());
+            .map(this::getBestVisibleAprilTag)
+            .flatMap(Optional::stream) // Convert Stream<Optional<V>> -> Stream<V>
+            .toList();
+    aprilTagPosePublisher.publish(aprilTagPoses);
   }
 
-  private static TimestampedValue<Pose3d> toRobotPoseTimestampedValue(EstimatedRobotPose pose) {
+  /** Gets the robot pose from the EstimatedRobotPose and convert to a timestamped value. */
+  private TimestampedValue<Pose3d> getRobotPoseFromEstimatedRobotPose(EstimatedRobotPose pose) {
     return TimestampedValue.withFpgaTimestamp(
         pose.timestampSeconds, Units.Seconds, pose.estimatedPose);
   }
 
-  private Stream<TimestampedValue<Pose3d>> toAprilTagPoseTimestampedValue(EstimatedRobotPose pose) {
-    if (pose.targetsUsed.isEmpty()) {
-      return Stream.empty();
+  /** Gets the highest-quality AprilTag used to estimate the position of the robot. */
+  private Optional<TimestampedValue<Pose3d>> getBestVisibleAprilTag(
+      EstimatedRobotPose estimatedRobotPose) {
+    List<PhotonTrackedTarget> visibleAprilTags = estimatedRobotPose.targetsUsed;
+    if (visibleAprilTags.isEmpty()) {
+      return Optional.empty();
     }
 
-    // Find the pose of the "best" AprilTag used for pose estimation and convert it to a timestamped
-    // value.
     Optional<Pose3d> poseOfBestAprilTag =
-        aprilTagFieldLayout.getTagPose(pose.targetsUsed.get(0).fiducialId);
-    return poseOfBestAprilTag
-        .map(
-            tagPose ->
-                TimestampedValue.withFpgaTimestamp(pose.timestampSeconds, Units.Seconds, tagPose))
-        .stream();
+        aprilTagFieldLayout.getTagPose(visibleAprilTags.get(0).fiducialId);
+    return poseOfBestAprilTag.map(
+        aprilTagPose ->
+            TimestampedValue.withFpgaTimestamp(
+                estimatedRobotPose.timestampSeconds, Units.Seconds, aprilTagPose));
   }
 }