Hack in time sync

.github/workflows/lint-format.yml

@@ -37,7 +37,7 @@ jobs:
         with:
           python-version: 3.11
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.37
+        run: pip3 install wpiformat==2024.41
       - name: Run
         run: wpiformat
       - name: Check output

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

@@ -19,7 +19,7 @@
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
@@ -129,16 +129,21 @@ public void updateCameraNickname(String newCameraNickname) {
 
     @Override
     public void accept(CVPipelineResult result) {
-        var now = WPIUtilJNI.now();
+        var now = NetworkTablesJNI.now();
+
+        var offset = NetworkTablesManager.getInstance().m_timeSyncClient.getOffset();
+
         var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
         var simplified =
                 new PhotonPipelineResult(
                         result.sequenceID,
-                        captureMicros,
-                        now,
+                        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);
         if (ConfigManager.getInstance().getConfig().getNetworkConfig().shouldPublishProto) {

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

@@ -39,6 +39,7 @@
 import org.photonvision.common.scripting.ScriptManager;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.file.JacksonUtils;
+import org.photonvision.jni.TimeSyncClient;
 
 public class NetworkTablesManager {
     private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
@@ -53,7 +54,12 @@ public class NetworkTablesManager {
     private StringSubscriber m_fieldLayoutSubscriber =
             kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
 
+    public TimeSyncClient m_timeSyncClient;
+
     private NetworkTablesManager() {
+        m_timeSyncClient = new TimeSyncClient("127.0.0.1", 5812, 1.0);
+        m_timeSyncClient.start();
+
         ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
         ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
 

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/build.gradle

@@ -1,3 +1,7 @@
+plugins {
+    id 'edu.wpi.first.WpilibTools' version '1.3.0'
+}
+
 import java.nio.file.Path
 
 ext {
@@ -314,3 +318,24 @@ publishing {
         }
     }
 }
+
+// setup wpilib bundled native libs
+wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
+
+def nativeConfigName = 'wpilibNatives'
+def nativeConfig = configurations.create(nativeConfigName)
+
+def nativeTasks = wpilibTools.createExtractionTasks {
+    configurationName = nativeConfigName
+}
+
+nativeTasks.addToSourceSetResources(sourceSets.test)
+
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())

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

@@ -24,12 +24,56 @@
 
 package org.photonvision;
 
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.apriltag.jni.AprilTagJNI;
+import edu.wpi.first.cscore.CameraServerCvJNI;
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.JNIWrapper;
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.net.WPINetJNI;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.util.CombinedRuntimeLoader;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.Timer;
+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.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncServer;
 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 +84,30 @@ 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(5812);
+        server.start();
+
+        var camera = new PhotonCamera("Microsoft_LifeCam_HD-3000");
+        PhotonCamera.setVersionCheckEnabled(false);
+
+        for (int i = 0; i < 100; i++) {
+            Thread.sleep(100);
+
+            var captureTime = camera.getLatestResult().getTimestampSeconds();
+            var now = Timer.getFPGATimestamp();
+
+            assertTrue(captureTime < now);
+        }
+    }
 }

photon-server/src/main/java/org/photonvision/Main.java

@@ -39,6 +39,7 @@
 import org.photonvision.common.networking.NetworkManager;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.jni.PhotonTargetingJniLoader;
 import org.photonvision.jni.RknnDetectorJNI;
 import org.photonvision.mrcal.MrCalJNILoader;
 import org.photonvision.raspi.LibCameraJNILoader;
@@ -379,7 +380,20 @@ public static void main(String[] args) {
             logger.error("Failed to load native libraries!", e);
             System.exit(1);
         }
-        logger.info("Native libraries loaded.");
+        logger.info("WPI JNI libraries loaded.");
+
+        try {
+            boolean success = PhotonTargetingJniLoader.load();
+
+            if (!success) {
+                logger.error("Failed to load PhotonTargetingJni");
+                System.exit(1);
+            }
+        } catch (Exception e) {
+            logger.error("Failed to load native libraries!", e);
+            System.exit(1);
+        }
+        logger.info("PhotonTargeting JNI loaded");
 
         try {
             if (Platform.isRaspberryPi()) {

photon-targeting/src/main/java/org/photonvision/jni/TimeSyncJNILoader.java → photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java

@@ -20,27 +20,30 @@
 import java.io.File;
 import java.io.FileOutputStream;
 import java.io.IOException;
+import java.nio.file.Files;
 import java.util.List;
 import org.photonvision.common.hardware.Platform;
 
-public class TimeSyncJNILoader {
-    public static void load() throws IOException {
+public class PhotonTargetingJniLoader {
+    public static boolean load() throws IOException {
         // We always extract the shared object (we could hash each so, but that's a lot
         // of work)
         String arch_name = Platform.getNativeLibraryFolderName();
-        var clazz = TimeSyncJNILoader.class;
+        var clazz = PhotonTargetingJniLoader.class;
 
         for (var libraryName : List.of("photontargeting", "photontargetingJNI")) {
             var nativeLibName = System.mapLibraryName(libraryName);
             var in = clazz.getResourceAsStream("/nativelibraries/" + arch_name + "/" + nativeLibName);
 
             if (in == null) {
-                return;
+                return false;
             }
 
             // It's important that we don't mangle the names of these files on Windows at
             // least
-            File temp = new File(System.getProperty("java.io.tmpdir"), nativeLibName);
+            var tempfolder = Files.createTempDirectory("nativeextract");
+            File temp = new File(tempfolder.toAbsolutePath().toString(), nativeLibName);
+            System.out.println(temp.getAbsolutePath().toString());
             FileOutputStream fos = new FileOutputStream(temp);
 
             int read = -1;
@@ -55,5 +58,7 @@ public static void load() throws IOException {
 
             System.out.println("Successfully loaded shared object " + temp.getName());
         }
+
+        return true;
     }
 }

photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java

@@ -17,6 +17,9 @@
 
 package org.photonvision.jni;
 
+import edu.wpi.first.networktables.NetworkTablesJNI;
+
+/** Send ping-pongs to estimate server time, relative to nt::now */
 public class TimeSyncClient {
     public static class PingMetadata {
         long offset;
@@ -39,10 +42,25 @@ public void stop() {
         TimeSyncClient.stop(handle);
     }
 
+    /**
+     * This offset, when added to the current value of nt::now(), yields the timestamp in the timebase
+     * of the TSP Server
+     *
+     * @return
+     */
     public long getOffset() {
         return TimeSyncClient.getOffset(handle);
     }
 
+    /**
+     * Best estimate of the current timestamp at the TSP server
+     *
+     * @return
+     */
+    public long currentServerTimestamp() {
+        return NetworkTablesJNI.now() + getOffset();
+    }
+
     private static native long create(String serverIP, int serverPort, double pingIntervalSeconds);
 
     private static native void start(long handle);

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java

@@ -169,12 +169,10 @@ public Optional<MultiTargetPNPResult> getMultiTagResult() {
      * @return The timestamp in seconds
      */
     public double getTimestampSeconds() {
-        return (ntReceiveTimestampMicros
-                        - (metadata.publishTimestampMicros - metadata.captureTimestampMicros))
-                / 1e6;
+        return metadata.captureTimestampMicros / 1e6;
     }
 
-    /** The time that the robot Received this result, in the FPGA timebase. */
+    /** The time that the robot received this result, in the FPGA timebase. */
     public long getNtReceiveTimestampMicros() {
         return ntReceiveTimestampMicros;
     }