Warn when getBestCameraToTarget returns 0, 0, 0 (#1334)

Resolves #915
PhotonVision · Jun 1, 2024 · 6ff7b3e · 6ff7b3e
1 parent e34b114
commit 6ff7b3e
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.apriltag.AprilTagDetection;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.wpilibj.DriverStation;
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
@@ -364,6 +365,9 @@ public boolean hasSubContours() {
     }
 
     public Transform3d getBestCameraToTarget3d() {
+        if (m_bestCameraToTarget3d.equals(new Transform3d())) {
+            DriverStation.reportWarning("3d mode is not enabled.", false);
+        }
         return m_bestCameraToTarget3d;
     }
 

diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h
@@ -22,6 +22,7 @@
 #include <utility>
 #include <vector>
 
+#include <frc/Errors.h>
 #include <frc/geometry/Transform3d.h>
 #include <wpi/SmallVector.h>
 
@@ -138,7 +139,12 @@ class PhotonTrackedTarget {
    * reprojection error is the ambiguity, which is between 0 and 1.
    * @return The pose of the target relative to the robot.
    */
-  frc::Transform3d GetBestCameraToTarget() const { return bestCameraToTarget; }
+  frc::Transform3d GetBestCameraToTarget() const {
+    if (bestCameraToTarget == frc::Transform3d()) {
+      FRC_ReportError(frc::warn::Warning, "3d mode is not enabled");
+    }
+    return bestCameraToTarget;
+  }
 
   /**
    * Get the transform that maps camera space (X = forward, Y = left, Z = up) to