debug

Author: ds12a

RobotSideCode/wallEYE/src/main/java/WallEye/Tag.java

@@ -21,12 +21,12 @@ public Point getCenter() {
     return new Point(centerX / 4.0, centerY / 4.0);
   }
 
-  public double getArea() {
+  public double getAvgDiag() {
     // Assume rectangle
     double width1 = Math.abs(corners[0].x() - corners[2].x());
     double height1 = Math.abs(corners[0].y() - corners[2].y());
     double width2 = Math.abs(corners[1].x() - corners[3].x());
     double height2 = Math.abs(corners[1].y() - corners[3].y());
-    return (width1 * height1 + width2 * height2) / 2.0;
+    return (Math.hypot(width1, height1) + Math.hypot(width2, height2)) / 2.0;
   }
 }

RobotSideCode/wallEYE/src/main/java/WallEye/WallEyeCam.java

@@ -149,6 +149,14 @@ public void processUDP(String rawDataString, long recievedTime) {
   private void processPoseMode(JsonObject dataCam) {
     newUpdateNum = dataCam.get("Update").getAsInt();
     Pose3d pose1 = parsePose3d(dataCam.get("Pose1").getAsJsonObject());
+
+    if (pose1 == null) {
+      System.err.println(
+          "POSE IS NULL POSE IS NULL POSE IS NULL POSE IS NULL POSE IS NULL POSE IS NULL POSE IS NULL");
+      System.err.println(dataCam.toString());
+      return;
+    }
+
     Pose3d pose2 = parsePose3d(dataCam.get("Pose2").getAsJsonObject());
     double ambiguity = dataCam.get("Ambig").getAsDouble();
 

RobotSideCode/wallEYE/src/main/java/WallEye/WallEyeTagResult.java

@@ -10,7 +10,7 @@
  */
 public class WallEyeTagResult extends WallEyeResult {
   private final List<Tag> tags;
-  private final double[] tagAreas;
+  private final double[] tagDiag;
   private final List<Point> centers;
 
   /**
@@ -36,11 +36,11 @@ public WallEyeTagResult(
     if (tagCorners == null) {
       this.tags = Collections.emptyList();
       this.centers = Collections.emptyList();
-      this.tagAreas = new double[0];
+      this.tagDiag = new double[0];
     } else {
       this.tags = new ArrayList<>(tagCorners.size());
       this.centers = new ArrayList<>(tagCorners.size());
-      this.tagAreas = new double[tagCorners.size()];
+      this.tagDiag = new double[tagCorners.size()];
 
       for (List<List<Double>> tagData : tagCorners) {
         Point[] corners = new Point[4];
@@ -53,7 +53,7 @@ public WallEyeTagResult(
         this.tags.add(tag);
 
         this.centers.add(tag.getCenter());
-        this.tagAreas[this.tags.size() - 1] = tag.getArea();
+        this.tagDiag[this.tags.size() - 1] = tag.getAvgDiag();
       }
     }
   }
@@ -66,8 +66,8 @@ public List<Point> getTagCenters() {
     return centers;
   }
 
-  public double[] getTagAreas() {
-    return tagAreas;
+  public double[] getTagDiags() {
+    return tagDiag;
   }
 
   @Override