From 7a4ea3dd564cad8d4d0a4e03092ebc076cb425d8 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 1 Nov 2024 20:50:21 -0700 Subject: [PATCH] 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. --- .../java/org/photonvision/PhotonCamera.java | 26 +--- .../simulation/PhotonCameraSim.java | 2 +- .../org/photonvision/PhotonCameraTest.java | 138 ++++++++++++++++++ .../java/org/photonvision/UnitTestUtils.java | 74 ++++++++++ .../org/photonvision/VisionSystemSimTest.java | 44 ++---- .../common/networktables/NTTopicSet.java | 3 +- .../networktables/PacketSubscriber.java | 7 + .../org/photonvision/jni/TimeSyncClient.java | 24 ++- .../org/photonvision/jni/TimeSyncServer.java | 9 +- .../targeting/PhotonPipelineMetadata.java | 16 +- .../proto/PhotonPipelineResultProto.java | 1 + .../src/main/native/jni/TimeSyncClientJNI.cpp | 15 ++ .../src/main/native/jni/TimeSyncServerJNI.cpp | 13 ++ 13 files changed, 305 insertions(+), 67 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 c70cd505fa..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(); @@ -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 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..92011fc5fc 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,119 @@ public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IO HAL.shutdown(); } + + private static Stream 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(); + } } 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..99af2e0f23 --- /dev/null +++ b/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); + } +} diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 401ac19bdc..991e6e73cf 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -28,6 +28,7 @@ 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; @@ -63,7 +64,6 @@ 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 { @@ -110,27 +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(); - } - } - throw new RuntimeException("Never saw sequence number " + seq); - } - @Test public void testEmpty() { Assertions.assertDoesNotThrow( @@ -247,7 +226,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 +330,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 +339,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 +364,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 +389,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()); 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); 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..02a05f3701 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( @@ -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/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; } 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); } } 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;