finished remaining vision pose estimator tests and trying to fix phot…

…on vision test errors

src/main/java/frc/lib/FaultLogger.java

@@ -48,12 +48,18 @@ public static interface FaultReporter {
   private static FaultsTable activeAlerts;
   private static FaultsTable totalAlerts;
 
+  private static boolean _hasBeenSetup = false;
+
   /** Must be called to setup the fault logger. */
   public static void setup(NetworkTableInstance ntInst) {
+    if (_hasBeenSetup) return;
+
     var base = ntInst.getTable("Faults");
 
     activeAlerts = new FaultsTable(base, "Active Faults");
     totalAlerts = new FaultsTable(base, "Total Faults");
+
+    _hasBeenSetup = true;
   }
 
   /** Must be called to setup the fault logger. */

src/main/java/frc/lib/UnitTestingUtil.java

@@ -44,31 +44,23 @@ public static void setupTests() {
     DogLog.setEnabled(false); // disabling doglog since it logs to the default nt instance
 
     FaultLogger.setup(_ntInst);
-
-    FaultLogger.clear();
-    FaultLogger.unregisterAll();
     FaultLogger.enableConsole(false);
 
     // delay 100 ms to wait for CTRE device enable
     Timer.delay(0.100);
   }
 
-  /** Resets the CommandScheduler and the test NT instance. */
-  public static void reset() {
-    _ntInst.close();
-    _ntInst = null;
-
-    CommandScheduler.getInstance().unregisterAllSubsystems();
-    CommandScheduler.getInstance().cancelAll();
-  }
-
   /**
    * Resets CommandScheduler and NT and closes all closeables. Please call in an @AfterEach method!
    *
    * @param closeables All closeables that need to be closed.
    */
   public static void reset(AutoCloseable... closeables) {
-    reset();
+    _ntInst.close();
+    _ntInst = null;
+
+    CommandScheduler.getInstance().unregisterAllSubsystems();
+    CommandScheduler.getInstance().cancelAll();
 
     for (AutoCloseable closeable : closeables) {
       try {
@@ -77,6 +69,11 @@ public static void reset(AutoCloseable... closeables) {
         e.printStackTrace();
       }
     }
+
+    FaultLogger.clear();
+    FaultLogger.unregisterAll();
+
+    HAL.shutdown();
   }
 
   /**

src/test/java/frc/robot/PhotonTest.java

@@ -0,0 +1,42 @@
+package frc.robot;
+
+import static frc.lib.UnitTestingUtil.getNtInst;
+import static frc.lib.UnitTestingUtil.reset;
+import static frc.lib.UnitTestingUtil.setupTests;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Test;
+import org.photonvision.PhotonCamera;
+import org.photonvision.simulation.PhotonCameraSim;
+
+// WHY IS THIS THROWING A PHOTON ERROR?
+@Disabled
+public class PhotonTest {
+  private PhotonCamera _cam;
+  private PhotonCameraSim _sim;
+
+  @BeforeEach
+  public void setup() {
+    setupTests();
+
+    _cam = new PhotonCamera(getNtInst(), "Cam");
+    _sim = new PhotonCameraSim(_cam);
+  }
+
+  @AfterEach
+  public void close() {
+    reset(_cam, _sim);
+  }
+
+  @Test
+  public void testOne() {
+    System.out.println("Test One");
+  }
+
+  @Test
+  public void testTwo() {
+    System.out.println("Test Two");
+  }
+}

src/test/java/frc/robot/SwerveTest.java

@@ -9,6 +9,8 @@
 import frc.robot.subsystems.Swerve;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Test;
 
 public class SwerveTest {
   private Swerve _swerve;
@@ -25,7 +27,8 @@ public void close() {
     reset(_swerve);
   }
 
-  // @Test
+  @Disabled
+  @Test
   public void driveTo() {
     // TODO: ts don't work
     var goal = new Pose2d(0.5, 0, Rotation2d.fromDegrees(5));

src/test/java/frc/robot/VisionPoseEstimatorTest.java

@@ -74,6 +74,9 @@ public void setup() {
         VisionPoseEstimator.buildFromConstants(
             testCam, getNtInst(), _fieldLayout, this::dummyGyroHeading);
 
+    // specific corner noise for these tests
+    _testCam.getCameraSim().prop.setCalibError(0.01, 0.001);
+
     _visionSystemSim = new VisionSystemSim("");
     _visionSystemSim.addCamera(_testCam.getCameraSim(), _testCam.robotToCam);
   }
@@ -121,7 +124,7 @@ public void singleTagResults() {
   @Test
   public void singleTagEstimate() {
     _visionSystemSim.addVisionTargets(
-        new VisionTargetSim(_fieldLayout.getTagPose(1).get(), TargetModel.kAprilTag36h11, 1));
+        new VisionTargetSim(_fieldLayout.getTagPose(2).get(), TargetModel.kAprilTag36h11, 2));
 
     _visionSystemSim.update(Pose2d.kZero);
 
@@ -132,6 +135,9 @@ public void singleTagEstimate() {
 
     var estimate = estimates.get(0);
 
+    // this test sometimes fails for some unknown reason, so this is here in case
+    System.out.println(estimate);
+
     assert estimate.isValid();
 
     // pose validity
@@ -142,13 +148,13 @@ public void singleTagEstimate() {
     assertEquals(0, estimate.pose().getRotation().getY(), 1e-2);
     assertEquals(0, estimate.pose().getRotation().getZ(), 1e-2);
 
-    // should see only ID 1
-    assertArrayEquals(new int[] {1}, estimate.detectedTags());
+    // should see only ID 2
+    assertArrayEquals(new int[] {2}, estimate.detectedTags());
 
     // distance validity
     assertEquals(
         _fieldLayout
-            .getTagPose(1)
+            .getTagPose(2)
             .get()
             .getTranslation()
             .getDistance(Pose3d.kZero.getTranslation()),
@@ -239,7 +245,37 @@ public void boundsFilter() {
 
   @Test
   public void ambiguityFilter() {
-    // TODO
+    _visionSystemSim.addVisionTargets(
+        new VisionTargetSim(_fieldLayout.getTagPose(5).get(), TargetModel.kAprilTag36h11, 5));
+
+    _visionSystemSim.update(Pose2d.kZero);
+
+    _testCam.update();
+
+    var estimates = _testCam.getNewEstimates(); // 1 estimate
+    assertEquals(1, estimates.size());
+
+    var estimate = estimates.get(0);
+
+    assertFalse(estimate.isValid());
+
+    assert estimate.ambiguity() > _testCam.ambiguityThreshold;
+
+    // should see only ID 5
+    assertArrayEquals(new int[] {5}, estimate.detectedTags());
+
+    // distance validity (is still accurate in ambigious scenarios)
+    assertEquals(
+        _fieldLayout
+            .getTagPose(5)
+            .get()
+            .getTranslation()
+            .getDistance(Pose3d.kZero.getTranslation()),
+        estimate.avgTagDistance(),
+        1e-2);
+
+    // std devs validity
+    assertArrayEquals(new double[] {-1, -1, -1}, estimate.stdDevs());
   }
 
   @Test
@@ -385,6 +421,35 @@ public void estimateSort() {
 
   @Test
   public void disambiguation() {
-    // TODO
+    _visionSystemSim.addVisionTargets(
+        new VisionTargetSim(_fieldLayout.getTagPose(2).get(), TargetModel.kAprilTag36h11, 2));
+
+    _visionSystemSim.update(Pose2d.kZero);
+
+    _testCam.update();
+
+    var estimates = _testCam.getNewEstimates(); // 1 estimate
+    assertEquals(1, estimates.size());
+
+    var estimate = estimates.get(0);
+
+    // this test sometimes fails for some unknown reason, so this is here in case
+    System.out.println(estimate);
+
+    assert estimate.isValid();
+
+    // everything alr checked in singleTagEstimate, so just checking disambiguation here ⤵
+
+    var closerError =
+        estimate.pose().getRotation().toRotation2d().minus(dummyGyroHeading(estimate.timestamp()));
+    var furtherError =
+        estimate
+            .altPose()
+            .getRotation()
+            .toRotation2d()
+            .minus(dummyGyroHeading(estimate.timestamp()));
+
+    // pose's heading should be closer to the gyro than altPose's heading
+    assert Math.abs(closerError.getRadians()) < Math.abs(furtherError.getRadians());
   }
 }

src/test/java/frc/robot/WheelRadiusCharacterizationTest.java

@@ -10,7 +10,10 @@
 import frc.robot.subsystems.Swerve;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Test;
 
+@Disabled
 public class WheelRadiusCharacterizationTest {
   // chassis omega during characterization (TODO: this would be tuneable later)
   private final double velOmega = 1;
@@ -31,7 +34,7 @@ public void close() {
     reset(_swerve);
   }
 
-  // @Test
+  @Test
   public void wheelRadiusCharacterization() {
     // run enough ticks to complete 4pi radians at the given characterization omega
     run(_characterization, (int) ((Math.PI * 4 / velOmega) / TICK_RATE.in(Seconds)));