Copy timesync client/server + JNI it

photon-core/build.gradle

@@ -17,6 +17,7 @@ def nativeTasks = wpilibTools.createExtractionTasks {
 
 nativeTasks.addToSourceSetResources(sourceSets.main)
 
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -22,6 +22,7 @@
 import edu.wpi.first.networktables.NetworkTableEvent;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.List;
+import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
@@ -146,15 +147,20 @@ public void accept(CVPipelineResult result) {
                             List.of(),
                             result.inputAndOutputFrame);
         else acceptedResult = result;
-        var now = WPIUtilJNI.now();
-        var captureMicros = MathUtils.nanosToMicros(acceptedResult.getImageCaptureTimestampNanos());
+
+        var now = NetworkTablesJNI.now();
+        var offset = NetworkTablesManager.getInstance().getOffset();
+
+        var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
         var simplified =
                 new PhotonPipelineResult(
-                        acceptedResult.sequenceID,
-                        captureMicros,
-                        now,
-                        TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
-                        acceptedResult.multiTagResult);
+                        result.sequenceID,
+                        captureMicros + offset,
+                        now + offset,
+                        TrackedTarget.simpleFromTrackedTargets(result.targets),
+                        result.multiTagResult);
+
+        // System.out.println(simplified.metadata);
 
         // random guess at size of the array
         ts.resultPublisher.set(simplified, 1024);

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java

@@ -39,6 +39,8 @@
 import org.photonvision.common.scripting.ScriptManager;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.file.JacksonUtils;
