Assert that version checking won't throw on startup (#1512)

# Overview

Previously if the coproc came up later, getProperty would return the
string literal "null", which made us print the BFW. Add tests to make
sure that we don't do that anymore by rebooting a sim coproc +
robot in a combination of different orders.

Author: mcm001

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -32,9 +32,7 @@
 import edu.wpi.first.math.numbers.*;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.BooleanSubscriber;
-import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoubleArraySubscriber;
-import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.IntegerEntry;
 import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
@@ -64,13 +62,6 @@ public class PhotonCamera implements AutoCloseable {
     PacketSubscriber<PhotonPipelineResult> resultSubscriber;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
-    DoublePublisher latencyMillisEntry;
-    BooleanPublisher hasTargetEntry;
-    DoublePublisher targetPitchEntry;
-    DoublePublisher targetYawEntry;
-    DoublePublisher targetAreaEntry;
-    DoubleArrayPublisher targetPoseEntry;
-    DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerPublisher pipelineIndexRequest, ledModeRequest;
@@ -86,13 +77,6 @@ public void close() {
         resultSubscriber.close();
         driverModePublisher.close();
         driverModeSubscriber.close();
-        latencyMillisEntry.close();
-        hasTargetEntry.close();
-        targetPitchEntry.close();
-        targetYawEntry.close();
-        targetAreaEntry.close();
-        targetPoseEntry.close();
-        targetSkewEntry.close();
         versionEntry.close();
         inputSaveImgEntry.close();
         outputSaveImgEntry.close();
@@ -111,13 +95,13 @@ public void close() {
 
     private static boolean VERSION_CHECK_ENABLED = true;
     private static long VERSION_CHECK_INTERVAL = 5;
-    private double lastVersionCheckTime = 0;
+    double lastVersionCheckTime = 0;
 
     private long prevHeartbeatValue = -1;
     private double prevHeartbeatChangeTime = 0;
     private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;
 
-    private double prevTimeSyncWarnTime = 0;
+    double prevTimeSyncWarnTime = 0;
     private static final double WARN_DEBOUNCE_SEC = 5;
 
     public static void setVersionCheckEnabled(boolean enabled) {
@@ -396,7 +380,7 @@ public final NetworkTable getCameraTable() {
         return cameraTable;
     }
 
-    private void verifyVersion() {
+    void verifyVersion() {
         if (!VERSION_CHECK_ENABLED) return;
 
         if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
@@ -433,7 +417,7 @@ private void verifyVersion() {
         // Check for connection status. Warn if disconnected.
         else if (!isConnected()) {
             DriverStation.reportWarning(
-                    "PhotonVision coprocessor at path " + path + " is not sending new data.", true);
+                    "PhotonVision coprocessor at path " + path + " is not sending new data.", false);
         }
 
         String versionString = versionEntry.get("");
@@ -448,7 +432,7 @@ else if (!isConnected()) {
                     "PhotonVision coprocessor at path "
                             + path
                             + " has not reported a message interface UUID - is your coprocessor's camera started?",
-                    true);
+                    false);
         } else if (!local_uuid.equals(remote_uuid)) {
             // Error on a verified version mismatch
             // But stay silent otherwise

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -68,7 +68,7 @@
 public class PhotonCameraSim implements AutoCloseable {
     private final PhotonCamera cam;
 
-    NTTopicSet ts = new NTTopicSet();
+    protected NTTopicSet ts = new NTTopicSet();
     private long heartbeatCounter = 1;
 
     /** This simulated camera's {@link SimCameraProperties} */

photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java

@@ -24,17 +24,30 @@
 
 package org.photonvision;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.photonvision.UnitTestUtils.waitForCondition;
+import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
+
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.NetworkTablesJNI;
 import edu.wpi.first.wpilibj.Timer;
 import java.io.IOException;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncClient;
 import org.photonvision.jni.WpilibLoader;
+import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 class PhotonCameraTest {
@@ -43,6 +56,16 @@ public static void load_wpilib() {
         WpilibLoader.loadLibraries();
     }
 
+    @BeforeEach
+    public void setup() {
+        HAL.initialize(500, 0);
+    }
+
+    @AfterEach
+    public void teardown() {
+        HAL.shutdown();
+    }
+
     @Test
     public void testEmpty() {
         Assertions.assertDoesNotThrow(
@@ -92,4 +115,119 @@ public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IO
 
         HAL.shutdown();
     }
+
+    private static Stream<Arguments> testNtOffsets() {
+        return Stream.of(
+                // various initializaiton orders
+                Arguments.of(1, 10, 30, 30),
+                Arguments.of(10, 2, 30, 30),
+                Arguments.of(10, 10, 30, 30),
+                // Reboot just the robot
+                Arguments.of(1, 1, 10, 30),
+                // Reboot just the coproc
+                Arguments.of(1, 1, 30, 10));
+    }
+
+    /**
+     * Try starting client before server and vice-versa, making sure that we never fail the version
+     * check
+     */
+    @ParameterizedTest
+    @MethodSource("testNtOffsets")
+    public void testRestartingRobotAndCoproc(
+            int robotStart, int coprocStart, int robotRestart, int coprocRestart) throws Throwable {
+        var robotNt = NetworkTableInstance.create();
+        var coprocNt = NetworkTableInstance.create();
+
+        robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
+        coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
+
+        TimeSyncClient tspClient = null;
+
+        var robotCamera = new PhotonCamera(robotNt, "MY_CAMERA");
+
+        // apparently need a PhotonCamera to hand down
+        var fakePhotonCoprocCam = new PhotonCamera(coprocNt, "MY_CAMERA");
+        var coprocSim = new PhotonCameraSim(fakePhotonCoprocCam);
+        coprocSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90));
+        coprocSim.prop.setFPS(30);
+        coprocSim.setMinTargetAreaPixels(20.0);
+
+        for (int i = 0; i < 20; i++) {
+            int seq = i + 1;
+
+            if (i == coprocRestart) {
+                System.out.println("Restarting coprocessor NT client");
+
+                fakePhotonCoprocCam.close();
+                coprocNt.close();
+                coprocNt = NetworkTableInstance.create();
+
+                coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
+
+                fakePhotonCoprocCam = new PhotonCamera(coprocNt, "MY_CAMERA");
+                coprocSim = new PhotonCameraSim(fakePhotonCoprocCam);
+                coprocSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90));
+                coprocSim.prop.setFPS(30);
+                coprocSim.setMinTargetAreaPixels(20.0);
+            }
+            if (i == robotRestart) {
+                System.out.println("Restarting robot NT server");
+
+                robotNt.close();
+                robotNt = NetworkTableInstance.create();
+                robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
+                robotCamera = new PhotonCamera(robotNt, "MY_CAMERA");
+            }
+
+            if (i == coprocStart || i == coprocRestart) {
+                coprocNt.setServer("127.0.0.1", 5940);
+                coprocNt.startClient4("testClient");
+
+                // PhotonCamera makes a server by default - connect to it
+                tspClient = new TimeSyncClient("127.0.0.1", 5810, 0.5);
+            }
+
+            if (i == robotStart || i == robotRestart) {
+                robotNt.startServer("networktables_random.json", "", 5941, 5940);
+            }
+
+            Thread.sleep(100);
+
+            if (i == Math.max(coprocStart, robotStart)) {
+                final var c = coprocNt;
+                final var r = robotNt;
+                waitForCondition("Coproc connection", () -> c.getConnections().length == 1);
+                waitForCondition("Rio connection", () -> r.getConnections().length == 1);
+            }
+
+            var result1 = new PhotonPipelineResult();
+            result1.metadata.captureTimestampMicros = seq * 100;
+            result1.metadata.publishTimestampMicros = seq * 150;
+            result1.metadata.sequenceID = seq;
+            if (tspClient != null) {
+                result1.metadata.timeSinceLastPong = tspClient.getPingMetadata().timeSinceLastPong();
+            } else {
+                result1.metadata.timeSinceLastPong = Long.MAX_VALUE;
+            }
+
+            coprocSim.submitProcessedFrame(result1, NetworkTablesJNI.now());
+            coprocNt.flush();
+
+            if (i > robotStart && i > coprocStart) {
+                var ret = waitForSequenceNumber(robotCamera, seq);
+                System.out.println(ret);
+            }
+
+            // force verifyVersion to do checks
+            robotCamera.lastVersionCheckTime = -100;
+            robotCamera.prevTimeSyncWarnTime = -100;
+            assertDoesNotThrow(robotCamera::verifyVersion);
+        }
+
+        coprocSim.close();
+        coprocNt.close();
+        robotNt.close();
+        tspClient.stop();
+    }
 }

photon-lib/src/test/java/org/photonvision/UnitTestUtils.java

@@ -0,0 +1,74 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import java.util.function.BooleanSupplier;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+public class UnitTestUtils {
+    static void waitForCondition(String name, BooleanSupplier condition) {
+        // wait up to 1 second for satisfaction
+        for (int i = 0; i < 100; i++) {
+            if (condition.getAsBoolean()) {
+                System.out.println(name + " satisfied on iteration " + i);
+                return;
+            }
+
+            try {
+                Thread.sleep(60);
+            } catch (InterruptedException e) {
+                e.printStackTrace();
+                fail(e);
+            }
+        }
+        throw new RuntimeException(name + " was never satisfied");
+    }
+
+    static PhotonPipelineResult waitForSequenceNumber(PhotonCamera camera, int seq) {
+        assertTrue(camera.heartbeatEntry.getTopic().getHandle() != 0);
+
+        System.out.println(
+                "Waiting for seq=" + seq + " on " + camera.heartbeatEntry.getTopic().getName());
+        // wait up to 1 second for a new result
+        for (int i = 0; i < 100; i++) {
+            var res = camera.getLatestResult();
+            System.out.println(res);
+            if (res.metadata.sequenceID == seq) {
+                return res;
+            }
+
+            try {
+                Thread.sleep(60);
+            } catch (InterruptedException e) {
+                e.printStackTrace();
+                fail(e);
+            }
+        }
+        throw new RuntimeException("Never saw sequence number " + seq);
+    }
+}