Merge branch 'main' of https://github.com/LimelightVision/limelightli…

…b-wpijava into main
LimelightVision · Nov 5, 2024 · 61b39e6 · 61b39e6
2 parents 9b8452b + 69ad9fd
commit 61b39e6
Showing 1 changed file with 15 additions and 7 deletions.
diff --git a/LimelightHelpers.java b/LimelightHelpers.java
@@ -540,6 +540,7 @@ public static class PoseEstimate {
         public double avgTagArea;
 
         public RawFiducial[] rawFiducials; 
+        public boolean isMegaTag2;
 
         /**
          * Instantiates a PoseEstimate object with default values
@@ -553,11 +554,12 @@ public PoseEstimate() {
             this.avgTagDist = 0;
             this.avgTagArea = 0;
             this.rawFiducials = new RawFiducial[]{};
+            this.isMegaTag2 = false;
         }
 
         public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, 
             int tagCount, double tagSpan, double avgTagDist, 
-            double avgTagArea, RawFiducial[] rawFiducials) {
+            double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) {
 
             this.pose = pose;
             this.timestampSeconds = timestampSeconds;
@@ -567,6 +569,7 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency,
             this.avgTagDist = avgTagDist;
             this.avgTagArea = avgTagArea;
             this.rawFiducials = rawFiducials;
+            this.isMegaTag2 = isMegaTag2;
         }
 
     }
@@ -666,7 +669,7 @@ private static double extractArrayEntry(double[] inData, int position){
         return inData[position];
     }
 
-    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) {
+    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) {
         DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName);
 
         TimestampedDoubleArray tsValue = poseEntry.getAtomic();
@@ -708,7 +711,7 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr
             }
         }
 
-        return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials);
+        return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2);
     }
 
     /**
@@ -802,6 +805,7 @@ public static void printPoseEstimate(PoseEstimate pose) {
         System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
         System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
         System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+        System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2);
         System.out.println();
 
         if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
@@ -824,6 +828,10 @@ public static void printPoseEstimate(PoseEstimate pose) {
         }
     }
 
+    public static Boolean validPoseEstimate(PoseEstimate pose) {
+        return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
+    }
+
     public static NetworkTable getLimelightNTTable(String tableName) {
         return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
     }
@@ -1228,7 +1236,7 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_wpiblue");
+        return getBotPoseEstimate(limelightName, "botpose_wpiblue", false);
     }
 
     /**
@@ -1239,7 +1247,7 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue");
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true);
     }
 
     /**
@@ -1263,7 +1271,7 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) {
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_wpired");
+        return getBotPoseEstimate(limelightName, "botpose_wpired", false);
     }
 
     /**
@@ -1273,7 +1281,7 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
      * @return
      */
     public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
-        return getBotPoseEstimate(limelightName, "botpose_orb_wpired");
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true);
     }
 
     /**