+import org.photonvision.jni.TimeSyncClient;
+import org.photonvision.jni.TimeSyncServer;
 
 public class NetworkTablesManager {
     private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
@@ -53,6 +55,9 @@ public class NetworkTablesManager {
     private StringSubscriber m_fieldLayoutSubscriber =
             kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
 
+    private TimeSyncClient m_timeSyncClient;
+    private TimeSyncServer m_timeSyncServer;
+
     private NetworkTablesManager() {
         ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
         ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
@@ -163,14 +168,33 @@ private void broadcastVersion() {
     }
 
     public void setConfig(NetworkConfig config) {
+        if (m_timeSyncClient != null) m_timeSyncClient.stop();
+        if (m_timeSyncServer != null) m_timeSyncServer.stop();
+        m_timeSyncClient = null;
+        m_timeSyncServer = null;
+
         if (config.runNTServer) {
             setServerMode();
+            m_timeSyncServer = new TimeSyncServer(5810);
+            m_timeSyncServer.start();
         } else {
             setClientMode(config.ntServerAddress);
+            m_timeSyncClient = new TimeSyncClient(config.ntServerAddress, 5810, 1.0);
+            m_timeSyncClient.start();
         }
+
         broadcastVersion();
     }
 
+    public long getOffset() {
+        if (m_timeSyncClient != null) return m_timeSyncClient.getOffset();
+        if (m_timeSyncServer != null) return 0;
+
+        // ????? should never hit
+        logger.error("Client and server and null?");
+        return 0;
+    }
+
     private void setClientMode(String ntServerAddress) {
         ntInstance.stopServer();
         ntInstance.startClient4("photonvision");

photon-core/src/main/java/org/photonvision/common/util/TestUtils.java

@@ -36,6 +36,7 @@
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
 import org.opencv.highgui.HighGui;
+import org.photonvision.jni.PhotonTargetingJniLoader;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 
 public class TestUtils {
@@ -68,14 +69,17 @@ public static boolean loadLibraries() {
             CombinedRuntimeLoader.loadLibraries(
                     TestUtils.class,
                     "wpiutiljni",
-                    "wpimathjni",
+                    "wpilibc",
                     "ntcorejni",
                     "wpinetjni",
                     "wpiHaljni",
+                    "wpi",
                     Core.NATIVE_LIBRARY_NAME,
                     "cscorejni",
                     "apriltagjni");
 
+            PhotonTargetingJniLoader.load();
+
             has_loaded = true;
         } catch (IOException e) {
             e.printStackTrace();

photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -25,7 +25,7 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.Arrays;
 import java.util.List;
 import org.opencv.core.Core;
@@ -98,7 +98,7 @@ public static int map(int value, int inMin, int inMax, int outMin, int outMax) {
     }
 
     public static long wpiNanoTime() {
-        return microsToNanos(WPIUtilJNI.now());
+        return microsToNanos(NetworkTablesJNI.now());
     }
 
     /**

photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java

@@ -45,6 +45,7 @@ public CapturedFrame getInputMat() {
         long time =
                 cvSink.grabFrame(mat.getMat())
                         * 1000; // Units are microseconds, epoch is the same as the Unix epoch
+        // this -should- be the same timebase as nt::now?
 
         if (time == 0) {
             var error = cvSink.getError();

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

@@ -222,7 +222,7 @@ public PhotonPipelineResult getLatestResult() {
         // Set the timestamp of the result. Since PacketSubscriber doesn't realize that the result
         // contains a thing with time knowledge, set it here.
         // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
-        // TODO: NT4 time sync is Not To Be Trusted, we should do something else?
+        // This is in nt::Now timebase so should Just Work with the new timesync stuff
         result.setReceiveTimestampMicros(ret.timestamp);
 
         return result;

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

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

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

@@ -395,6 +395,14 @@ public void update(Pose3d robotPoseMeters) {
             // process a PhotonPipelineResult with visible targets
             var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets);
             // publish this info to NT at estimated timestamp of receive
+            System.out.println(
+                    "Camera "
+                            // + camSim.ts.bestTargetPosX.getTopic().getName()
+                            + camSim.getCamera().getName()
+                            + ": result at "
+                            + timestampNT
+                            + ":\n"
+                            + camResult);
             camSim.submitProcessedFrame(camResult, timestampNT);
             // display debug results
             for (var target : camResult.getTargets()) {

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

@@ -24,12 +24,49 @@
 
 package org.photonvision;
 
+import edu.wpi.first.apriltag.jni.AprilTagJNI;
+import edu.wpi.first.cscore.CameraServerCvJNI;
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.hal.JNIWrapper;
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.net.WPINetJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.util.CombinedRuntimeLoader;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.io.IOException;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.Test;
+import org.opencv.core.Core;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 class PhotonCameraTest {
+    public static void load_wpilib() {
+        NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
+        WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
+        WPIMathJNI.Helper.setExtractOnStaticLoad(false);
+        CameraServerJNI.Helper.setExtractOnStaticLoad(false);
+        CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
+        JNIWrapper.Helper.setExtractOnStaticLoad(false);
+        WPINetJNI.Helper.setExtractOnStaticLoad(false);
+        AprilTagJNI.Helper.setExtractOnStaticLoad(false);
+
+        try {
+            CombinedRuntimeLoader.loadLibraries(
+                    PhotonCameraTest.class,
+                    "wpiutiljni",
+                    "wpimathjni",
+                    "ntcorejni",
+                    "wpinetjni",
+                    "wpiHaljni",
+                    Core.NATIVE_LIBRARY_NAME,
+                    "cscorejni",
+                    "apriltagjni");
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+    }
+
     @Test
     public void testEmpty() {
         Assertions.assertDoesNotThrow(
@@ -40,4 +77,41 @@ public void testEmpty() {
                     PhotonPipelineResult.photonStruct.pack(packet, ret);
                 });
     }
+
+    // @Test
+    // public void testMeme() throws InterruptedException, IOException {
+    //     load_wpilib();
+    //     PhotonTargetingJniLoader.load();
+
+    //     // HAL.initialize(500, 0);
+
+    //     NetworkTableInstance.getDefault().stopClient();
+    //     NetworkTableInstance.getDefault().startServer();
+
+    //     var server = new TimeSyncServer(5810);
+    //     server.start();
+
+    //     var camera = new PhotonCamera("Arducam_OV9281_USB_Camera");
+    //     PhotonCamera.setVersionCheckEnabled(false);
+
+    //     for (int i = 0; i < 20; i++) {
+    //         Thread.sleep(100);
+
+    //         var res = camera.getLatestResult();
+    //         var captureTime = res.getTimestampSeconds();
+    //         var now = Timer.getFPGATimestamp();
+
+    //         // expectTrue(captureTime < now);
+
+    //         System.out.println(
+    //                 "sequence "
+    //                         + res.metadata.sequenceID
+    //                         + " image capture "
+    //                         + captureTime
+    //                         + " recieved at "
+    //                         + res.getNtReceiveTimestampMicros() / 1e6);
+    //     }
+
+    //     server.stop();
+    // }
 }