From 486873215bda4fbf89dca31cba90ed98c16c42a6 Mon Sep 17 00:00:00 2001 From: Matt M Date: Thu, 31 Oct 2024 15:24:59 -0700 Subject: [PATCH 1/7] Assert that version checking won't throw on startup --- .../java/org/photonvision/PhotonCamera.java | 8 +- .../org/photonvision/VisionSystemSimTest.java | 102 ++++++++++++++++-- .../networktables/PacketSubscriber.java | 7 ++ .../org/photonvision/jni/TimeSyncClient.java | 4 +- .../targeting/PhotonPipelineMetadata.java | 16 +-- 5 files changed, 118 insertions(+), 19 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index c70cd505fa..726f963063 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -111,13 +111,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 +396,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; @@ -448,7 +448,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 diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 401ac19bdc..9ddf427891 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -24,6 +24,7 @@ package org.photonvision; +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -41,6 +42,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTablesJNI; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.IOException; import java.util.ArrayList; @@ -59,6 +61,7 @@ import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.jni.PhotonTargetingJniLoader; +import org.photonvision.jni.TimeSyncClient; import org.photonvision.jni.WpilibLoader; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; @@ -126,6 +129,7 @@ private PhotonPipelineResult waitForSequenceNumber(PhotonCamera camera, int seq) Thread.sleep(10); } catch (InterruptedException e) { e.printStackTrace(); + fail(e); } } throw new RuntimeException("Never saw sequence number " + seq); @@ -247,7 +251,8 @@ public void testNotVisibleVert2() { assertTrue(waitForSequenceNumber(camera, 1).hasTargets()); - // Pitched back camera should mean target goes out of view below the robot as distance increases + // Pitched back camera should mean target goes out of view below the robot as + // distance increases robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); @@ -350,7 +355,8 @@ public void testPitchAngles(double testPitch) throws InterruptedException { assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); - // Since the camera is level with the target, a positive-upward point will mean the target is in + // Since the camera is level with the target, a positive-upward point will mean + // the target is in // the // lower half of the image // which should produce negative pitch. @@ -358,7 +364,8 @@ public void testPitchAngles(double testPitch) throws InterruptedException { } private static Stream testDistanceCalcArgs() { - // Arbitrary and fairly random assortment of distances, camera pitches, and heights + // Arbitrary and fairly random assortment of distances, camera pitches, and + // heights return Stream.of( Arguments.of(5, -15.98, 0), Arguments.of(6, -15.98, 1), @@ -382,7 +389,8 @@ private static Stream testDistanceCalcArgs() { @ParameterizedTest @MethodSource("testDistanceCalcArgs") public void testDistanceCalc(double testDist, double testPitch, double testHeight) { - // Assume dist along ground and tgt height the same. Iterate over other parameters. + // Assume dist along ground and tgt height the same. Iterate over other + // parameters. final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI * 0.98)); @@ -406,10 +414,13 @@ public void testDistanceCalc(double testDist, double testPitch, double testHeigh visionSysSim.update(robotPose); - // Note that target 2d yaw/pitch accuracy is hindered by two factors in photonvision: - // 1. These are calculated with the average of the minimum area rectangle, which does not + // Note that target 2d yaw/pitch accuracy is hindered by two factors in + // photonvision: + // 1. These are calculated with the average of the minimum area rectangle, which + // does not // actually find the target center because of perspective distortion. - // 2. Yaw and pitch are calculated separately which gives incorrect pitch values. + // 2. Yaw and pitch are calculated separately which gives incorrect pitch + // values. var res = waitForSequenceNumber(camera, 1); assertTrue(res.hasTargets()); @@ -579,4 +590,81 @@ public void testPoseEstimation() { assertEquals(0, pose.getZ(), .01); assertEquals(Math.toRadians(5), pose.getRotation().getZ(), 0.01); } + + private static Stream testNtOffsets() { + return Stream.of(Arguments.of(1, 10), Arguments.of(10, 2), Arguments.of(10, 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) throws Throwable { + + var robotNt = NetworkTableInstance.create(); + var coprocNt = NetworkTableInstance.create(); + + 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); + + int seq = 1; + for (int i = 0; i < 30; i++) { + if (i == coprocStart) { + 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) { + robotNt.startServer("networktables_random.json", "", 5941, 5940); + robotNt.startClient4("testClient"); + } + + if (i > coprocStart + 1 && i > robotStart + 1) { + System.out.println("coproc connections: " + coprocNt.getConnections().length); + System.out.println("robot connections: " + robotNt.getConnections().length); + assertEquals(1, coprocNt.getConnections().length); + assertEquals(1, robotNt.getConnections().length); + } + + var result1 = new PhotonPipelineResult(); + result1.metadata.captureTimestampMicros = NetworkTablesJNI.now(); + 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()); + + if (i > robotStart && i > coprocStart) { + var ret = waitForSequenceNumber(robotCamera, seq); + System.out.println(ret); + } + + robotCamera.lastVersionCheckTime = -100; + robotCamera.prevTimeSyncWarnTime = -100; + assertDoesNotThrow(robotCamera::verifyVersion); + + seq += 1; + Thread.sleep(100); + } + + coprocSim.close(); + coprocNt.close(); + robotNt.close(); + tspClient.stop(); + } } diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java index 9a045d5b72..6f43aca03d 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java @@ -90,6 +90,13 @@ public void close() { public String getInterfaceUUID() { // ntcore hands us a JSON string with leading/trailing quotes - remove those var uuidStr = subscriber.getTopic().getProperty("message_uuid"); + + // "null" can be returned if the property does not exist. From system knowledge, uuid can never + // be the string literal "null". + if (uuidStr.equals("null")) { + return ""; + } + return uuidStr.replace("\"", ""); } diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java index eee1047f76..d98834a96c 100644 --- a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java +++ b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java @@ -31,9 +31,9 @@ public static class PingMetadata { public long pingsSent; // incoming count public long pongsReceived; - // when we last heard back from the server + // when we last heard back from the server, uS, in local time base public long lastPongTime; - // RTT2, time from ping send to pong recieve at the client + // RTT2, time from ping send to pong receive at the client, uS public long rtt2; public PingMetadata( diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java index 4633a31fec..c856ba3e45 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java @@ -30,7 +30,7 @@ public class PhotonPipelineMetadata implements PhotonStructSerializable>> 32)); result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32)); result = prime * result + (int) (publishTimestampMicros ^ (publishTimestampMicros >>> 32)); + result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32)); + result = prime * result + (int) (timeSinceLastPong ^ (timeSinceLastPong >>> 32)); return result; } @@ -98,9 +101,10 @@ public boolean equals(Object obj) { if (obj == null) return false; if (getClass() != obj.getClass()) return false; PhotonPipelineMetadata other = (PhotonPipelineMetadata) obj; - if (sequenceID != other.sequenceID) return false; if (captureTimestampMicros != other.captureTimestampMicros) return false; if (publishTimestampMicros != other.publishTimestampMicros) return false; + if (sequenceID != other.sequenceID) return false; + if (timeSinceLastPong != other.timeSinceLastPong) return false; return true; } From f17ca19564df5ba892ee808cb2cebf5857904c6d Mon Sep 17 00:00:00 2001 From: Matt M Date: Thu, 31 Oct 2024 17:34:10 -0700 Subject: [PATCH 2/7] asdf --- .../src/test/java/org/photonvision/VisionSystemSimTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 9ddf427891..dae339b3ef 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -618,7 +618,7 @@ public void testRestartingRobotAndCoproc(int robotStart, int coprocStart) throws coprocSim.setMinTargetAreaPixels(20.0); int seq = 1; - for (int i = 0; i < 30; i++) { + for (int i = 0; i < 20; i++) { if (i == coprocStart) { coprocNt.setServer("127.0.0.1", 5940); coprocNt.startClient4("testClient"); From 94c19d366ef3a730917772616bd6578b4ebcab77 Mon Sep 17 00:00:00 2001 From: Matt M Date: Thu, 31 Oct 2024 17:41:01 -0700 Subject: [PATCH 3/7] run lint --- .../src/test/java/org/photonvision/VisionSystemSimTest.java | 1 - 1 file changed, 1 deletion(-) diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index dae339b3ef..61bde3886f 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -602,7 +602,6 @@ private static Stream testNtOffsets() { @ParameterizedTest @MethodSource("testNtOffsets") public void testRestartingRobotAndCoproc(int robotStart, int coprocStart) throws Throwable { - var robotNt = NetworkTableInstance.create(); var coprocNt = NetworkTableInstance.create(); From 1c874468bf5201ef9fd4554bc33d4347c5447dc6 Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 31 Oct 2024 20:03:39 -0700 Subject: [PATCH 4/7] Add more robustness tests --- .../java/org/photonvision/PhotonCamera.java | 2 +- .../simulation/PhotonCameraSim.java | 2 +- .../org/photonvision/PhotonCameraTest.java | 140 ++++++++++++++++++ .../java/org/photonvision/UnitTestUtils.java | 50 +++++++ .../org/photonvision/VisionSystemSimTest.java | 103 +------------ .../common/networktables/NTTopicSet.java | 3 +- 6 files changed, 195 insertions(+), 105 deletions(-) create mode 100644 photon-lib/src/test/java/org/photonvision/UnitTestUtils.java diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 726f963063..4f983c66e4 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -433,7 +433,7 @@ 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(""); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 76931a581d..e71d489f15 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/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} */ diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 95cc2736fe..b28d49e4e6 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/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,121 @@ public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IO HAL.shutdown(); } + + private static Stream testNtOffsets() { + return Stream.of( + // various initializaiton orders + Arguments.of(1, 10, 20, 20), Arguments.of(10, 2, 20, 20), Arguments.of(10, 10, 200, 20), + // Reboot just the robot + Arguments.of(1, 1, 10, 20), + // Reboot just the coproc + Arguments.of(1, 1, 20, 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(0, 255, System.out::println); + 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); + robotNt.startClient4("testClient"); + } + + 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()); + + 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); + + Thread.sleep(100); + } + + coprocSim.close(); + coprocNt.close(); + robotNt.close(); + tspClient.stop(); + } } diff --git a/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java b/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java new file mode 100644 index 0000000000..19fb4271c1 --- /dev/null +++ b/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java @@ -0,0 +1,50 @@ +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); + } +} diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 61bde3886f..991e6e73cf 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -24,11 +24,11 @@ package org.photonvision; -import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import static org.junit.jupiter.api.Assertions.fail; +import static org.photonvision.UnitTestUtils.waitForSequenceNumber; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; @@ -42,7 +42,6 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.NetworkTablesJNI; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.IOException; import java.util.ArrayList; @@ -61,12 +60,10 @@ import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; import org.photonvision.jni.PhotonTargetingJniLoader; -import org.photonvision.jni.TimeSyncClient; import org.photonvision.jni.WpilibLoader; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.simulation.VisionTargetSim; -import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; class VisionSystemSimTest { @@ -113,28 +110,6 @@ public void teardown() { HAL.shutdown(); } - private 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(); - if (res.metadata.sequenceID == seq) { - return res; - } - - try { - Thread.sleep(10); - } catch (InterruptedException e) { - e.printStackTrace(); - fail(e); - } - } - throw new RuntimeException("Never saw sequence number " + seq); - } - @Test public void testEmpty() { Assertions.assertDoesNotThrow( @@ -590,80 +565,4 @@ public void testPoseEstimation() { assertEquals(0, pose.getZ(), .01); assertEquals(Math.toRadians(5), pose.getRotation().getZ(), 0.01); } - - private static Stream testNtOffsets() { - return Stream.of(Arguments.of(1, 10), Arguments.of(10, 2), Arguments.of(10, 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) throws Throwable { - var robotNt = NetworkTableInstance.create(); - var coprocNt = NetworkTableInstance.create(); - - 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); - - int seq = 1; - for (int i = 0; i < 20; i++) { - if (i == coprocStart) { - 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) { - robotNt.startServer("networktables_random.json", "", 5941, 5940); - robotNt.startClient4("testClient"); - } - - if (i > coprocStart + 1 && i > robotStart + 1) { - System.out.println("coproc connections: " + coprocNt.getConnections().length); - System.out.println("robot connections: " + robotNt.getConnections().length); - assertEquals(1, coprocNt.getConnections().length); - assertEquals(1, robotNt.getConnections().length); - } - - var result1 = new PhotonPipelineResult(); - result1.metadata.captureTimestampMicros = NetworkTablesJNI.now(); - 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()); - - if (i > robotStart && i > coprocStart) { - var ret = waitForSequenceNumber(robotCamera, seq); - System.out.println(ret); - } - - robotCamera.lastVersionCheckTime = -100; - robotCamera.prevTimeSyncWarnTime = -100; - assertDoesNotThrow(robotCamera::verifyVersion); - - seq += 1; - Thread.sleep(100); - } - - coprocSim.close(); - coprocNt.close(); - robotNt.close(); - tspClient.stop(); - } } diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 1b1fd2698f..62935fc1f1 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -80,7 +80,8 @@ public void updateEntries() { .publish( PhotonPipelineResult.photonStruct.getTypeString(), PubSubOption.periodic(0.01), - PubSubOption.sendAll(true)); + PubSubOption.sendAll(true), + PubSubOption.keepDuplicates(true)); resultPublisher = new PacketPublisher(rawBytesEntry, PhotonPipelineResult.photonStruct); From 4f73d1a9218959b028943e1d89f8baa0795b2f92 Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 31 Oct 2024 20:14:02 -0700 Subject: [PATCH 5/7] run lint, fix tests --- .../java/org/photonvision/PhotonCamera.java | 16 ------------- .../org/photonvision/PhotonCameraTest.java | 13 +++++----- .../java/org/photonvision/UnitTestUtils.java | 24 +++++++++++++++++++ 3 files changed, 30 insertions(+), 23 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 4f983c66e4..97b9271369 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/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 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(); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index b28d49e4e6..264a284dfd 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -119,12 +119,13 @@ public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IO private static Stream testNtOffsets() { return Stream.of( // various initializaiton orders - Arguments.of(1, 10, 20, 20), Arguments.of(10, 2, 20, 20), Arguments.of(10, 10, 200, 20), + 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, 20), + Arguments.of(1, 1, 10, 30), // Reboot just the coproc - Arguments.of(1, 1, 20, 10) - ); + Arguments.of(1, 1, 30, 10)); } /** @@ -162,7 +163,6 @@ public void testRestartingRobotAndCoproc( coprocNt.close(); coprocNt = NetworkTableInstance.create(); - coprocNt.addLogger(0, 255, System.out::println); coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message)); fakePhotonCoprocCam = new PhotonCamera(coprocNt, "MY_CAMERA"); @@ -213,6 +213,7 @@ public void testRestartingRobotAndCoproc( } coprocSim.submitProcessedFrame(result1, NetworkTablesJNI.now()); + coprocNt.flush(); if (i > robotStart && i > coprocStart) { var ret = waitForSequenceNumber(robotCamera, seq); @@ -223,8 +224,6 @@ public void testRestartingRobotAndCoproc( robotCamera.lastVersionCheckTime = -100; robotCamera.prevTimeSyncWarnTime = -100; assertDoesNotThrow(robotCamera::verifyVersion); - - Thread.sleep(100); } coprocSim.close(); diff --git a/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java b/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java index 19fb4271c1..99af2e0f23 100644 --- a/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java +++ b/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java @@ -1,3 +1,27 @@ +/* + * 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; From 47b8426df467bdc1457daf016c20b72bc0de6814 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 1 Nov 2024 20:34:50 -0700 Subject: [PATCH 6/7] paranoia stuff --- .../org/photonvision/PhotonCameraTest.java | 1 - .../org/photonvision/jni/TimeSyncClient.java | 20 +++++++++++++------ .../org/photonvision/jni/TimeSyncServer.java | 9 ++++++++- .../src/main/native/jni/TimeSyncClientJNI.cpp | 15 ++++++++++++++ .../src/main/native/jni/TimeSyncServerJNI.cpp | 13 ++++++++++++ 5 files changed, 50 insertions(+), 8 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 264a284dfd..92011fc5fc 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -190,7 +190,6 @@ public void testRestartingRobotAndCoproc( if (i == robotStart || i == robotRestart) { robotNt.startServer("networktables_random.json", "", 5941, 5940); - robotNt.startClient4("testClient"); } Thread.sleep(100); diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java index d98834a96c..02a05f3701 100644 --- a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java +++ b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java @@ -87,10 +87,8 @@ public TimeSyncClient(String server, int port, double interval) { this.port = port; this.interval = interval; - synchronized (mutex) { - this.handle = TimeSyncClient.create(server, port, interval); - TimeSyncClient.start(handle); - } + this.handle = TimeSyncClient.create(server, port, interval); + TimeSyncClient.start(handle); } public void setServer(String newServer) { @@ -121,7 +119,12 @@ public void stop() { */ public long getOffset() { synchronized (mutex) { - return TimeSyncClient.getOffset(handle); + if (handle > 0) { + return TimeSyncClient.getOffset(handle); + } + + System.err.println("TimeSyncClient: use after free?"); + return 0; } } @@ -136,7 +139,12 @@ public long currentServerTimestamp() { public PingMetadata getPingMetadata() { synchronized (mutex) { - return TimeSyncClient.getLatestMetadata(handle); + if (handle > 0) { + return TimeSyncClient.getLatestMetadata(handle); + } + + System.err.println("TimeSyncClient: use after free?"); + return new PingMetadata(0, 0, 0, 0, 0); } } diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java index 455702531a..b660a439c4 100644 --- a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java +++ b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java @@ -18,6 +18,7 @@ package org.photonvision.jni; public class TimeSyncServer { + private final Object mutex = new Object(); private long handle; public TimeSyncServer(int port) { @@ -25,7 +26,13 @@ public TimeSyncServer(int port) { } public void start() { - TimeSyncServer.start(handle); + synchronized (mutex) { + if (handle > 0) { + TimeSyncServer.start(handle); + } else { + System.err.println("TimeSyncServer: use after free?"); + } + } } public void stop() { diff --git a/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp b/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp index d84572c659..8b982865a5 100644 --- a/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp +++ b/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp @@ -24,6 +24,17 @@ using namespace wpi::tsp; +#define CHECK_PTR(ptr) \ + if (!ptr) { \ + fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ + return; \ + } +#define CHECK_PTR_RETURN(ptr, default) \ + if (!ptr) { \ + fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ + return default; \ + } + /** * Finds a class and keeps it as a global reference. * @@ -117,6 +128,7 @@ JNIEXPORT void JNICALL Java_org_photonvision_jni_TimeSyncClient_start (JNIEnv*, jclass, jlong ptr) { + CHECK_PTR(ptr); TimeSyncClient* client = reinterpret_cast(ptr); client->Start(); } @@ -130,6 +142,7 @@ JNIEXPORT void JNICALL Java_org_photonvision_jni_TimeSyncClient_stop (JNIEnv*, jclass, jlong ptr) { + CHECK_PTR(ptr); TimeSyncClient* client = reinterpret_cast(ptr); client->Stop(); delete client; @@ -144,6 +157,7 @@ JNIEXPORT jlong JNICALL Java_org_photonvision_jni_TimeSyncClient_getOffset (JNIEnv*, jclass, jlong ptr) { + CHECK_PTR_RETURN(ptr, 0); TimeSyncClient* client = reinterpret_cast(ptr); return client->GetOffset(); } @@ -157,6 +171,7 @@ JNIEXPORT jobject JNICALL Java_org_photonvision_jni_TimeSyncClient_getLatestMetadata (JNIEnv* env, jclass, jlong ptr) { + CHECK_PTR_RETURN(ptr, nullptr); TimeSyncClient* client = reinterpret_cast(ptr); auto m{client->GetMetadata()}; auto ret = env->NewObject(metadataClass, metadataCtor, m.offset, m.pingsSent, diff --git a/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp b/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp index 3c7fc2dac8..2991ac5676 100644 --- a/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp +++ b/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp @@ -24,6 +24,17 @@ using namespace wpi::tsp; +#define CHECK_PTR(ptr) \ + if (!ptr) { \ + fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ + return; \ + } +#define CHECK_PTR_RETURN(ptr, default) \ + if (!ptr) { \ + fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \ + return default; \ + } + extern "C" { /* @@ -47,6 +58,7 @@ JNIEXPORT void JNICALL Java_org_photonvision_jni_TimeSyncServer_start (JNIEnv*, jclass, jlong ptr) { + CHECK_PTR(ptr); TimeSyncServer* server = reinterpret_cast(ptr); server->Start(); } @@ -60,6 +72,7 @@ JNIEXPORT void JNICALL Java_org_photonvision_jni_TimeSyncServer_stop (JNIEnv*, jclass, jlong ptr) { + CHECK_PTR(ptr); TimeSyncServer* server = reinterpret_cast(ptr); server->Stop(); delete server; From 74f795a8f28f0ffb2f704d9d4151947f9843c404 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 1 Nov 2024 20:38:02 -0700 Subject: [PATCH 7/7] gasdlfkja --- .../photonvision/targeting/proto/PhotonPipelineResultProto.java | 1 + 1 file changed, 1 insertion(+) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 25b29d144a..97bf80e6cc 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -69,5 +69,6 @@ public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { msg.setSequenceId(value.metadata.getSequenceID()); msg.setCaptureTimestampMicros(value.metadata.getCaptureTimestampMicros()); msg.setNtPublishTimestampMicros(value.metadata.getPublishTimestampMicros()); + msg.setTimeSinceLastPongMicros(value.metadata.timeSinceLastPong); } }