fix race condition

Team766 · Oct 26, 2024 · 0f45bdf · 0f45bdf
1 parent 069de95
commit 0f45bdf
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 29 deletions.
diff --git a/src/main/java/com/team766/orin/GetApriltagPoseData.java b/src/main/java/com/team766/orin/GetApriltagPoseData.java
@@ -15,27 +15,23 @@ public static ArrayList<AprilTag> getAllTags() {
         double[] tagData;
 
         try {
-            tagIds = GetOrinRawValue.getTagIds();
             tagData = GetOrinRawValue.getRawPoseData();
         } catch (ValueNotFoundOnTableError e) {
             return apriltags; // Can just return an array of zero apriltags here
         }
 
-        if (tagData.length / 3 != tagIds.length) return apriltags;
+        if (tagData.length % 4 != 0 || tagData.length == 0) return apriltags;
 
-        int counter = 0;
-        for (int i = 0; i < tagIds.length; i++) {
+        for (int i = 0; i < tagData.length; i+=4) {
             AprilTag tag =
                     new AprilTag(
-                            tagIds[i],
+                            (int) tagData[i],
                             new Pose3d(
                                     new Translation3d(
-                                            tagData[counter++],
-                                            tagData[counter++],
-                                            tagData[counter++]),
+                                            tagData[i+1],
+                                            tagData[i+2],
+                                            tagData[i+3]),
                                     new Rotation3d()));
-            // Remove used values from array
-
             apriltags.add(tag);
         }
         return apriltags;

diff --git a/src/main/java/com/team766/orin/GetOrinRawValue.java b/src/main/java/com/team766/orin/GetOrinRawValue.java
@@ -25,10 +25,6 @@ public class GetOrinRawValue {
     private static double[] arr = new double[] {Double.NEGATIVE_INFINITY};
     private static DoubleArrayEntry poseValues = pose.getEntry(arr);
 
-    private static IntegerArrayTopic tagId = table.getIntegerArrayTopic("tag_id");
-    private static long[] array1 = new long[] {Integer.MIN_VALUE};
-    private static IntegerArrayEntry tagIdValues = tagId.getEntry(array1);
-
     public static double[] getRawPoseData() throws ValueNotFoundOnTableError {
         if (poseValues.get().length == 1 && poseValues.get()[0] == Double.NEGATIVE_INFINITY) {
             throw new ValueNotFoundOnTableError("Pose Data Not Present on Table");
@@ -37,23 +33,8 @@ public static double[] getRawPoseData() throws ValueNotFoundOnTableError {
         return poseValues.get();
     }
 
-    public static int[] getTagIds() throws ValueNotFoundOnTableError {
-        long[] longValues = tagIdValues.get();
-
-        if (longValues.length == 1 && longValues[0] == Integer.MIN_VALUE) {
-            throw new ValueNotFoundOnTableError("Tag ID Data Not Present on Table");
-        }
-
-        int[] toReturn = new int[longValues.length];
-        for (int i = 0; i < longValues.length; i++) {
-            toReturn[i] = (int) longValues[i];
-        }
-        return toReturn;
-    }
-
     public static void closeAll() {
         poseValues.close();
-        tagIdValues.close();
     }
 }