Create timesync JNI for testing client

.github/workflows/build.yml

@@ -160,14 +160,14 @@ jobs:
       - run: git fetch --tags --force
       - run: |
           chmod +x gradlew
-          ./gradlew photon-targeting:build photon-lib:build -Pbuildalldesktop -i
-      - run: ./gradlew photon-lib:publish photon-targeting:publish -Pbuildalldesktop
+          ./gradlew photon-targeting:build photon-lib:build -i
+      - run: ./gradlew photon-lib:publish photon-targeting:publish
         name: Publish
         env:
           ARTIFACTORY_API_KEY: ${{ secrets.ARTIFACTORY_API_KEY }}
         if: github.event_name == 'push' && github.repository_owner == 'photonvision'
       # Copy artifacts to build/outputs/maven
-      - run: ./gradlew photon-lib:publish photon-targeting:publish -PcopyOfflineArtifacts -Pbuildalldesktop
+      - run: ./gradlew photon-lib:publish photon-targeting:publish -PcopyOfflineArtifacts
       - uses: actions/upload-artifact@v4
         with:
           name: maven-${{ matrix.artifact-name }}

.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

README.md

@@ -40,6 +40,8 @@ Note that these are case sensitive!
 - `-PtgtIp`: Specifies where `./gradlew deploy` should try to copy the fat JAR to
 - `-Pprofile`: enables JVM profiling
 
+If you're cross-compiling, you'll need the wpilib toolchain installed. This can be done via Gradle: for example `./gradlew installArm64Toolchain` or `./gradlew installRoboRioToolchain`
+
 ## Out-of-Source Dependencies
 
 PhotonVision uses the following additonal out-of-source repositories for building code.

build.gradle

@@ -1,12 +1,15 @@
 import edu.wpi.first.toolchain.*
 
 plugins {
+    id "java"
+    id "cpp"
     id "com.diffplug.spotless" version "6.24.0"
     id "edu.wpi.first.NativeUtils" version "2024.6.1" apply false
     id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
     id "edu.wpi.first.GradleRIO" version "2024.3.2"
     id 'edu.wpi.first.WpilibTools' version '1.3.0'
     id 'com.google.protobuf' version '0.9.4' apply false
+    id 'edu.wpi.first.GradleJni' version '1.1.0'
 }
 
 allprojects {

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

@@ -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().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,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
@@ -159,14 +164,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/hardware/PlatformUtils.java

@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.hardware;
+
+import java.io.IOException;
+import org.photonvision.common.util.ShellExec;
+
+@SuppressWarnings("unused")
+public class PlatformUtils {
+    private static final ShellExec shell = new ShellExec(true, false);
+    private static final boolean isRoot = checkForRoot();
+
+    @SuppressWarnings("StatementWithEmptyBody")
+    private static boolean checkForRoot() {
+        if (Platform.isLinux()) {
+            try {
+                shell.executeBashCommand("id -u");
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+
+            while (!shell.isOutputCompleted()) {
+                // TODO: add timeout
+            }
+
+            if (shell.getExitCode() == 0) {
+                return shell.getOutput().split("\n")[0].equals("0");
+            }
+
+        } else {
+            return true;
+        }
+        return false;
+    }
+
+    public static boolean isRoot() {
+        return isRoot;
+    }
+}

photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java

@@ -24,6 +24,7 @@
 import org.photonvision.common.dataflow.DataChangeSource;
 import org.photonvision.common.dataflow.events.DataChangeEvent;
 import org.photonvision.common.hardware.Platform;
+import org.photonvision.common.hardware.PlatformUtils;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ShellExec;
@@ -54,7 +55,7 @@ public void initialize(boolean shouldManage) {
         var config = ConfigManager.getInstance().getConfig().getNetworkConfig();
         logger.info("Setting " + config.connectionType + " with team " + config.ntServerAddress);
         if (Platform.isLinux()) {
-            if (!Platform.isRoot()) {
+            if (!PlatformUtils.isRoot()) {
                 logger.error("Cannot manage hostname without root!");
             }
 

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/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/main/java/org/photonvision/PhotonCamera.java

@@ -220,7 +220,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

@@ -68,7 +68,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()) {