From 9ac10502642e897ada42f2fecd028764727f35db Mon Sep 17 00:00:00 2001
From: Andrew Gasser <agasser@users.noreply.github.com>
Date: Tue, 14 Feb 2023 13:30:30 -0600
Subject: [PATCH] Handle IOException in Apriltag example (#807)

* Deal with IOException

* Fix import
---
 .../java/frc/robot/PhotonCameraWrapper.java   | 35 +++++++++++--------
 1 file changed, 21 insertions(+), 14 deletions(-)

diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
index a2160391d..0f45153b8 100644
--- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
+++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
@@ -27,6 +27,7 @@ package frc.robot;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.Constants.VisionConstants;
 import java.io.IOException;
 import java.util.Optional;
@@ -36,25 +37,27 @@ import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 
 public class PhotonCameraWrapper {
-    public PhotonCamera photonCamera;
-    public PhotonPoseEstimator photonPoseEstimator;
+    private PhotonCamera photonCamera;
+    private PhotonPoseEstimator photonPoseEstimator;
 
     public PhotonCameraWrapper() {
-        AprilTagFieldLayout atfl = null;
-        try {
-            atfl = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
-        } catch (IOException e) {
-            e.printStackTrace();
-        }
-
         // Change the name of your camera here to whatever it is in the PhotonVision UI.
         photonCamera = new PhotonCamera(VisionConstants.cameraName);
 
-        // Create pose estimator
-        photonPoseEstimator =
-                new PhotonPoseEstimator(
-                        atfl, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam);
-        photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        try {
+            // Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the field.
+            AprilTagFieldLayout fieldLayout = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
+            // Create pose estimator
+            photonPoseEstimator =
+                    new PhotonPoseEstimator(
+                            fieldLayout, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam);
+            photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+        } catch (IOException e) {
+            // The AprilTagFieldLayout failed to load. We won't be able to estimate poses if we don't know
+            // where the tags are.
+            DriverStation.reportError("Failed to load AprilTagFieldLayout", e.getStackTrace());
+            photonPoseEstimator = null;
+        }
     }
 
     /**
@@ -63,6 +66,10 @@ public class PhotonCameraWrapper {
      *     the estimate
      */
     public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
+        if (photonPoseEstimator == null) {
+            // The field layout failed to load, so we cannot estimate poses.
+            return Optional.empty();
+        }
         photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
         return photonPoseEstimator.update();
     }