Add mitigation for OOB apriltags

frc868 · Mar 23, 2024 · dec007b · dec007b
1 parent 3ab489d
commit dec007b
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java b/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java
@@ -95,6 +95,10 @@ public Optional<EstimatedRobotPose> getEstimatedGlobalPose(
                 estimatedRobotPose = photonEstimatedRobotPose.get().estimatedPose;
                 detectedAprilTags = getPosesFromTargets(result.targets, estimatedRobotPose,
                         robotToCam);
+
+                if (estimatedRobotPose.getZ() > 1 || estimatedRobotPose.getZ() < -1) {
+                    return Optional.empty();
+                }
             } else {
                 detectedAprilTags = new Pose3d[0];
                 estimatedRobotPose = new Pose3d(-100, -100, -100, new Rotation3d());