diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 6c7e710ebb..7d342d07cb 100644
--- a/.github/workflows/build.yml
+++ b/.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 }}
diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml
index fb97eb77af..09f86fc6c6 100644
--- a/.github/workflows/lint-format.yml
+++ b/.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
diff --git a/README.md b/README.md
index 4fcf69c59e..bd5e5af698 100644
--- 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/build.gradle b/build.gradle
index 4ba2dad4cc..18aac2cc63 100644
--- a/build.gradle
+++ b/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 {
diff --git a/photon-core/build.gradle b/photon-core/build.gradle
index a63354dcb6..dc082ed58e 100644
--- 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/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
index 11aa38852b..fe42a3c969 100644
--- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
+++ b/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) {
diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
index 4cd2d1e0bd..b9addc1e45 100644
--- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
+++ b/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");
diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/PlatformUtils.java b/photon-core/src/main/java/org/photonvision/common/hardware/PlatformUtils.java
new file mode 100644
index 0000000000..c5bee8d20a
--- /dev/null
+++ b/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 .
+ */
+
+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;
+ }
+}
diff --git a/photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java b/photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java
index 0ce9b169c5..47b6540708 100644
--- a/photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java
+++ b/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!");
}
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
index 25b7112635..0717150acb 100644
--- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
+++ b/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();
diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
index 5efb81ef24..167effaea5 100644
--- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
+++ b/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());
}
/**
diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
index 5cc5e3cbaf..7198f264c5 100644
--- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
+++ b/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();
diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle
index 455bde947d..6b9d1f04e3 100644
--- a/photon-lib/build.gradle
+++ b/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())
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
index 1130d20a0a..e33d90efcc 100644
--- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
+++ b/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;
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 f5ab57e5c1..137fdd0aff 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();
- private long heartbeatCounter = 0;
+ private long heartbeatCounter = 1;
/** This simulated camera's {@link SimCameraProperties} */
public final SimCameraProperties prop;
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
index b61b939465..5aae41fedc 100644
--- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
+++ b/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()) {
diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
index 4b02a8d76b..d3eb6c1089 100644
--- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
+++ b/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();
+ // }
}
diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
index 9ae688e7ed..a96329e20d 100644
--- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
+++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -64,8 +64,8 @@ void testLowestAmbiguityStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
- 0,
- 0,
+ 11 * 1000000,
+ 1100000,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -130,7 +130,6 @@ void testLowestAmbiguityStrategy() {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
- cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
@@ -150,8 +149,8 @@ void testClosestToCameraHeightStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
- 0,
- 0,
+ 4000000,
+ 1100000,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -217,8 +216,6 @@ void testClosestToCameraHeightStrategy() {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
- cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6));
-
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
@@ -240,8 +237,8 @@ void closestToReferencePoseStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
- 0,
- 0,
+ 17000000,
+ 1100000,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -306,7 +303,6 @@ void closestToReferencePoseStrategy() {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
- cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -330,8 +326,8 @@ void closestToLastPose() {
cameraOne.result =
new PhotonPipelineResult(
0,
- 0,
- 0,
+ 1000000,
+ 1100000,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -396,7 +392,6 @@ void closestToLastPose() {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
- cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -412,8 +407,8 @@ void closestToLastPose() {
cameraOne.result =
new PhotonPipelineResult(
0,
- 0,
- 0,
+ 7000000,
+ 1100000,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -478,7 +473,6 @@ void closestToLastPose() {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
- cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6));
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
@@ -495,8 +489,8 @@ void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
- 0,
- 0,
+ 20000000,
+ 1100000,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -519,7 +513,6 @@ void cacheIsInvalidated() {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
- result.setReceiveTimestampMicros((long) (20 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -529,7 +522,7 @@ void cacheIsInvalidated() {
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
- cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
+ cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
Optional estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
@@ -563,8 +556,8 @@ void averageBestPoses() {
cameraOne.result =
new PhotonPipelineResult(
0,
- 0,
- 0,
+ 20 * 1000000,
+ 1100000,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -629,7 +622,6 @@ void averageBestPoses() {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
- cameraOne.result.setReceiveTimestampMicros(20 * 1000000);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
index 859aaf1cb6..0e460b3a7c 100644
--- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
+++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
@@ -77,6 +77,7 @@ public void testEmpty() {
@BeforeAll
public static void setUp() {
// NT live for debug purposes
+ NetworkTableInstance.getDefault().stopClient();
NetworkTableInstance.getDefault().startServer();
// No version check for testing
@@ -117,42 +118,50 @@ public void testVisibilityCupidShuffle() {
// To the right, to the right
var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
// To the right, to the right
robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
// To the left, to the left
robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
// To the left, to the left
robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
// now kick, now kick
robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
+
assertTrue(camera.getLatestResult().hasTargets());
// now kick, now kick
robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5));
visionSysSim.update(robotPose);
+
assertTrue(camera.getLatestResult().hasTargets());
// now walk it by yourself
robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
// now walk it by yourself
visionSysSim.adjustCamera(
cameraSim, new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI)));
visionSysSim.update(robotPose);
+
assertTrue(camera.getLatestResult().hasTargets());
}
@@ -169,11 +178,13 @@ public void testNotVisibleVert1() {
var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
+
assertTrue(camera.getLatestResult().hasTargets());
visionSysSim.adjustCamera( // vooop selfie stick
cameraSim, new Transform3d(new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI)));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
}
@@ -192,11 +203,13 @@ public void testNotVisibleVert2() {
var robotPose = new Pose2d(new Translation2d(13.98, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
+
assertTrue(camera.getLatestResult().hasTargets());
// 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);
+
assertFalse(camera.getLatestResult().hasTargets());
}
@@ -214,10 +227,12 @@ public void testNotVisibleTgtSize() {
var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
+
assertTrue(camera.getLatestResult().hasTargets());
robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
}
@@ -236,10 +251,12 @@ public void testNotVisibleTooFarForLEDs() {
var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
+
assertTrue(camera.getLatestResult().hasTargets());
robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
+
assertFalse(camera.getLatestResult().hasTargets());
}
@@ -259,6 +276,7 @@ public void testYawAngles(double testYaw) {
// If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+)
var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(testYaw));
visionSysSim.update(robotPose);
+
var res = camera.getLatestResult();
assertTrue(res.hasTargets());
var tgt = res.getBestTarget();
@@ -284,8 +302,11 @@ public void testPitchAngles(double testPitch) {
cameraSim,
new Transform3d(
new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)));
+
visionSysSim.update(robotPose);
+
var res = camera.getLatestResult();
+ System.out.println("Got result: " + res);
assertTrue(res.hasTargets());
var tgt = res.getBestTarget();
@@ -349,6 +370,7 @@ public void testDistanceCalc(double testDist, double testPitch, double testHeigh
// 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.
+
var res = camera.getLatestResult();
assertTrue(res.hasTargets());
var tgt = res.getBestTarget();
@@ -450,6 +472,7 @@ public void testMultipleTargets() {
var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25));
visionSysSim.update(robotPose);
+
var res = camera.getLatestResult();
assertTrue(res.hasTargets());
List tgtList;
@@ -479,6 +502,7 @@ public void testPoseEstimation() {
new VisionTargetSim(tagList.get(0).pose, TargetModel.kAprilTag16h5, 0));
visionSysSim.update(robotPose);
+
var results =
VisionEstimation.estimateCamPosePNP(
camera.getCameraMatrix().get(),
@@ -499,6 +523,7 @@ public void testPoseEstimation() {
new VisionTargetSim(tagList.get(2).pose, TargetModel.kAprilTag16h5, 2));
visionSysSim.update(robotPose);
+
results =
VisionEstimation.estimateCamPosePNP(
camera.getCameraMatrix().get(),
diff --git a/photon-server/build.gradle b/photon-server/build.gradle
index 7c8c2ba078..2515d65b9e 100644
--- a/photon-server/build.gradle
+++ b/photon-server/build.gradle
@@ -47,9 +47,7 @@ tasks.register("buildAndCopyUI") {
run {
environment "PATH_PREFIX", "../"
-}
-run {
if (project.hasProperty("profile")) {
jvmArgs=[
"-Dcom.sun.management.jmxremote=true",
@@ -105,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
index 9dd5e7ad70..3204855fdf 100644
--- a/photon-server/src/main/java/org/photonvision/Main.java
+++ b/photon-server/src/main/java/org/photonvision/Main.java
@@ -17,6 +17,7 @@
package org.photonvision;
+import edu.wpi.first.hal.HAL;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
@@ -379,7 +380,12 @@ 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.");
+
+ if (!HAL.initialize(500, 0)) {
+ logger.error("Failed to initialize the HAL! Giving up :(");
+ System.exit(1);
+ }
try {
if (Platform.isRaspberryPi()) {
diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle
index a00897e23e..6fa06b8629 100644
--- a/photon-targeting/build.gradle
+++ b/photon-targeting/build.gradle
@@ -1,3 +1,7 @@
+plugins {
+ id 'edu.wpi.first.WpilibTools' version '1.3.0'
+}
+
ext {
nativeName = "photontargeting"
}
@@ -19,6 +23,43 @@ nativeUtils {
sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
+apply plugin: 'edu.wpi.first.GradleJni'
+
+// explicitly specify the opencv libraries linked to by nativeUtils.useRequiredLibrary(it, "opencv_shared")
+def OPENCV_LIBRARIES_TO_EXCLUDE = [
+ "**/libopencv_core.so.*",
+ "**/libopencv_stitching.so.*",
+ "**/libopencv_dnn.so.*",
+ "**/libopencv_photo.so.*",
+ "**/libopencv_video.so.*",
+ "**/libopencv_flann.so.*",
+ "**/libopencv_ml.so.*",
+ "**/libopencv_calib3d.so.*",
+ "**/libopencv_imgproc.so.*",
+ "**/libopencv_objdetect.so.*",
+ "**/libopencv_videoio.so.*",
+ "**/libopencv_highgui.so.*",
+ "**/libopencv_aruco.so.*",
+ "**/libopencv_gapi.so.*",
+ "**/libopencv_features2d.so.*",
+ "**/libopencv_imgcodecs.so.*",
+]
+nativeUtils.nativeDependencyContainer.getByName("opencv_shared").extraSharedExcludes.addAll(OPENCV_LIBRARIES_TO_EXCLUDE)
+println nativeUtils.nativeDependencyContainer.getByName("opencv_shared").extraSharedExcludes.get()
+
+// Folder whose contents will be included in the final jar
+def outputsFolder = file("$buildDir/extra_resources")
+
+// Sync task: like the copy task, but all files that exist in the destination directory will be deleted before copying files
+task syncOutputsFolder(type: Sync) {
+ into outputsFolder
+}
+
+jar {
+ from outputsFolder
+ dependsOn syncOutputsFolder
+}
+
model {
components {
"${nativeName}"(NativeLibrarySpec) {
@@ -42,14 +83,59 @@ model {
it.tasks.withType(CppCompile) {
it.dependsOn generateProto
}
- if(project.hasProperty('includePhotonTargeting')) {
- lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared'
+ }
+
+ nativeUtils.useRequiredLibrary(it, "wpiutil_shared")
+ nativeUtils.useRequiredLibrary(it, "wpimath_shared")
+ nativeUtils.useRequiredLibrary(it, "wpinet_shared")
+ nativeUtils.useRequiredLibrary(it, "wpilibc_shared")
+ nativeUtils.useRequiredLibrary(it, "ntcore_shared")
+ }
+ "${nativeName}JNI"(JniNativeLibrarySpec) {
+
+ enableCheckTask project.hasProperty('doJniCheck')
+ javaCompileTasks << compileJava
+ jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio)
+ jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm32)
+ jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.linuxarm64)
+
+ sources {
+ cpp {
+ source {
+ srcDirs 'src/main/native/jni'
+ include '**/*.cpp', '**/*.cc'
+ }
}
}
- nativeUtils.useRequiredLibrary(it, "wpilib_shared")
- nativeUtils.useRequiredLibrary(it, "apriltag_shared")
- nativeUtils.useRequiredLibrary(it, "opencv_shared")
+ binaries.all {
+ lib library: nativeName, linkage: 'shared'
+ }
+
+ nativeUtils.useRequiredLibrary(it, "wpiutil_shared")
+ nativeUtils.useRequiredLibrary(it, "wpinet_shared")
+ nativeUtils.useRequiredLibrary(it, "ntcore_shared")
+ }
+
+ all {
+ binaries.withType(SharedLibraryBinarySpec) { binary ->
+ // check that we're building for the platform (per PArchOverride/wpilib plat detection)
+ if (binary.targetPlatform.name == jniPlatform) {
+
+ // only include release binaries (hard coded for now)
+ def isDebug = binary.buildType.name.contains('debug')
+ if (!isDebug) {
+ syncOutputsFolder {
+ // Just shove the shared library into the root of the jar output by photon-targeting:jar
+ from(binary.sharedLibraryFile) {
+ into "nativelibraries/${wpilibNativeName}/"
+ }
+ // And (not sure if this is a hack) make the jar task depend on the build task
+ dependsOn binary.identifier.projectScopedName
+ }
+ }
+ }
+ }
}
}
testSuites {
@@ -76,17 +162,11 @@ model {
it.tasks.withType(CppCompile) {
it.dependsOn generateProto
}
- if(project.hasProperty('includePhotonTargeting')) {
- lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared'
- }
}
- nativeUtils.useRequiredLibrary(it, "cscore_shared")
- nativeUtils.useRequiredLibrary(it, "cameraserver_shared")
nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
nativeUtils.useRequiredLibrary(it, "googletest_static")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
- nativeUtils.useRequiredLibrary(it, "opencv_shared")
}
}
@@ -129,3 +209,27 @@ cppHeadersZip {
into '/'
}
}
+
+// make sure native libraries can be loaded in tests
+test {
+ classpath += files(outputsFolder)
+ dependsOn syncOutputsFolder
+}
+
+// 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("wpiutil")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/Platform.java b/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java
similarity index 90%
rename from photon-core/src/main/java/org/photonvision/common/hardware/Platform.java
rename to photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java
index 57f1eab868..e2d0155609 100644
--- a/photon-core/src/main/java/org/photonvision/common/hardware/Platform.java
+++ b/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java
@@ -17,13 +17,11 @@
package org.photonvision.common.hardware;
-import com.jogamp.common.os.Platform.OSType;
import edu.wpi.first.util.RuntimeDetector;
import java.io.BufferedReader;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
-import org.photonvision.common.util.ShellExec;
@SuppressWarnings("unused")
public enum Platform {
@@ -63,7 +61,6 @@ private enum OSType {
UNKNOWN
}
- private static final ShellExec shell = new ShellExec(true, false);
public final String description;
public final String nativeLibraryFolderName;
public final boolean isPi;
@@ -72,7 +69,6 @@ private enum OSType {
// Set once at init, shouldn't be needed after.
private static final Platform currentPlatform = getCurrentPlatform();
- private static final boolean isRoot = checkForRoot();
Platform(
String description,
@@ -115,10 +111,6 @@ public static String getNativeLibraryFolderName() {
return currentPlatform.nativeLibraryFolderName;
}
- public static boolean isRoot() {
- return isRoot;
- }
-
public static boolean isSupported() {
return currentPlatform.isSupported;
}
@@ -131,29 +123,6 @@ public static boolean isSupported() {
private static final String UnknownPlatformString =
String.format("Unknown Platform. OS: %s, Architecture: %s", OS_NAME, OS_ARCH);
- @SuppressWarnings("StatementWithEmptyBody")
- private static boolean checkForRoot() {
- if (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;
- }
-
private static Platform getCurrentPlatform() {
if (RuntimeDetector.isWindows()) {
if (RuntimeDetector.is32BitIntel()) {
diff --git a/photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java b/photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java
new file mode 100644
index 0000000000..943ee1d5a4
--- /dev/null
+++ b/photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java
@@ -0,0 +1,64 @@
+/*
+ * 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 .
+ */
+
+package org.photonvision.jni;
+
+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 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 = 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 false;
+ }
+
+ // It's important that we don't mangle the names of these files on Windows at
+ // least
+ 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;
+ byte[] buffer = new byte[1024];
+ while ((read = in.read(buffer)) != -1) {
+ fos.write(buffer, 0, read);
+ }
+ fos.close();
+ in.close();
+
+ System.load(temp.getAbsolutePath());
+
+ System.out.println("Successfully loaded shared object " + temp.getName());
+ }
+
+ return true;
+ }
+}
diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java
new file mode 100644
index 0000000000..55bf987ec4
--- /dev/null
+++ b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java
@@ -0,0 +1,76 @@
+/*
+ * 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 .
+ */
+
+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;
+ long pingsSent;
+ long pingsRecieved;
+ long lastPongTime;
+ }
+
+ private long handle;
+
+ public TimeSyncClient(String server, int port, double interval) {
+ this.handle = TimeSyncClient.create(server, port, interval);
+ }
+
+ public void start() {
+ TimeSyncClient.start(handle);
+ }
+
+ public void stop() {
+ if (handle > 0) {
+ TimeSyncClient.stop(handle);
+ handle = 0;
+ }
+ }
+
+ /**
+ * 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);
+
+ private static native void stop(long handle);
+
+ private static native long getOffset(long handle);
+
+ private static native PingMetadata getLatestMetadata();
+}
diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java
new file mode 100644
index 0000000000..455702531a
--- /dev/null
+++ b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java
@@ -0,0 +1,43 @@
+/*
+ * 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 .
+ */
+
+package org.photonvision.jni;
+
+public class TimeSyncServer {
+ private long handle;
+
+ public TimeSyncServer(int port) {
+ this.handle = TimeSyncServer.create(port);
+ }
+
+ public void start() {
+ TimeSyncServer.start(handle);
+ }
+
+ public void stop() {
+ if (handle > 0) {
+ TimeSyncServer.stop(handle);
+ handle = 0;
+ }
+ }
+
+ private static native long create(int port);
+
+ private static native void start(long handle);
+
+ private static native void stop(long handle);
+}
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
index 75a4882291..e2adb61d9e 100644
--- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
@@ -52,19 +52,19 @@ public PhotonPipelineResult() {
* Constructs a pipeline result.
*
* @param sequenceID The number of frames processed by this camera since boot
- * @param captureTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
- * captured the image this result contains the targeting info of
- * @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
- * published targeting info
+ * @param captureTimestampMicros The time, in uS in the coprocessor's timebase, that the
+ * coprocessor captured the image this result contains the targeting info of
+ * @param publishTimestampMicros The time, in uS in the coprocessor's timebase, that the
+ * coprocessor published targeting info
* @param targets The list of targets identified by the pipeline.
*/
public PhotonPipelineResult(
long sequenceID,
- long captureTimestamp,
- long publishTimestamp,
+ long captureTimestampMicros,
+ long publishTimestampMicros,
List targets) {
this(
- new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
+ new PhotonPipelineMetadata(captureTimestampMicros, publishTimestampMicros, sequenceID),
targets,
Optional.empty());
}
@@ -169,12 +169,10 @@ public Optional 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;
}
diff --git a/photon-targeting/src/main/native/cpp/net/TimeSyncClientServer.cpp b/photon-targeting/src/main/native/cpp/net/TimeSyncClientServer.cpp
new file mode 100644
index 0000000000..6c5d286e55
--- /dev/null
+++ b/photon-targeting/src/main/native/cpp/net/TimeSyncClientServer.cpp
@@ -0,0 +1,320 @@
+/*
+ * 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 .
+ */
+
+#include "net/TimeSyncClientServer.h"
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include "ntcore_cpp.h"
+
+template <>
+struct wpi::Struct {
+ static constexpr std::string_view GetTypeString() { return "TspPing"; }
+ static constexpr size_t GetSize() { return 10; }
+ static constexpr std::string_view GetSchema() {
+ return "uint8 version;uint8 message_id;uint64 client_time";
+ }
+
+ static TspPing Unpack(std::span data) {
+ return TspPing{
+ wpi::UnpackStruct(data),
+ wpi::UnpackStruct(data),
+ wpi::UnpackStruct(data),
+ };
+ }
+ static void Pack(std::span data, const TspPing& value) {
+ wpi::PackStruct<0>(data, value.version);
+ wpi::PackStruct<1>(data, value.message_id);
+ wpi::PackStruct<2>(data, value.client_time);
+ }
+};
+
+template <>
+struct wpi::Struct {
+ static constexpr std::string_view GetTypeString() { return "TspPong"; }
+ static constexpr size_t GetSize() { return 18; }
+ static constexpr std::string_view GetSchema() {
+ return "uint8 version;uint8 message_id;uint64 client_time;uint64_t "
+ "server_time";
+ }
+
+ static TspPong Unpack(std::span data) {
+ return TspPong{
+ TspPing{
+ wpi::UnpackStruct(data),
+ wpi::UnpackStruct(data),
+ wpi::UnpackStruct(data),
+ },
+ wpi::UnpackStruct(data),
+ };
+ }
+ static void Pack(std::span data, const TspPong& value) {
+ wpi::PackStruct<0>(data, value.version);
+ wpi::PackStruct<1>(data, value.message_id);
+ wpi::PackStruct<2>(data, value.client_time);
+ wpi::PackStruct<10>(data, value.server_time);
+ }
+};
+
+static_assert(wpi::StructSerializable);
+static_assert(wpi::StructSerializable);
+
+static void ClientLoggerFunc(unsigned int level, const char* file,
+ unsigned int line, const char* msg) {
+ if (level == 20) {
+ fmt::print(stderr, "TimeSyncClient: {}\n", msg);
+ return;
+ }
+
+ std::string_view levelmsg;
+ if (level >= 50) {
+ levelmsg = "CRITICAL";
+ } else if (level >= 40) {
+ levelmsg = "ERROR";
+ } else if (level >= 30) {
+ levelmsg = "WARNING";
+ } else {
+ return;
+ }
+ fmt::print(stderr, "TimeSyncClient: {}: {} ({}:{})\n", levelmsg, msg, file,
+ line);
+}
+
+static void ServerLoggerFunc(unsigned int level, const char* file,
+ unsigned int line, const char* msg) {
+ if (level == 20) {
+ fmt::print(stderr, "TimeSyncServer: {}\n", msg);
+ return;
+ }
+
+ std::string_view levelmsg;
+ if (level >= 50) {
+ levelmsg = "CRITICAL";
+ } else if (level >= 40) {
+ levelmsg = "ERROR";
+ } else if (level >= 30) {
+ levelmsg = "WARNING";
+ } else {
+ return;
+ }
+ fmt::print(stderr, "TimeSyncServer: {}: {} ({}:{})\n", levelmsg, msg, file,
+ line);
+}
+
+void wpi::TimeSyncServer::UdpCallback(uv::Buffer& data, size_t n,
+ const sockaddr& sender, unsigned flags) {
+ fmt::println("TimeSyncServer got ping!");
+
+ TspPing ping{wpi::UnpackStruct(data.bytes())};
+
+ if (ping.version != 1) {
+ WPI_ERROR(m_logger, "Bad version from client?");
+ return;
+ }
+ if (ping.message_id != 1) {
+ WPI_ERROR(m_logger, "Bad message id from client?");
+ return;
+ }
+
+ uint64_t current_time = m_timeProvider();
+
+ TspPong pong{ping, current_time};
+ pong.message_id = 2;
+
+ wpi::SmallVector::GetSize()> pongData(
+ wpi::Struct::GetSize());
+ wpi::PackStruct(pongData, pong);
+
+ // Wrap our buffer - pongData should free itself for free
+ wpi::uv::Buffer pongBuf{pongData};
+ int sent =
+ m_udp->TrySend(sender, wpi::SmallVector{pongBuf});
+ fmt::println("Pong ret: {}", sent);
+ if (static_cast(sent) != wpi::Struct::GetSize()) {
+ WPI_ERROR(m_logger, "Didn't send the whole pong back?");
+ return;
+ }
+
+ WPI_INFO(m_logger, "Got ping: {} {} {}", ping.version, ping.message_id,
+ ping.client_time);
+ WPI_INFO(m_logger, "Sent pong: {} {} {} {}", pong.version, pong.message_id,
+ pong.client_time, pong.server_time);
+}
+
+wpi::TimeSyncServer::TimeSyncServer(int port,
+ std::function timeProvider)
+ : m_logger{::ServerLoggerFunc},
+ m_timeProvider{timeProvider},
+ m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)} {
+ m_loopRunner.ExecSync(
+ [this, port](uv::Loop&) { m_udp->Bind("0.0.0.0", port); });
+}
+
+void wpi::TimeSyncServer::Start() {
+ m_loopRunner.ExecSync([this](uv::Loop&) {
+ m_udp->received.connect(&wpi::TimeSyncServer::UdpCallback, this);
+ m_udp->StartRecv();
+ });
+}
+
+void wpi::TimeSyncServer::Stop() { m_loopRunner.Stop(); }
+
+void wpi::TimeSyncClient::Tick() {
+ fmt::println("wpi::TimeSyncClient::Tick");
+ // Regardless of if we've gotten a pong back yet, we'll ping again. this is
+ // pretty naive but should be "fine" for now?
+
+ uint64_t ping_local_time{m_timeProvider()};
+
+ TspPing ping{.version = 1, .message_id = 1, .client_time = ping_local_time};
+
+ wpi::SmallVector::GetSize()> pingData(
+ wpi::Struct::GetSize());
+ wpi::PackStruct(pingData, ping);
+
+ // Wrap our buffer - pingData should free itself
+ wpi::uv::Buffer pingBuf{pingData};
+ int sent = m_udp->TrySend(wpi::SmallVector{pingBuf});
+
+ if (static_cast(sent) != wpi::Struct::GetSize()) {
+ WPI_ERROR(m_logger, "Didn't send the whole ping out? sent {} bytes", sent);
+ return;
+ }
+
+ {
+ std::lock_guard lock{m_offsetMutex};
+ m_metadata.pingsSent++;
+ }
+
+ m_lastPing = ping;
+}
+
+void wpi::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes,
+ const sockaddr& sender, unsigned flags) {
+ uint64_t pong_local_time = m_timeProvider();
+
+ if (static_cast(nbytes) != wpi::Struct::GetSize()) {
+ WPI_ERROR(m_logger, "Got {} bytes for pong?", nbytes);
+ return;
+ }
+
+ TspPong pong{
+ wpi::UnpackStruct(buf.bytes()),
+ };
+
+ fmt::println("->[client] Got pong: {} {} {} {}", pong.version,
+ pong.message_id, pong.client_time, pong.server_time);
+
+ if (pong.version != 1) {
+ fmt::println("Bad version from server?");
+ return;
+ }
+ if (pong.message_id != 2) {
+ fmt::println("Bad message id from server?");
+ return;
+ }
+
+ TspPing ping = m_lastPing;
+
+ if (pong.client_time != ping.client_time) {
+ WPI_WARNING(m_logger,
+ "Pong was not a reply to our ping? Got ping {} vs pong {}",
+ ping.client_time, pong.client_time);
+ return;
+ }
+
+ // when time = send_time+rtt2/2, server time = server time
+ // server time = local time + offset
+ // offset = (server time - local time) = (server time) - (send_time +
+ // rtt2/2)
+ auto rtt2 = pong_local_time - ping.client_time;
+ int64_t serverTimeOffsetUs = pong.server_time - rtt2 / 2 - ping.client_time;
+
+ {
+ std::lock_guard lock{m_offsetMutex};
+ m_metadata.offset = serverTimeOffsetUs;
+ m_metadata.pongsRecieved++;
+ m_metadata.lastPongTime = pong_local_time;
+ }
+
+ using std::cout;
+ fmt::println("Ping-ponged! RTT2 {} uS, offset {} uS", rtt2,
+ serverTimeOffsetUs);
+ fmt::println("Estimated server time {} s",
+ (m_timeProvider() + serverTimeOffsetUs) / 1000000.0);
+}
+
+wpi::TimeSyncClient::TimeSyncClient(std::string_view server, int remote_port,
+ std::chrono::milliseconds ping_delay,
+ std::function timeProvider)
+ : m_logger(::ClientLoggerFunc),
+ m_timeProvider(timeProvider),
+ m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)},
+ m_pingTimer{wpi::uv::Timer::Create(m_loopRunner.GetLoop())},
+ m_serverIP{server},
+ m_serverPort{remote_port},
+ m_loopDelay(ping_delay) {
+ struct sockaddr_in serverAddr;
+ uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr);
+
+ m_loopRunner.ExecSync(
+ [this, serverAddr](uv::Loop&) { m_udp->Connect(serverAddr); });
+
+ fmt::println("Starting client for {}:{}", server, remote_port);
+}
+
+void wpi::TimeSyncClient::Start() {
+ fmt::println("Connecting recieved");
+
+ m_loopRunner.ExecSync([this](uv::Loop&) {
+ m_udp->received.connect(&wpi::TimeSyncClient::UdpCallback, this);
+ m_udp->StartRecv();
+ });
+
+ fmt::println("Starting pinger");
+ using namespace std::chrono_literals;
+ m_pingTimer->timeout.connect(&wpi::TimeSyncClient::Tick, this);
+
+ m_loopRunner.ExecSync(
+ [this](uv::Loop&) { m_pingTimer->Start(m_loopDelay, m_loopDelay); });
+}
+
+void wpi::TimeSyncClient::Stop() { m_loopRunner.Stop(); }
+
+int64_t wpi::TimeSyncClient::GetOffset() {
+ std::lock_guard lock{m_offsetMutex};
+ return m_metadata.offset;
+}
+
+wpi::TimeSyncClient::Metadata wpi::TimeSyncClient::GetMetadata() {
+ std::lock_guard lock{m_offsetMutex};
+ return m_metadata;
+}
diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp
index 0bd08c1cee..cc67ec213f 100644
--- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp
+++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp
@@ -17,6 +17,8 @@
#include "photon/targeting/proto/MultiTargetPNPResultProto.h"
+#include
+
#include "photon.pb.h"
#include "photon/targeting/proto/PNPResultProto.h"
diff --git a/photon-targeting/src/main/native/include/net/TimeSyncClientServer.h b/photon-targeting/src/main/native/include/net/TimeSyncClientServer.h
new file mode 100644
index 0000000000..d13bee8947
--- /dev/null
+++ b/photon-targeting/src/main/native/include/net/TimeSyncClientServer.h
@@ -0,0 +1,135 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include "ntcore_cpp.h"
+
+struct TspPing {
+ uint8_t version;
+ uint8_t message_id;
+ uint64_t client_time;
+};
+
+struct TspPong : public TspPing {
+ TspPong(TspPing ping, uint64_t servertime)
+ : TspPing{ping}, server_time{servertime} {}
+ uint64_t server_time;
+};
+
+namespace wpi {
+
+class TimeSyncServer {
+ using SharedUdpPtr = std::shared_ptr;
+
+ EventLoopRunner m_loopRunner{};
+
+ wpi::Logger m_logger;
+ std::function m_timeProvider;
+ SharedUdpPtr m_udp;
+
+ std::thread m_listener;
+
+ private:
+ void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender,
+ unsigned flags);
+
+ public:
+ explicit TimeSyncServer(int port = 5810,
+ std::function timeProvider = nt::Now);
+
+ /**
+ * Start listening for pings
+ */
+ void Start();
+ /**
+ * Stop our loop runner. After stopping, we cannot restart.
+ */
+ void Stop();
+};
+
+class TimeSyncClient {
+ public:
+ struct Metadata {
+ int64_t offset{0};
+ size_t pingsSent;
+ size_t pongsRecieved;
+ uint64_t lastPongTime;
+ };
+
+ private:
+ using SharedUdpPtr = std::shared_ptr;
+ using SharedTimerPtr = std::shared_ptr;
+
+ EventLoopRunner m_loopRunner{};
+
+ wpi::Logger m_logger;
+ std::function m_timeProvider;
+
+ SharedUdpPtr m_udp;
+ SharedTimerPtr m_pingTimer;
+
+ std::string m_serverIP;
+ int m_serverPort;
+
+ std::chrono::milliseconds m_loopDelay;
+
+ std::mutex m_offsetMutex;
+ Metadata m_metadata;
+
+ // We only allow the most recent ping to stay alive, so only keep track of it
+ TspPing m_lastPing;
+
+ void Tick();
+
+ void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender,
+ unsigned flags);
+
+ public:
+ TimeSyncClient(std::string_view server, int remote_port,
+ std::chrono::milliseconds ping_delay,
+ std::function timeProvider = nt::Now);
+
+ void Start();
+ void Stop();
+ int64_t GetOffset();
+ Metadata GetMetadata();
+};
+
+} // namespace wpi
diff --git a/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp b/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp
new file mode 100644
index 0000000000..a7c197295c
--- /dev/null
+++ b/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp
@@ -0,0 +1,102 @@
+/*
+ * 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 .
+ */
+
+#include
+
+#include
+#include
+
+#include "net/TimeSyncClientServer.h"
+
+using namespace wpi;
+
+extern "C" {
+
+/*
+ * Class: org_photonvision_jni_TimeSyncClient
+ * Method: create
+ * Signature: (Ljava/lang/String;ID)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_photonvision_jni_TimeSyncClient_create
+ (JNIEnv* env, jclass, jstring name, jint port, jdouble interval)
+{
+ using namespace std::chrono_literals;
+
+ const char* c_name{env->GetStringUTFChars(name, 0)};
+ std::string cpp_name{c_name};
+ jlong ret{reinterpret_cast(
+ new TimeSyncClient(cpp_name, static_cast(port),
+ std::chrono::duration_cast(
+ std::chrono::duration(interval))))};
+ env->ReleaseStringUTFChars(name, c_name);
+ return ret;
+}
+
+/*
+ * Class: org_photonvision_jni_TimeSyncClient
+ * Method: start
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_photonvision_jni_TimeSyncClient_start
+ (JNIEnv*, jclass, jlong ptr)
+{
+ TimeSyncClient* client = reinterpret_cast(ptr);
+ client->Start();
+}
+
+/*
+ * Class: org_photonvision_jni_TimeSyncClient
+ * Method: stop
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_photonvision_jni_TimeSyncClient_stop
+ (JNIEnv*, jclass, jlong ptr)
+{
+ TimeSyncClient* client = reinterpret_cast(ptr);
+ client->Stop();
+ delete client;
+}
+
+/*
+ * Class: org_photonvision_jni_TimeSyncClient
+ * Method: getOffset
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_photonvision_jni_TimeSyncClient_getOffset
+ (JNIEnv*, jclass, jlong ptr)
+{
+ TimeSyncClient* client = reinterpret_cast(ptr);
+ return client->GetOffset();
+}
+
+/*
+ * Class: org_photonvision_jni_TimeSyncClient
+ * Method: getLatestMetadata
+ * Signature: ()Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_org_photonvision_jni_TimeSyncClient_getLatestMetadata
+ (JNIEnv*, jclass)
+{
+ return nullptr;
+}
+
+} // extern "C"
diff --git a/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp b/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp
new file mode 100644
index 0000000000..42a4f088b1
--- /dev/null
+++ b/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp
@@ -0,0 +1,68 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+
+#include
+
+#include "net/TimeSyncClientServer.h"
+
+using namespace wpi;
+
+extern "C" {
+
+/*
+ * Class: org_photonvision_jni_TimeSyncServer
+ * Method: create
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_photonvision_jni_TimeSyncServer_create
+ (JNIEnv*, jclass, jint port)
+{
+ return reinterpret_cast(new TimeSyncServer(port));
+}
+
+/*
+ * Class: org_photonvision_jni_TimeSyncServer
+ * Method: start
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_photonvision_jni_TimeSyncServer_start
+ (JNIEnv*, jclass, jlong ptr)
+{
+ TimeSyncServer* server = reinterpret_cast(ptr);
+ server->Start();
+}
+
+/*
+ * Class: org_photonvision_jni_TimeSyncServer
+ * Method: stop
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_photonvision_jni_TimeSyncServer_stop
+ (JNIEnv*, jclass, jlong ptr)
+{
+ TimeSyncServer* server = reinterpret_cast(ptr);
+ server->Stop();
+ delete server;
+}
+
+} // extern "C"
diff --git a/photon-targeting/src/test/java/net/TimeSyncTest.java b/photon-targeting/src/test/java/net/TimeSyncTest.java
new file mode 100644
index 0000000000..9ab64679f8
--- /dev/null
+++ b/photon-targeting/src/test/java/net/TimeSyncTest.java
@@ -0,0 +1,109 @@
+/*
+ * 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 .
+ */
+
+package net;
+
+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.NetworkTablesJNI;
+import edu.wpi.first.util.CombinedRuntimeLoader;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.io.IOException;
+import org.junit.jupiter.api.Test;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncClient;
+import org.photonvision.jni.TimeSyncServer;
+
+public class TimeSyncTest {
+ 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(
+ TimeSyncTest.class, "wpiutiljni", "wpimathjni", "wpinetjni", "ntcorejni", "wpiHaljni");
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ }
+
+ @Test
+ public void smoketest() throws InterruptedException {
+ try {
+ load_wpilib();
+ PhotonTargetingJniLoader.load();
+ } catch (IOException e) {
+ assertTrue(false);
+ }
+
+ HAL.initialize(1000, 0);
+
+ var server = new TimeSyncServer(5812);
+ var client = new TimeSyncClient("127.0.0.1", 5812, 1.0);
+
+ System.err.println("Waiting: PID=" + ProcessHandle.current().pid());
+
+ server.start();
+ client.start();
+
+ for (int i = 0; i < 5; i++) {
+ Thread.sleep(1000);
+ System.out.println(client.getOffset());
+ }
+
+ 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();
+ // }
+}
diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp
index 0fb5697029..5398fae4c1 100644
--- a/photon-targeting/src/test/native/cpp/PacketTest.cpp
+++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp
@@ -16,6 +16,7 @@
*/
#include
+#include
#include
diff --git a/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp b/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp
new file mode 100644
index 0000000000..04a3509db6
--- /dev/null
+++ b/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp
@@ -0,0 +1,38 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+#include
+
+TEST(TimeSyncProtocolTest, TestClient) {
+ using namespace wpi;
+ using namespace std::chrono_literals;
+
+ HAL_Initialize(500, 0);
+
+ TimeSyncServer server{5812};
+
+ server.Start();
+
+ for (int i = 0; i < 10; i++) {
+ std::this_thread::sleep_for(50ms);
+ // busywait
+ }
+
+ server.Stop();
+}
diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py
index 617f0ccbad..9afa3b3551 100644
--- a/photonlib-python-examples/aimandrange/robot.py
+++ b/photonlib-python-examples/aimandrange/robot.py
@@ -25,8 +25,6 @@
import math
import wpilib
-import wpimath
-import wpimath.geometry
import drivetrain
from photonlibpy import PhotonCamera
diff --git a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py
index e195147143..235b61fa34 100644
--- a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py
+++ b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py
@@ -2,5 +2,3 @@
This test module imports tests that come with pyfrc, and can be used
to test basic functionality of just about any robot.
"""
-
-from pyfrc.tests import *
diff --git a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py
index e195147143..235b61fa34 100644
--- a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py
+++ b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py
@@ -2,5 +2,3 @@
This test module imports tests that come with pyfrc, and can be used
to test basic functionality of just about any robot.
"""
-
-from pyfrc.tests import *
diff --git a/photonlib-python-examples/poseest/tests/pyfrc_test.py b/photonlib-python-examples/poseest/tests/pyfrc_test.py
index e195147143..235b61fa34 100644
--- a/photonlib-python-examples/poseest/tests/pyfrc_test.py
+++ b/photonlib-python-examples/poseest/tests/pyfrc_test.py
@@ -2,5 +2,3 @@
This test module imports tests that come with pyfrc, and can be used
to test basic functionality of just about any robot.
"""
-
-from pyfrc.tests import *