Actually load wpilibc

PhotonVision · Sep 22, 2024 · 0bc13fe · 0bc13fe
1 parent 1c4cdb3
commit 0bc13fe
Show file tree

Hide file tree

Showing 11 changed files with 103 additions and 37 deletions.
diff --git a/README.md b/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.

diff --git a/photon-core/build.gradle b/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")

diff --git a/...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
@@ -131,7 +131,7 @@ public void updateCameraNickname(String newCameraNickname) {
     public void accept(CVPipelineResult result) {
         var now = NetworkTablesJNI.now();
 
-        var offset = NetworkTablesManager.getInstance().m_timeSyncClient.getOffset();
+        var offset = NetworkTablesManager.getInstance().getOffset();
 
         var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
         var simplified =

diff --git a/...re/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/...re/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
@@ -40,6 +40,7 @@
 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();
@@ -54,12 +55,10 @@ public class NetworkTablesManager {
     private StringSubscriber m_fieldLayoutSubscriber =
             kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
 
-    public TimeSyncClient m_timeSyncClient;
+    private TimeSyncClient m_timeSyncClient;
+    private TimeSyncServer m_timeSyncServer;
 
     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
 
@@ -165,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");

diff --git a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
@@ -69,10 +69,11 @@ public static boolean loadLibraries() {
             CombinedRuntimeLoader.loadLibraries(
                     TestUtils.class,
                     "wpiutiljni",
-                    "wpimathjni",
+                    "wpilibc",
                     "ntcorejni",
                     "wpinetjni",
                     "wpiHaljni",
+                    "wpi",
                     Core.NATIVE_LIBRARY_NAME,
                     "cscorejni",
                     "apriltagjni");

diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
@@ -27,17 +27,23 @@
 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 {
@@ -78,38 +84,38 @@ public void testEmpty() {
                 });
     }
 
-    // @Test
-    // public void testMeme() throws InterruptedException, IOException {
-    //     load_wpilib();
-    //     PhotonTargetingJniLoader.load();
+    @Test
+    public void testMeme() throws InterruptedException, IOException {
+        load_wpilib();
+        PhotonTargetingJniLoader.load();
 
-    //     HAL.initialize(500, 0);
+        HAL.initialize(500, 0);
 
-    //     NetworkTableInstance.getDefault().stopClient();
-    //     NetworkTableInstance.getDefault().startServer();
+        NetworkTableInstance.getDefault().stopClient();
+        NetworkTableInstance.getDefault().startServer();
 
-    //     var server = new TimeSyncServer(5812);
-    //     server.start();
+        var server = new TimeSyncServer(5810);
+        server.start();
 
-    //     var camera = new PhotonCamera("Microsoft_LifeCam_HD-3000");
-    //     PhotonCamera.setVersionCheckEnabled(false);
+        var camera = new PhotonCamera("Arducam_OV9281_USB_Camera");
+        PhotonCamera.setVersionCheckEnabled(false);
 
-    //     for (int i = 0; i < 200; i++) {
-    //         Thread.sleep(100);
+        for (int i = 0; i < 20000; i++) {
+            Thread.sleep(100);
 
-    //         var res = camera.getLatestResult();
-    //         var captureTime = res.getTimestampSeconds();
-    //         var now = Timer.getFPGATimestamp();
+            var res = camera.getLatestResult();
+            var captureTime = res.getTimestampSeconds();
+            var now = Timer.getFPGATimestamp();
 
-    //         // expectTrue(captureTime < now);
+            // expectTrue(captureTime < now);
 
-    //         System.out.println(
-    //                 "sequence "
-    //                         + res.metadata.sequenceID
-    //                         + " image capture "
-    //                         + captureTime
-    //                         + " recieved at "
-    //                         + now);
-    //     }
-    // }
+            System.out.println(
+                    "sequence "
+                            + res.metadata.sequenceID
+                            + " image capture "
+                            + captureTime
+                            + " recieved at "
+                            + now);
+        }
+    }
 }
diff --git a/photon-server/build.gradle b/photon-server/build.gradle
@@ -103,7 +103,7 @@ task findDeployTarget {
 
 task deploy {
     dependsOn findDeployTarget
-    dependsOn assemble
+    dependsOn 'shadowJar'
 
     doLast {
         println 'Starting deployment to ' + findDeployTarget.rmt.host

diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java
@@ -59,6 +59,8 @@
 import org.photonvision.vision.processes.VisionSourceManager;
 import org.photonvision.vision.target.TargetModel;
 
+import edu.wpi.first.hal.HAL;
+
 public class Main {
     public static final int DEFAULT_WEBPORT = 5800;
 
@@ -381,6 +383,11 @@ public static void main(String[] args) {
         }
         logger.info("WPI JNI libraries loaded.");
 
+        if (!HAL.initialize(500, 0)) {
+                logger.error("Failed to initialize the HAL! Giving up :(");
+                System.exit(1);
+        }
+
         try {
             if (Platform.isRaspberryPi()) {
                 LibCameraJNILoader.forceLoad();

diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java
@@ -28,7 +28,7 @@ public static class PingMetadata {
         long lastPongTime;
     }
 
-    final long handle;
+    private long handle;
 
     public TimeSyncClient(String server, int port, double interval) {
         this.handle = TimeSyncClient.create(server, port, interval);
@@ -39,7 +39,10 @@ public void start() {
     }
 
     public void stop() {
-        TimeSyncClient.stop(handle);
+        if (handle > 0) {
+            TimeSyncClient.stop(handle);
+            handle = 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
@@ -18,7 +18,7 @@
 package org.photonvision.jni;
 
 public class TimeSyncServer {
-    final long handle;
+    private long handle;
 
     public TimeSyncServer(int port) {
         this.handle = TimeSyncServer.create(port);
@@ -29,7 +29,10 @@ public void start() {
     }
 
     public void stop() {
-        TimeSyncServer.stop(handle);
+        if (handle > 0) {
+            TimeSyncServer.stop(handle);
+            handle = 0;
+        }
     }
 
     private static native long create(int port);

diff --git a/photon-targeting/src/test/java/net/TimeSyncTest.java b/photon-targeting/src/test/java/net/TimeSyncTest.java
@@ -81,4 +81,29 @@ public void smoketest() throws InterruptedException {
         server.stop();
         client.stop();
     }
+
+    // @Test
+    // public void runClient() throws InterruptedException {
+    //     try {
+    //         load_wpilib();
+    //         PhotonTargetingJniLoader.load();
+    //     } catch (IOException e) {
+    //         assertTrue(false);
+    //     }
+
+    //     HAL.initialize(1000, 0);
+
+    //     var client = new TimeSyncClient("10.0.0.141", 5810, 1.0);
+
+    //     System.err.println("Waiting: PID=" + ProcessHandle.current().pid());
+
+    //     client.start();
+
+    //     for (int i = 0; i < 20; i++) {
+    //         Thread.sleep(1000);
+    //         System.out.println(client.getOffset());
+    //     }
+
+    //     client.stop();
+    // }
 }