diff --git a/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java b/src/main/java/com/techhounds/houndutil/houndlib/AprilTagPhotonCamera.java index 690d412..901cff4 100644 --- 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 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());