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 *