Update to match new WPILib organization

build.gradle

@@ -1,13 +1,13 @@
-import edu.wpi.first.toolchain.*
+import org.wpilib.toolchain.*
 
 plugins {
     id "cpp"
     id "com.diffplug.spotless" version "8.1.0"
-    id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2025.0"
+    id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0"
-    id "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
+    id "org.wpilib.GradleRIO" version "2027.0.0-alpha-2"
     id 'org.photonvision.tools.WpilibTools' version '2.4.1-photon'
     id 'com.google.protobuf' version '0.9.3' apply false
-    id 'edu.wpi.first.GradleJni' version '1.1.0'
+    id 'org.wpilib.GradleJni' version '2027.0.0'
     id "org.ysb33r.doxygen" version "2.0.0" apply false
     id 'com.gradleup.shadow' version '8.3.4' apply false
     id "com.github.node-gradle.node" version "7.0.1" apply false
@@ -33,7 +33,7 @@ ext.allOutputsFolder = file("$project.buildDir/outputs")
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2027.0.0-alpha-2"
+    wpilibVersion = "2027.0.0-alpha-3-203-g0001ddc7e"
     wpimathVersion = wpilibVersion
     openCVYear = "2025"
     openCVversion = "4.10.0-3"

docs/source/docs/apriltag-pipelines/multitag.md

@@ -46,7 +46,7 @@ This multi-target pose estimate can be accessed using PhotonLib. We suggest usin
       {
         auto multiTagResult = result.MultiTagResult();
         if (multiTagResult.has_value()) {
-          frc::Transform3d fieldToCamera = multiTagResult->estimatedPose.best;
+          wpi::math::Transform3d fieldToCamera = multiTagResult->estimatedPose.best;
         }
       }
 

docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md

@@ -60,7 +60,7 @@ You can also get the pipeline latency from a pipeline result using the `getLaten
    .. code-block:: c++
 
       // Get the pipeline latency.
-      units::second_t latency = result.GetLatency();
+      wpi::units::second_t latency = result.GetLatency();
 
    .. code-block:: python
 

docs/source/docs/programming/photonlib/getting-target-data.md

@@ -107,7 +107,7 @@ You can get a list of tracked targets using the `getTargets()`/`GetTargets()` (J
    .. code-block:: c++
 
       // Get a list of currently tracked targets.
-      wpi::ArrayRef<photonlib::PhotonTrackedTarget> targets = result.GetTargets();
+      std::span<photonlib::PhotonTrackedTarget> targets = result.GetTargets();
 
    .. code-block:: python
 
@@ -166,8 +166,8 @@ You can get the {ref}`best target <docs/reflectiveAndShape/contour-filtering:Con
       double pitch = target.GetPitch();
       double area = target.GetArea();
       double skew = target.GetSkew();
-      frc::Transform2d pose = target.GetCameraToTarget();
+      wpi::math::Transform2d pose = target.GetCameraToTarget();
-      wpi::SmallVector<std::pair<double, double>, 4> corners = target.GetCorners();
+      wpi::util::SmallVector<std::pair<double, double>, 4> corners = target.GetCorners();
 
    .. code-block:: python
 
@@ -206,8 +206,8 @@ All of the data above (**except skew**) is available when using AprilTags.
       // Get information from target.
       int targetID = target.GetFiducialId();
       double poseAmbiguity = target.GetPoseAmbiguity();
-      frc::Transform3d bestCameraToTarget = target.getBestCameraToTarget();
+      wpi::math::Transform3d bestCameraToTarget = target.getBestCameraToTarget();
-      frc::Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget();
+      wpi::math::Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget();
 
    .. code-block:: python
 

docs/source/docs/programming/photonlib/using-target-data.md

@@ -38,8 +38,8 @@ You can get your robot's `Pose2D` on the field using various camera data, target
    .. code-block:: c++
 
       // Calculate robot's field relative pose
-      frc::Pose2D robotPose = photonlib::EstimateFieldToRobot(
+      wpi::math::Pose2d robotPose = photonlib::EstimateFieldToRobot(
-        kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, frc::Rotation2d(units::degree_t(-target.GetYaw())), frc::Rotation2d(units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot);
+        kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, wpi::math::Rotation2d(wpi::units::degree_t(-target.GetYaw())), wpi::math::Rotation2d(wpi::units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot);
 
    .. code-block:: python
 
@@ -106,8 +106,8 @@ You can get a [translation](https://docs.wpilib.org/en/latest/docs/software/adva
    .. code-block:: c++
 
       // Calculate a translation from the camera to the target.
-      frc::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslation(
+      wpi::math::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslation(
-        distance, frc::Rotation2d(units::degree_t(-target.GetYaw())));
+        distance, wpi::math::Rotation2d(wpi::units::degree_t(-target.GetYaw())));
 
    .. code-block:: python
 

docs/source/docs/quick-start/networking.md

@@ -81,7 +81,7 @@ If you would like to access your Ethernet-connected vision device from a compute
 
     .. code-block:: c++
 
-        wpi::PortForwarder::GetInstance().Add(5800, "photonvision.local", 5800);
+        wpi::net::PortForwarder::GetInstance().Add(5800, "photonvision.local", 5800);
 
     .. code-block:: python
 

photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java

@@ -20,7 +20,6 @@ package org.photonvision.common.configuration;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.cscore.UsbCameraInfo;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.UUID;
@@ -35,6 +34,7 @@ import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.pipeline.CVPipelineSettings;
 import org.photonvision.vision.pipeline.DriverModePipelineSettings;
 import org.photonvision.vision.processes.PipelineManager;
+import org.wpilib.vision.camera.UsbCameraInfo;
 
 public class CameraConfiguration {
     private static final Logger logger = new Logger(CameraConfiguration.class, LogGroup.Camera);

photon-core/src/main/java/org/photonvision/common/configuration/LegacyConfigProvider.java

@@ -18,8 +18,6 @@
 package org.photonvision.common.configuration;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 import java.io.File;
 import java.io.IOException;
 import java.io.UncheckedIOException;
@@ -40,6 +38,8 @@ import org.photonvision.common.util.file.JacksonUtils;
 import org.photonvision.vision.pipeline.CVPipelineSettings;
 import org.photonvision.vision.pipeline.DriverModePipelineSettings;
 import org.photonvision.vision.processes.VisionSource;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.apriltag.AprilTagFields;
 import org.zeroturnaround.zip.ZipUtil;
 
 class LegacyConfigProvider extends ConfigProvider {

photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java

@@ -17,11 +17,11 @@
 
 package org.photonvision.common.configuration;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import java.util.Collection;
 import java.util.HashMap;
 import java.util.List;
 import org.photonvision.vision.processes.VisionSource;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
 
 public class PhotonConfiguration {
     private final HardwareConfig hardwareConfig;

photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java

@@ -17,9 +17,6 @@
 
 package org.photonvision.common.configuration;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.cscore.UsbCameraInfo;
 import java.io.File;
 import java.io.IOException;
 import java.io.UncheckedIOException;
@@ -40,6 +37,9 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.file.JacksonUtils;
 import org.photonvision.vision.pipeline.CVPipelineSettings;
 import org.photonvision.vision.pipeline.DriverModePipelineSettings;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.apriltag.AprilTagFields;
+import org.wpilib.vision.camera.UsbCameraInfo;
 
 /**
  * Saves settings in a SQLite database file (called photon.sqlite).

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

@@ -17,11 +17,11 @@
 
 package org.photonvision.common.dataflow.networktables;
 
-import edu.wpi.first.networktables.NetworkTableEvent;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.Subscriber;
 import java.util.EnumSet;
 import java.util.function.Consumer;
+import org.wpilib.networktables.NetworkTableEvent;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.Subscriber;
 
 public class NTDataChangeListener {
     private final NetworkTableInstance instance;

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

@@ -17,10 +17,6 @@
 
 package org.photonvision.common.dataflow.networktables;
 
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEvent;
-import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.List;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
@@ -35,6 +31,10 @@ import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.NetworkTableEvent;
+import org.wpilib.networktables.NetworkTablesJNI;
 
 public class NTDataPublisher implements CVPipelineResultConsumer {
     private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.General);
@@ -177,7 +177,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
 
         var offset = NetworkTablesManager.getInstance().getOffset();
 
-        // Transform the metadata timestamps from the local nt::Now timebase to the Time Sync Server's
+        // Transform the metadata timestamps from the local wpi::nt::Now timebase to the Time Sync
+        // Server's
         // timebase
         var simplified =
                 new PhotonPipelineResult(

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

@@ -17,15 +17,15 @@
 
 package org.photonvision.common.dataflow.networktables;
 
-import edu.wpi.first.networktables.BooleanSubscriber;
-import edu.wpi.first.networktables.IntegerSubscriber;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEvent.Kind;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StringSubscriber;
 import java.util.EnumSet;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.wpilib.networktables.BooleanSubscriber;
+import org.wpilib.networktables.IntegerSubscriber;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.NetworkTableEvent.Kind;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.StringSubscriber;
 
 // Helper to print when the robot transitions modes
 public class NTDriverStation {
@@ -125,7 +125,7 @@ public class NTDriverStation {
     }
 
     // Copied from
-    // https://github.com/wpilibsuite/allwpilib/blob/07192285f65321a2f7363227a2216f09b715252d/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java#L123C1-L140C4
+    // https://github.com/wpilibsuite/allwpilib/blob/07192285f65321a2f7363227a2216f09b715252d/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java#L123C1-L140C4
     // TODO: upstream!
     /**
      * Gets the current control word of the driver station.

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

@@ -17,18 +17,6 @@
 
 package org.photonvision.common.dataflow.networktables;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.cscore.CameraServerJNI;
-import edu.wpi.first.networktables.LogMessage;
-import edu.wpi.first.networktables.MultiSubscriber;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEvent;
-import edu.wpi.first.networktables.NetworkTableEvent.Kind;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StringSubscriber;
-import edu.wpi.first.wpilibj.Alert;
-import edu.wpi.first.wpilibj.Alert.AlertType;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.io.IOException;
 import java.util.Arrays;
 import java.util.EnumSet;
@@ -47,6 +35,18 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.networking.NetworkUtils;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.file.JacksonUtils;
+import org.wpilib.networktables.LogMessage;
+import org.wpilib.networktables.MultiSubscriber;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.NetworkTableEvent;
+import org.wpilib.networktables.NetworkTableEvent.Kind;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.StringSubscriber;
+import org.wpilib.smartdashboard.SmartDashboard;
+import org.wpilib.util.Alert;
+import org.wpilib.util.Alert.AlertType;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.camera.CameraServerJNI;
 
 public class NetworkTablesManager {
     private static final Logger logger =

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

@@ -17,16 +17,16 @@
 
 package org.photonvision.common.dataflow.networktables;
 
-import edu.wpi.first.cscore.CameraServerJNI;
-import edu.wpi.first.networktables.IntegerPublisher;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
 import org.photonvision.common.configuration.NetworkConfig;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.jni.TimeSyncClient;
 import org.photonvision.jni.TimeSyncServer;
+import org.wpilib.networktables.IntegerPublisher;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.vision.camera.CameraServerJNI;
 
 public class TimeSyncManager {
     private static final Logger logger = new Logger(TimeSyncManager.class, LogGroup.NetworkTables);
@@ -66,7 +66,7 @@ public class TimeSyncManager {
     public synchronized long getOffset() {
         // if we're a client, return the offset to server time
         if (m_client != null) return m_client.getOffset();
-        // if we're a server, our time (nt::Now) is the same as network time
+        // if we're a server, our time (wpi::nt::Now) is the same as network time
         if (m_server != null) return 0;
 
         // ????? should never hit

photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIProgramSettings.java

@@ -17,7 +17,7 @@
 
 package org.photonvision.common.dataflow.websocket;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
 
 public class UIProgramSettings {
     public UIProgramSettings(

photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java

@@ -21,8 +21,6 @@ import com.diozero.api.DeviceMode;
 import com.diozero.internal.spi.NativeDeviceFactoryInterface;
 import com.diozero.sbc.BoardPinInfo;
 import com.diozero.sbc.DeviceFactoryHelper;
-import edu.wpi.first.networktables.IntegerPublisher;
-import edu.wpi.first.networktables.IntegerSubscriber;
 import java.io.IOException;
 import java.util.HashSet;
 import java.util.List;
@@ -38,6 +36,9 @@ import org.photonvision.common.hardware.gpio.CustomDeviceFactory;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ShellExec;
+import org.photonvision.common.util.TimedTaskManager;
+import org.wpilib.networktables.IntegerPublisher;
+import org.wpilib.networktables.IntegerSubscriber;
 
 public class HardwareManager {
     private static HardwareManager instance;

photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java

@@ -21,7 +21,6 @@ import com.diozero.devices.LED;
 import com.diozero.devices.PwmLed;
 import com.diozero.internal.spi.NativeDeviceFactoryInterface;
 import com.diozero.sbc.BoardPinInfo;
-import edu.wpi.first.networktables.NetworkTableEvent;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.concurrent.atomic.AtomicInteger;
@@ -31,6 +30,7 @@ import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.math.MathUtils;
+import org.wpilib.networktables.NetworkTableEvent;
 
 public class VisionLED implements AutoCloseable {
     private static final Logger logger = new Logger(VisionLED.class, LogGroup.VisionModule);

photon-core/src/main/java/org/photonvision/common/hardware/metrics/SystemMonitor.java

@@ -17,9 +17,6 @@
 
 package org.photonvision.common.hardware.metrics;
 
-import edu.wpi.first.cscore.CameraServerJNI;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.ProtobufPublisher;
 import java.io.IOException;
 import java.nio.file.FileStore;
 import java.nio.file.Files;
@@ -37,6 +34,9 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.networking.NetworkUtils;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.file.ProgramDirectoryUtilities;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.ProtobufPublisher;
+import org.wpilib.vision.camera.CameraServerJNI;
 import oshi.SystemInfo;
 import oshi.hardware.CentralProcessor;
 import oshi.hardware.CentralProcessor.PhysicalProcessor;

photon-core/src/main/java/org/photonvision/common/hardware/metrics/proto/DeviceMetricsProto.java

@@ -17,9 +17,9 @@
 
 package org.photonvision.common.hardware.metrics.proto;
 
-import edu.wpi.first.util.protobuf.Protobuf;
 import org.photonvision.common.hardware.metrics.DeviceMetrics;
 import org.photonvision.proto.Photon.ProtobufDeviceMetrics;
+import org.wpilib.util.protobuf.Protobuf;
 import us.hebi.quickbuf.Descriptors.Descriptor;
 
 public class DeviceMetricsProto implements Protobuf<DeviceMetrics, ProtobufDeviceMetrics> {

photon-core/src/main/java/org/photonvision/common/logging/Logger.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.common.logging;
 
-import edu.wpi.first.math.Pair;
 import java.io.*;
 import java.nio.file.Path;
 import java.text.ParseException;
@@ -28,6 +27,7 @@ import org.photonvision.common.configuration.PathManager;
 import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.util.TimedTaskManager;
+import org.wpilib.math.util.Pair;
 
 /** TODO: get rid of static {} blocks and refactor to singleton pattern */
 public class Logger {

photon-core/src/main/java/org/photonvision/common/logging/PvCSCoreLogger.java

@@ -17,8 +17,8 @@
 
 package org.photonvision.common.logging;
 
-import edu.wpi.first.cscore.CameraServerJNI;
 import java.nio.file.Path;
+import org.wpilib.vision.camera.CameraServerJNI;
 
 /** Redirect cscore logs to our logger */
 public class PvCSCoreLogger {

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

@@ -17,7 +17,6 @@
 
 package org.photonvision.common.networking;
 
-import edu.wpi.first.networktables.NetworkTableInstance;
 import java.io.IOException;
 import java.net.InetAddress;
 import java.net.NetworkInterface;
@@ -30,6 +29,7 @@ import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.ShellExec;
+import org.wpilib.networktables.NetworkTableInstance;
 
 public class NetworkUtils {
     private static final Logger logger = new Logger(NetworkUtils.class, LogGroup.General);

photon-core/src/main/java/org/photonvision/common/util/SerializationUtils.java

@@ -17,10 +17,10 @@
 
 package org.photonvision.common.util;
 
-import edu.wpi.first.math.geometry.Transform3d;
 import java.util.HashMap;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.wpilib.math.geometry.Transform3d;
 
 public final class SerializationUtils {
     private static final Logger logger = new Logger(SerializationUtils.class, LogGroup.General);

photon-core/src/main/java/org/photonvision/common/util/TestUtils.java

@@ -18,9 +18,6 @@
 package org.photonvision.common.util;
 
 import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.util.Units;
 import java.awt.HeadlessException;
 import java.io.File;
 import java.io.IOException;
@@ -31,6 +28,8 @@ import org.opencv.highgui.HighGui;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.util.Units;
 
 public class TestUtils {
     @SuppressWarnings("unused")

photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -17,19 +17,19 @@
 
 package org.photonvision.common.util.math;
 
-import edu.wpi.first.apriltag.AprilTagPoseEstimate;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.CoordinateSystem;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-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.networktables.NetworkTablesJNI;
 import java.util.Arrays;
 import java.util.List;
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
+import org.wpilib.math.geometry.CoordinateSystem;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.util.Units;
+import org.wpilib.networktables.NetworkTablesJNI;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimate;
 
 public class MathUtils {
     MathUtils() {}

photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java

@@ -21,8 +21,6 @@ import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.geometry.Pose3d;
-import java.awt.Color;
 import java.nio.file.Path;
 import java.util.Arrays;
 import java.util.List;
@@ -31,10 +29,7 @@ import org.opencv.core.Core;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
 import org.opencv.core.Point3;
-import org.opencv.core.Scalar;
+import org.wpilib.math.geometry.Pose3d;
-import org.opencv.imgcodecs.Imgcodecs;
-import org.opencv.imgproc.Imgproc;
-import org.photonvision.common.util.ColorHelper;
 
 // Ignore the previous calibration data that was stored in the json file.
 @JsonIgnoreProperties(ignoreUnknown = true)

photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java

@@ -19,14 +19,14 @@ package org.photonvision.vision.calibration;
 
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Num;
 import java.util.Arrays;
 import org.ejml.simple.SimpleMatrix;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
 import org.photonvision.vision.opencv.Releasable;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.util.Num;
 
 /** JSON-serializable image. Data is stored as a raw JSON array. */
 public class JsonMatOfDouble implements Releasable {

photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java

@@ -17,9 +17,6 @@
 
 package org.photonvision.vision.camera;
 
-import edu.wpi.first.cscore.UsbCameraInfo;
-import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.util.PixelFormat;
 import java.nio.file.Path;
 import java.util.HashMap;
 import org.photonvision.common.configuration.CameraConfiguration;
@@ -28,6 +25,9 @@ import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
 import org.photonvision.vision.processes.VisionSource;
 import org.photonvision.vision.processes.VisionSourceSettables;
+import org.wpilib.util.PixelFormat;
+import org.wpilib.vision.camera.UsbCameraInfo;
+import org.wpilib.vision.camera.VideoMode;
 
 public class FileVisionSource extends VisionSource {
     private final FileFrameProvider frameProvider;

photon-core/src/main/java/org/photonvision/vision/camera/PVCameraInfo.java

@@ -24,9 +24,9 @@ import com.fasterxml.jackson.annotation.JsonProperty;
 import com.fasterxml.jackson.annotation.JsonSubTypes;
 import com.fasterxml.jackson.annotation.JsonTypeInfo;
 import com.fasterxml.jackson.annotation.JsonTypeName;
-import edu.wpi.first.cscore.UsbCameraInfo;
 import java.util.Arrays;
 import java.util.Objects;
+import org.wpilib.vision.camera.UsbCameraInfo;
 
 @JsonTypeInfo(use = JsonTypeInfo.Id.NAME, include = JsonTypeInfo.As.WRAPPER_OBJECT)
 @JsonIgnoreProperties(ignoreUnknown = true)

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311CameraSettables.java

@@ -17,8 +17,8 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
 
 /*
  * This class holds the non-windows camera quirks for the Arducam OV2311. This version supports auto-exposure, while windows does not.

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV2311WindowsCameraSettables.java

@@ -17,10 +17,9 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoException;
-import edu.wpi.first.math.MathUtil;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
+import org.wpilib.vision.camera.VideoException;
 
 /*
  * This class holds the windows specific camera quirks for the Arducam ov2311. A windows version is needed because windows doesn't expose the auto exposure properties of the arducam.
@@ -50,7 +49,7 @@ public class ArduOV2311WindowsCameraSettables extends GenericUSBCameraSettables
     public void setExposureRaw(double exposureRaw) {
         if (exposureRaw >= 0.0) {
             try {
-                int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
+                int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure);
                 camera.setExposureManual(propVal);
                 this.lastExposureRaw = exposureRaw;
             } catch (VideoException e) {

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9281CameraSettables.java

@@ -17,8 +17,8 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
 
 public class ArduOV9281CameraSettables extends GenericUSBCameraSettables {
     public ArduOV9281CameraSettables(CameraConfiguration configuration, UsbCamera camera) {

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/ArduOV9782CameraSettables.java

@@ -17,8 +17,8 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
 
 public class ArduOV9782CameraSettables extends GenericUSBCameraSettables {
     public ArduOV9782CameraSettables(CameraConfiguration configuration, UsbCamera camera) {

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/GenericUSBCameraSettables.java

@@ -17,12 +17,6 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoException;
-import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.cscore.VideoProperty;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.PixelFormat;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collections;
@@ -33,6 +27,11 @@ import java.util.stream.Collectors;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.vision.camera.CameraQuirk;
 import org.photonvision.vision.processes.VisionSourceSettables;
+import org.wpilib.util.PixelFormat;
+import org.wpilib.vision.camera.UsbCamera;
+import org.wpilib.vision.camera.VideoException;
+import org.wpilib.vision.camera.VideoMode;
+import org.wpilib.vision.camera.VideoProperty;
 
 public class GenericUSBCameraSettables extends VisionSourceSettables {
     // We need to remember the last exposure set when exiting
@@ -131,7 +130,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
 
             softSet("white_balance_automatic", 0);
 
-            int propVal = (int) MathUtil.clamp(temp, minWhiteBalanceTemp, maxWhiteBalanceTemp);
+            int propVal = (int) Math.clamp(temp, minWhiteBalanceTemp, maxWhiteBalanceTemp);
 
             logger.debug(
                     "Setting property "
@@ -230,7 +229,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
             try {
                 if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
 
-                int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
+                int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure);
 
                 logger.debug(
                         "Setting property "

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/InnoOV9281CameraSettables.java

@@ -17,8 +17,8 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
 
 public class InnoOV9281CameraSettables extends GenericUSBCameraSettables {
     public InnoOV9281CameraSettables(CameraConfiguration configuration, UsbCamera camera) {

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kCameraSettables.java

@@ -17,11 +17,10 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoException;
-import edu.wpi.first.math.MathUtil;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.util.math.MathUtils;
+import org.wpilib.vision.camera.UsbCamera;
+import org.wpilib.vision.camera.VideoException;
 
 public class LifeCam3kCameraSettables extends GenericUSBCameraSettables {
     // Lifecam only allows specific exposures. Pulled this list from
@@ -45,7 +44,7 @@ public class LifeCam3kCameraSettables extends GenericUSBCameraSettables {
     public void setExposureRaw(double exposureRaw) {
         if (exposureRaw >= 0.0) {
             try {
-                int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
+                int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure);
 
                 propVal = MathUtils.quantize(propVal, allowableExposures);
 

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/LifeCam3kWindowsCameraSettables.java

@@ -17,10 +17,9 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoException;
-import edu.wpi.first.math.MathUtil;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
+import org.wpilib.vision.camera.VideoException;
 
 public class LifeCam3kWindowsCameraSettables extends GenericUSBCameraSettables {
     public LifeCam3kWindowsCameraSettables(CameraConfiguration configuration, UsbCamera camera) {
@@ -41,7 +40,7 @@ public class LifeCam3kWindowsCameraSettables extends GenericUSBCameraSettables {
     public void setExposureRaw(double exposureRaw) {
         if (exposureRaw >= 0.0) {
             try {
-                int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
+                int propVal = (int) Math.clamp(exposureRaw, minExposure, maxExposure);
 
                 // exposureAbsProp.set(propVal);
                 camera.setExposureManual(propVal);

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/PsEyeCameraSettables.java

@@ -17,8 +17,8 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
 
 public class PsEyeCameraSettables extends GenericUSBCameraSettables {
     public PsEyeCameraSettables(CameraConfiguration configuration, UsbCamera camera) {

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/See3Cam24CUGSettables.java

@@ -17,8 +17,8 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cscore.UsbCamera;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.wpilib.vision.camera.UsbCamera;
 
 /*
  * This class holds the camera quirks for the See3Cam 24UGS.

photon-core/src/main/java/org/photonvision/vision/camera/USBCameras/USBCameraSource.java

@@ -17,10 +17,6 @@
 
 package org.photonvision.vision.camera.USBCameras;
 
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoException;
-import edu.wpi.first.cscore.VideoProperty;
 import java.util.*;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.hardware.Platform;
@@ -33,6 +29,10 @@ import org.photonvision.vision.frame.FrameProvider;
 import org.photonvision.vision.frame.provider.USBFrameProvider;
 import org.photonvision.vision.processes.VisionSource;
 import org.photonvision.vision.processes.VisionSourceSettables;
+import org.wpilib.vision.camera.UsbCamera;
+import org.wpilib.vision.camera.VideoException;
+import org.wpilib.vision.camera.VideoProperty;
+import org.wpilib.vision.stream.CameraServer;
 
 public class USBCameraSource extends VisionSource {
     private final Logger logger;

photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSettables.java

@@ -17,10 +17,6 @@
 
 package org.photonvision.vision.camera.csi;
 
-import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.util.PixelFormat;
 import java.util.HashMap;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.util.math.MathUtils;
@@ -28,6 +24,9 @@ import org.photonvision.raspi.LibCameraJNI;
 import org.photonvision.vision.camera.csi.LibcameraGpuSource.FPSRatedVideoMode;
 import org.photonvision.vision.opencv.ImageRotationMode;
 import org.photonvision.vision.processes.VisionSourceSettables;
+import org.wpilib.math.util.Pair;
+import org.wpilib.util.PixelFormat;
+import org.wpilib.vision.camera.VideoMode;
 
 public class LibcameraGpuSettables extends VisionSourceSettables {
     private FPSRatedVideoMode currentVideoMode;
@@ -142,7 +141,7 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
         // 80,000 uS seems like an exposure value that will be greater than ever needed while giving
         // enough control over exposure.
-        exposureRaw = MathUtil.clamp(exposureRaw, minExposure, maxExposure);
+        exposureRaw = Math.clamp(exposureRaw, minExposure, maxExposure);
 
         var success = LibCameraJNI.setExposure(r_ptr, (int) exposureRaw);
         if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera exposure");
@@ -164,7 +163,7 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
         // than ever needed) from 0 to 100 (UI values).
         var success =
                 LibCameraJNI.setAnalogGain(
-                        r_ptr, MathUtil.clamp(MathUtils.map(gain, 0.0, 100.0, 1.0, 10.0), 1.0, 10.0));
+                        r_ptr, Math.clamp(MathUtils.map(gain, 0.0, 100.0, 1.0, 10.0), 1.0, 10.0));
         if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera gain");
     }
 

photon-core/src/main/java/org/photonvision/vision/camera/csi/LibcameraGpuSource.java

@@ -17,8 +17,6 @@
 
 package org.photonvision.vision.camera.csi;
 
-import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.util.PixelFormat;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.logging.LogGroup;
@@ -29,6 +27,8 @@ import org.photonvision.vision.frame.FrameProvider;
 import org.photonvision.vision.frame.provider.LibcameraGpuFrameProvider;
 import org.photonvision.vision.processes.VisionSource;
 import org.photonvision.vision.processes.VisionSourceSettables;
+import org.wpilib.util.PixelFormat;
+import org.wpilib.vision.camera.VideoMode;
 
 public class LibcameraGpuSource extends VisionSource {
     static final Logger logger = new Logger(LibcameraGpuSource.class, LogGroup.Camera);

photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java

@@ -17,11 +17,11 @@
 
 package org.photonvision.vision.frame;
 
-import edu.wpi.first.cscore.VideoMode;
 import org.opencv.core.Point;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.opencv.ImageRotationMode;
+import org.wpilib.vision.camera.VideoMode;
 
 /** Represents the properties of a frame. */
 public class FrameStaticProperties {

photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java

@@ -17,11 +17,6 @@
 
 package org.photonvision.vision.frame.consumer;
 
-import edu.wpi.first.networktables.IntegerEntry;
-import edu.wpi.first.networktables.IntegerSubscriber;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.StringSubscriber;
-import edu.wpi.first.wpilibj.DriverStation.MatchType;
 import java.io.File;
 import java.text.DateFormat;
 import java.text.SimpleDateFormat;
@@ -34,6 +29,11 @@ import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.vision.frame.StaticFrames;
 import org.photonvision.vision.opencv.CVMat;
+import org.wpilib.driverstation.DriverStation.MatchType;
+import org.wpilib.networktables.IntegerEntry;
+import org.wpilib.networktables.IntegerSubscriber;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.StringSubscriber;
 
 public class FileSaveFrameConsumer implements Consumer<CVMat> {
     private final Logger logger = new Logger(FileSaveFrameConsumer.class, LogGroup.General);

photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java

@@ -17,12 +17,12 @@
 
 package org.photonvision.vision.frame.consumer;
 
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.*;
-import edu.wpi.first.util.PixelFormat;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.StaticFrames;
 import org.photonvision.vision.opencv.CVMat;
+import org.wpilib.util.PixelFormat;
+import org.wpilib.vision.camera.*;
+import org.wpilib.vision.stream.CameraServer;
 
 public class MJPGFrameConsumer implements AutoCloseable {
     private static final double MAX_FRAMERATE = -1;

photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java

@@ -17,17 +17,18 @@
 
 package org.photonvision.vision.frame.provider;
 
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.CvSink;
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.util.PixelFormat;
-import edu.wpi.first.util.RawFrame;
 import org.opencv.core.Mat;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.jni.CscoreExtras;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.processes.VisionSourceSettables;
+import org.wpilib.networktables.BooleanSubscriber;
+import org.wpilib.util.PixelFormat;
+import org.wpilib.util.RawFrame;
+import org.wpilib.vision.camera.CvSink;
+import org.wpilib.vision.camera.UsbCamera;
+import org.wpilib.vision.stream.CameraServer;
 
 public class USBFrameProvider extends CpuImageProcessor {
     private final Logger logger;
@@ -80,7 +81,7 @@ public class USBFrameProvider extends CpuImageProcessor {
         if (m_blockForFrames) {
             // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
             var mat = new CVMat();
-            // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
+            // This is from wpi::util::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
             // Hal::initialize was called
             // TODO - under the hood, this incurs an extra copy. We should avoid this, if we
             // can.
@@ -105,7 +106,7 @@ public class USBFrameProvider extends CpuImageProcessor {
                     cameraMode.width * 3,
                     PixelFormat.kBGR);
 
-            // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
+            // This is from wpi::util::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
             // Hal::initialize was called
             long captureTimeUs =
                     CscoreExtras.grabRawSinkFrameTimeoutLastTime(

photon-core/src/main/java/org/photonvision/vision/opencv/CVMat.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.opencv;
 
-import edu.wpi.first.util.RawFrame;
 import java.lang.ref.PhantomReference;
 import java.lang.ref.ReferenceQueue;
 import java.util.Collections;
@@ -27,6 +26,7 @@ import java.util.concurrent.atomic.AtomicInteger;
 import org.opencv.core.Mat;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.wpilib.util.RawFrame;
 
 public class CVMat implements Releasable {
     private static final Logger logger = new Logger(CVMat.class, LogGroup.General);

photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java

@@ -17,11 +17,11 @@
 
 package org.photonvision.vision.opencv;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
 import org.opencv.core.Core;
 import org.opencv.core.Point;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.util.Units;
 
 /**
  * An image rotation about the camera's +Z axis, which points out of the camera towards the world.

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java

@@ -17,13 +17,13 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.apriltag.AprilTagDetection;
-import edu.wpi.first.apriltag.AprilTagDetector;
 import java.util.List;
 import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipe.CVPipe;
+import org.wpilib.vision.apriltag.AprilTagDetection;
+import org.wpilib.vision.apriltag.AprilTagDetector;
 
 public class AprilTagDetectionPipe
         extends CVPipe<

photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java

@@ -17,16 +17,16 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.apriltag.AprilTagDetection;
-import edu.wpi.first.apriltag.AprilTagPoseEstimate;
-import edu.wpi.first.apriltag.AprilTagPoseEstimator;
-import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipe.CVPipe;
+import org.wpilib.vision.apriltag.AprilTagDetection;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimate;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimator;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimator.Config;
 
 public class AprilTagPoseEstimatorPipe
         extends CVPipe<

photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoPoseEstimatorPipe.java

@@ -17,11 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.apriltag.AprilTagPoseEstimate;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
 import java.util.ArrayList;
 import java.util.List;
 import org.opencv.calib3d.Calib3d;
@@ -34,6 +29,11 @@ import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipe.CVPipe;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimate;
 
 public class ArucoPoseEstimatorPipe
         extends CVPipe<

photon-core/src/main/java/org/photonvision/vision/pipe/impl/CalculateFPSPipe.java

@@ -17,9 +17,9 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.filter.LinearFilter;
-import edu.wpi.first.wpilibj.Timer;
 import org.photonvision.vision.pipe.CVPipe;
+import org.wpilib.math.filter.LinearFilter;
+import org.wpilib.system.Timer;
 
 public class CalculateFPSPipe
         extends CVPipe<Void, Integer, CalculateFPSPipe.CalculateFPSPipeParams> {

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.Pair;
 import java.awt.*;
 import java.util.List;
 import org.opencv.core.Mat;
@@ -32,6 +31,7 @@ import org.photonvision.vision.pipe.MutatingPipe;
 import org.photonvision.vision.target.RobotOffsetPointMode;
 import org.photonvision.vision.target.TargetCalculations;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.util.Pair;
 
 public class Draw2dCrosshairPipe
         extends MutatingPipe<

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.Pair;
 import java.awt.*;
 import java.util.List;
 import org.opencv.core.*;
@@ -31,6 +30,7 @@ import org.photonvision.vision.opencv.CVShape;
 import org.photonvision.vision.opencv.ContourShape;
 import org.photonvision.vision.pipe.MutatingPipe;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.util.Pair;
 
 public class Draw2dTargetsPipe
         extends MutatingPipe<Pair<Mat, List<TrackedTarget>>, Draw2dTargetsPipe.Draw2dTargetsParams> {

photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.Pair;
 import java.awt.*;
 import java.util.List;
 import org.opencv.calib3d.Calib3d;
@@ -33,6 +32,7 @@ import org.photonvision.vision.frame.FrameDivisor;
 import org.photonvision.vision.pipe.MutatingPipe;
 import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.util.Pair;
 
 public class Draw3dTargetsPipe
         extends MutatingPipe<Pair<Mat, List<TrackedTarget>>, Draw3dTargetsPipe.Draw3dContoursParams> {

photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.Pair;
 import java.awt.Color;
 import java.util.List;
 import org.opencv.core.Mat;
@@ -28,6 +27,7 @@ import org.photonvision.common.util.ColorHelper;
 import org.photonvision.vision.frame.FrameDivisor;
 import org.photonvision.vision.pipe.MutatingPipe;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.util.Pair;
 
 public class DrawCalibrationPipe
         extends MutatingPipe<

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.Pair;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
@@ -33,6 +32,7 @@ import org.photonvision.vision.frame.FrameDivisor;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.pipeline.UICalibrationData;
+import org.wpilib.math.util.Pair;
 
 public class FindBoardCornersPipe
         extends CVPipe<

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.Pair;
 import java.util.ArrayList;
 import java.util.List;
 import org.opencv.core.Mat;
@@ -27,6 +26,7 @@ import org.opencv.imgproc.Moments;
 import org.photonvision.vision.opencv.CVShape;
 import org.photonvision.vision.opencv.Contour;
 import org.photonvision.vision.pipe.CVPipe;
+import org.wpilib.math.util.Pair;
 
 public class FindCirclesPipe
         extends CVPipe<Pair<Mat, List<Contour>>, List<CVShape>, FindCirclesPipe.FindCirclePipeParams> {

photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
@@ -29,6 +28,7 @@ import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
 
 /** Estimate the camera pose given multiple Apriltag observations */
 public class MultiTargetPNPPipe

photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java

@@ -17,10 +17,6 @@
 
 package org.photonvision.vision.pipe.impl;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
 import java.util.List;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.Core;
@@ -33,6 +29,10 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.linalg.VecBuilder;
 
 public class SolvePNPPipe
         extends CVPipe<List<TrackedTarget>, List<TrackedTarget>, SolvePNPPipe.SolvePNPPipeParams> {

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -17,15 +17,6 @@
 
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.apriltag.AprilTagDetection;
-import edu.wpi.first.apriltag.AprilTagDetector;
-import edu.wpi.first.apriltag.AprilTagPoseEstimate;
-import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
-import edu.wpi.first.math.geometry.CoordinateSystem;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
@@ -50,6 +41,15 @@ import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe.MultiTargetPNPPipePa
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
+import org.wpilib.math.geometry.CoordinateSystem;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.util.Units;
+import org.wpilib.vision.apriltag.AprilTagDetection;
+import org.wpilib.vision.apriltag.AprilTagDetector;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimate;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimator.Config;
 
 public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipelineSettings> {
     private static final Logger logger = new Logger(AprilTagPipeline.class, LogGroup.VisionModule);

photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java

@@ -17,11 +17,6 @@
 
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.apriltag.AprilTagPoseEstimate;
-import edu.wpi.first.math.geometry.CoordinateSystem;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
@@ -45,6 +40,11 @@ import org.photonvision.vision.pipe.impl.MultiTargetPNPPipe.MultiTargetPNPPipePa
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
+import org.wpilib.math.geometry.CoordinateSystem;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.util.Units;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimate;
 
 public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
     private static final Logger logger = new Logger(ArucoPipeline.class, LogGroup.VisionModule);

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -17,8 +17,6 @@
 
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.util.Units;
 import java.nio.file.Path;
 import java.util.ArrayList;
 import java.util.List;
@@ -43,6 +41,8 @@ import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
 import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
+import org.wpilib.math.util.Pair;
+import org.wpilib.math.util.Units;
 
 public class Calibrate3dPipeline
         extends CVPipeline<CVPipelineResult, Calibration3dPipelineSettings> {

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java

@@ -17,9 +17,9 @@
 
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.math.util.Units;
 import org.opencv.core.Size;
 import org.photonvision.vision.frame.FrameDivisor;
+import org.wpilib.math.util.Units;
 
 public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
     public int boardHeight = 8;

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.math.Pair;
 import java.util.Arrays;
 import java.util.List;
 import org.opencv.core.Point;
@@ -32,6 +31,7 @@ import org.photonvision.vision.pipe.impl.*;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.PotentialTarget;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.util.Pair;
 
 public class ColoredShapePipeline
         extends CVPipeline<CVPipelineResult, ColoredShapePipelineSettings> {

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.math.Pair;
 import java.util.List;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
@@ -26,6 +25,7 @@ import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
 import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
 import org.photonvision.vision.pipe.impl.ResizeImagePipe;
 import org.photonvision.vision.pipeline.result.DriverModePipelineResult;
+import org.wpilib.math.util.Pair;
 
 public class DriverModePipeline
         extends CVPipeline<DriverModePipelineResult, DriverModePipelineSettings> {

photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.pipeline;
 
-import edu.wpi.first.math.Pair;
 import java.util.List;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
@@ -25,6 +24,7 @@ import org.photonvision.vision.opencv.DualOffsetValues;
 import org.photonvision.vision.pipe.impl.*;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.util.Pair;
 
 /**
  * This is a "fake" pipeline that is just used to move identical pipe sets out of real pipelines. It

photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java

@@ -104,9 +104,9 @@ public class CVPipelineResult implements Releasable {
     }
 
     /**
-     * Get the latency between now (wpi::Now) and the time at which the image was captured. FOOTGUN:
+     * Get the latency between now (wpi::util::Now) and the time at which the image was captured.
-     * the latency is relative to the time at which this method is called. Waiting to call this method
+     * FOOTGUN: the latency is relative to the time at which this method is called. Waiting to call
-     * will change the latency this method returns.
+     * this method will change the latency this method returns.
      */
     @Deprecated
     public double getLatencyMillis() {

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -17,9 +17,6 @@
 
 package org.photonvision.vision.processes;
 
-import edu.wpi.first.cscore.CameraServerJNI;
-import edu.wpi.first.cscore.VideoException;
-import edu.wpi.first.math.util.Units;
 import io.javalin.websocket.WsContext;
 import java.util.ArrayList;
 import java.util.HashMap;
@@ -57,6 +54,9 @@ import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.util.Units;
+import org.wpilib.vision.camera.CameraServerJNI;
+import org.wpilib.vision.camera.VideoException;
 
 /**
  * This is the God Class

photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java

@@ -18,7 +18,6 @@
 package org.photonvision.vision.processes;
 
 import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.math.Pair;
 import java.util.ArrayList;
 import java.util.LinkedHashMap;
 import java.util.List;
@@ -39,6 +38,7 @@ import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
 import org.photonvision.vision.pipeline.PipelineType;
 import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.target.RobotOffsetPointOperation;
+import org.wpilib.math.util.Pair;
 
 @SuppressWarnings("unchecked")
 public class VisionModuleChangeSubscriber extends DataChangeSubscriber {

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.processes;
 
-import edu.wpi.first.cscore.UsbCamera;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collection;
@@ -47,6 +46,7 @@ import org.photonvision.vision.camera.FileVisionSource;
 import org.photonvision.vision.camera.PVCameraInfo;
 import org.photonvision.vision.camera.USBCameras.USBCameraSource;
 import org.photonvision.vision.camera.csi.LibcameraGpuSource;
+import org.wpilib.vision.camera.UsbCamera;
 
 /**
  * This class manages starting up VisionModules for serialized devices ({@link

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java

@@ -17,7 +17,6 @@
 
 package org.photonvision.vision.processes;
 
-import edu.wpi.first.cscore.VideoMode;
 import java.util.HashMap;
 import org.opencv.core.Size;
 import org.photonvision.common.configuration.CameraConfiguration;
@@ -25,6 +24,7 @@ import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.FrameStaticProperties;
+import org.wpilib.vision.camera.VideoMode;
 
 public abstract class VisionSourceSettables {
     protected Logger logger;

photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java

@@ -21,7 +21,6 @@ import com.fasterxml.jackson.annotation.JsonAlias;
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonIgnore;
 import com.fasterxml.jackson.annotation.JsonProperty;
-import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
 import java.util.List;
 import org.opencv.core.MatOfPoint3f;
@@ -29,6 +28,7 @@ import org.opencv.core.Point3;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
 import org.photonvision.vision.pipe.impl.SolvePNPPipe;
+import org.wpilib.math.util.Units;
 
 /**
  * A model representing the vertices of targets with known shapes. The vertices are in the EDN

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -17,9 +17,6 @@
 
 package org.photonvision.vision.target;
 
-import edu.wpi.first.apriltag.AprilTagDetection;
-import edu.wpi.first.apriltag.AprilTagPoseEstimate;
-import edu.wpi.first.math.geometry.Transform3d;
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
@@ -40,6 +37,9 @@ import org.photonvision.vision.opencv.CVShape;
 import org.photonvision.vision.opencv.Contour;
 import org.photonvision.vision.opencv.DualOffsetValues;
 import org.photonvision.vision.opencv.Releasable;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.vision.apriltag.AprilTagDetection;
+import org.wpilib.vision.apriltag.AprilTagPoseEstimate;
 
 public class TrackedTarget implements Releasable {
     public final Contour m_mainContour;

photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java

@@ -21,7 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
-import edu.wpi.first.cscore.UsbCameraInfo;
 import java.io.IOException;
 import java.nio.file.Path;
 import java.util.Collection;
@@ -43,6 +42,7 @@ import org.photonvision.vision.pipeline.ColoredShapePipelineSettings;
 import org.photonvision.vision.pipeline.ObjectDetectionPipelineSettings;
 import org.photonvision.vision.pipeline.PipelineType;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
+import org.wpilib.vision.camera.UsbCameraInfo;
 
 public class SQLConfigTest {
     @TempDir private Path tmpDir;

photon-core/src/test/java/org/photonvision/common/util/CoordinateConversionTest.java

@@ -19,14 +19,14 @@ package org.photonvision.common.util;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.LoadJNI;
 import org.photonvision.common.util.math.MathUtils;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.linalg.VecBuilder;
 
 public class CoordinateConversionTest {
     @BeforeAll

photon-core/src/test/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumerTest.java

@@ -22,13 +22,6 @@ import static org.junit.jupiter.api.Assertions.assertNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.MatchType;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.io.File;
 import java.io.IOException;
 import java.util.Date;
@@ -43,6 +36,13 @@ import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.jni.LibraryLoader;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.DriverStation.MatchType;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.simulation.DriverStationSim;
+import org.wpilib.simulation.SimHooks;
+import org.wpilib.smartdashboard.SmartDashboard;
 
 public class FileSaveFrameConsumerTest {
     NetworkTableInstance inst = null;

photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java

@@ -19,7 +19,6 @@ package org.photonvision.vision.pipeline;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.geometry.Translation3d;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.LoadJNI;
@@ -30,6 +29,7 @@ import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TargetModel;
+import org.wpilib.math.geometry.Translation3d;
 
 public class AprilTagTest {
     @BeforeEach

photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java

@@ -19,7 +19,6 @@ package org.photonvision.vision.pipeline;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-import edu.wpi.first.math.geometry.Translation3d;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.LoadJNI;
@@ -30,6 +29,7 @@ import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TargetModel;
+import org.wpilib.math.geometry.Translation3d;
 
 public class ArucoPipelineTest {
     @BeforeEach

photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java

@@ -20,7 +20,6 @@ package org.photonvision.vision.pipeline;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.util.Units;
 import java.io.File;
 import java.io.IOException;
 import java.nio.file.Path;
@@ -47,6 +46,7 @@ import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.pipeline.UICalibrationData.BoardType;
 import org.photonvision.vision.pipeline.UICalibrationData.TagFamily;
+import org.wpilib.math.util.Units;
 
 public class Calibrate3dPipeTest {
     @BeforeAll

photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java

@@ -21,9 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.LoadJNI;
@@ -38,6 +35,9 @@ import org.photonvision.vision.opencv.ContourIntersectionDirection;
 import org.photonvision.vision.pipe.impl.HSVPipe;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TargetModel;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.util.Units;
 
 public class SolvePNPTest {
     private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";

photon-core/src/test/java/org/photonvision/vision/processes/MockUsbCameraSource.java

@@ -17,14 +17,14 @@
 
 package org.photonvision.vision.processes;
 
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoMode;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.camera.USBCameras.GenericUSBCameraSettables;
 import org.photonvision.vision.camera.USBCameras.USBCameraSource;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
+import org.wpilib.vision.camera.UsbCamera;
+import org.wpilib.vision.camera.VideoMode;
 
 public class MockUsbCameraSource extends USBCameraSource {
     /** Used for unit tests to better simulate a usb camera without a camera being present. */

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -21,7 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
-import edu.wpi.first.cscore.VideoMode;
 import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
@@ -40,6 +39,7 @@ import org.photonvision.vision.frame.FrameProvider;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.wpilib.vision.camera.VideoMode;
 
 public class VisionModuleManagerTest {
     @BeforeAll

photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java

@@ -21,7 +21,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.cscore.UsbCameraInfo;
 import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
@@ -35,6 +34,7 @@ import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.file.JacksonUtils;
 import org.photonvision.vision.camera.PVCameraInfo;
+import org.wpilib.vision.camera.UsbCameraInfo;
 
 public class VisionSourceManagerTest {
     // Test harness that overrides getConnectedCameras, but uses USB cameras for

photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java

@@ -20,8 +20,6 @@ package org.photonvision.vision.target;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation3d;
 import java.util.List;
 import java.util.stream.Stream;
 import org.junit.jupiter.api.BeforeAll;
@@ -39,6 +37,8 @@ import org.photonvision.vision.calibration.CameraLensModel;
 import org.photonvision.vision.calibration.JsonMatOfDouble;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.DualOffsetValues;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Translation3d;
 
 public class TargetCalculationsTest {
     private static Size imageSize = new Size(1280, 720);

photon-lib/build.gradle

@@ -12,7 +12,7 @@ ext {
 
 apply plugin: 'cpp'
 apply plugin: 'google-test-test-suite'
-apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: 'org.wpilib.NativeUtils'
 
 apply from: "${rootDir}/shared/config.gradle"
 apply from: "${rootDir}/shared/javacommon.gradle"
@@ -93,7 +93,7 @@ model {
             nativeUtils.useRequiredLibrary(it, "datalog_shared")
             nativeUtils.useRequiredLibrary(it, "cscore_shared")
             nativeUtils.useRequiredLibrary(it, "cameraserver_shared")
-            nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
+            nativeUtils.useRequiredLibrary(it, "wpilib_shared")
             nativeUtils.useRequiredLibrary(it, "googletest_static")
             nativeUtils.useRequiredLibrary(it, "apriltag_shared")
             nativeUtils.useRequiredLibrary(it, "opencv_shared")

photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java

@@ -24,10 +24,10 @@
 
 package org.photonvision;
 
-import edu.wpi.first.math.geometry.Pose3d;
 import java.util.List;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.wpilib.math.geometry.Pose3d;
 
 /** An estimated pose based on pipeline result */
 public class EstimatedRobotPose {

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -24,26 +24,6 @@
 
 package org.photonvision;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.numbers.*;
-import edu.wpi.first.networktables.BooleanPublisher;
-import edu.wpi.first.networktables.BooleanSubscriber;
-import edu.wpi.first.networktables.DoubleArraySubscriber;
-import edu.wpi.first.networktables.IntegerEntry;
-import edu.wpi.first.networktables.IntegerPublisher;
-import edu.wpi.first.networktables.IntegerSubscriber;
-import edu.wpi.first.networktables.MultiSubscriber;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.PubSubOption;
-import edu.wpi.first.networktables.StringSubscriber;
-import edu.wpi.first.wpilibj.Alert;
-import edu.wpi.first.wpilibj.Alert.AlertType;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Timer;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
@@ -53,6 +33,26 @@ import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.common.networktables.PacketSubscriber;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.timesync.TimeSyncSingleton;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.math.linalg.MatBuilder;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.numbers.*;
+import org.wpilib.math.util.Nat;
+import org.wpilib.networktables.BooleanPublisher;
+import org.wpilib.networktables.BooleanSubscriber;
+import org.wpilib.networktables.DoubleArraySubscriber;
+import org.wpilib.networktables.IntegerEntry;
+import org.wpilib.networktables.IntegerPublisher;
+import org.wpilib.networktables.IntegerSubscriber;
+import org.wpilib.networktables.MultiSubscriber;
+import org.wpilib.networktables.NetworkTable;
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.PubSubOption;
+import org.wpilib.networktables.StringSubscriber;
+import org.wpilib.system.Timer;
+import org.wpilib.util.Alert;
+import org.wpilib.util.Alert.AlertType;
 
 /** Represents a camera that is connected to PhotonVision. */
 public class PhotonCamera implements AutoCloseable {

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -24,28 +24,28 @@
 
 package org.photonvision;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.cscore.OpenCvLoader;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.math.numbers.N8;
-import edu.wpi.first.wpilibj.DriverStation;
 import java.util.*;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
+import org.wpilib.math.numbers.N8;
+import org.wpilib.math.util.Pair;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.camera.OpenCvLoader;
 
 /**
  * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a

photon-lib/src/main/java/org/photonvision/PhotonUtils.java

@@ -24,7 +24,7 @@
 
 package org.photonvision;
 
-import edu.wpi.first.math.geometry.*;
+import org.wpilib.math.geometry.*;
 
 public final class PhotonUtils {
     private PhotonUtils() {
@@ -39,7 +39,7 @@ public final class PhotonUtils {
      * for there to exist a height differential between goal and camera. The larger this differential,
      * the more accurate the distance estimate will be.
      *
-     * <p>Units can be converted using the {@link edu.wpi.first.math.util.Units} class.
+     * <p>Units can be converted using the {@link org.wpilib.math.util.Units} class.
      *
      * @param cameraHeightMeters The physical height of the camera off the floor in meters.
      * @param targetHeightMeters The physical height of the target off the floor in meters. This

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -24,19 +24,6 @@
 
 package org.photonvision.simulation;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.CvSource;
-import edu.wpi.first.cscore.OpenCvLoader;
-import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.util.PixelFormat;
-import edu.wpi.first.util.WPIUtilJNI;
-import edu.wpi.first.wpilibj.RobotController;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
@@ -60,6 +47,18 @@ import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.PnpResult;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.util.Pair;
+import org.wpilib.system.RobotController;
+import org.wpilib.util.PixelFormat;
+import org.wpilib.util.WPIUtilJNI;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
+import org.wpilib.vision.apriltag.AprilTagFields;
+import org.wpilib.vision.camera.CvSource;
+import org.wpilib.vision.camera.OpenCvLoader;
+import org.wpilib.vision.camera.VideoSource.ConnectionStrategy;
+import org.wpilib.vision.stream.CameraServer;
 
 /**
  * A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this
@@ -295,8 +294,8 @@ public class PhotonCameraSim implements AutoCloseable {
      */
     public boolean canSeeCorners(Point[] points) {
         for (var point : points) {
-            if (MathUtil.clamp(point.x, 0, prop.getResWidth()) != point.x
+            if (Math.clamp(point.x, 0, prop.getResWidth()) != point.x
-                    || MathUtil.clamp(point.y, 0, prop.getResHeight()) != point.y) {
+                    || Math.clamp(point.y, 0, prop.getResHeight()) != point.y) {
                 return false; // point is outside of resolution
             }
         }

photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java

@@ -25,19 +25,6 @@
 package org.photonvision.simulation;
 
 import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.math.MatBuilder;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
-import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.Vector;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.numbers.*;
-import edu.wpi.first.wpilibj.DriverStation;
 import java.io.IOException;
 import java.nio.file.Path;
 import java.util.ArrayList;
@@ -51,6 +38,18 @@ import org.opencv.core.Point;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.estimation.OpenCVHelp;
 import org.photonvision.estimation.RotTrlTransform3d;
+import org.wpilib.driverstation.DriverStation;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.linalg.MatBuilder;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.linalg.Vector;
+import org.wpilib.math.numbers.*;
+import org.wpilib.math.util.Nat;
+import org.wpilib.math.util.Pair;
 
 /**
  * Calibration and performance values for this camera.
@@ -167,7 +166,7 @@ public class SimCameraProperties {
 
     public SimCameraProperties setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
         if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) {
-            fovDiag = Rotation2d.fromDegrees(MathUtil.clamp(fovDiag.getDegrees(), 1, 179));
+            fovDiag = Rotation2d.fromDegrees(Math.clamp(fovDiag.getDegrees(), 1, 179));
             DriverStation.reportError(
                     "Requested invalid FOV! Clamping between (1, 179) degrees...", false);
         }

photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java

@@ -24,13 +24,6 @@
 
 package org.photonvision.simulation;
 
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.cscore.CvSource;
-import edu.wpi.first.cscore.OpenCvLoader;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.RawFrame;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.HashMap;
@@ -48,6 +41,13 @@ import org.opencv.core.Size;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.estimation.OpenCVHelp;
 import org.photonvision.estimation.RotTrlTransform3d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Translation3d;
+import org.wpilib.math.util.Units;
+import org.wpilib.util.RawFrame;
+import org.wpilib.vision.apriltag.AprilTag;
+import org.wpilib.vision.camera.CvSource;
+import org.wpilib.vision.camera.OpenCvLoader;
 
 public class VideoSimUtil {
     // Tag IDs start at 0, this should be set to 1 greater than the maximum tag ID required

photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java

@@ -24,15 +24,6 @@
 
 package org.photonvision.simulation;
 
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
 import java.util.Collection;
 import java.util.HashMap;
@@ -43,6 +34,15 @@ import java.util.Optional;
 import java.util.Set;
 import org.photonvision.PhotonCamera;
 import org.photonvision.estimation.TargetModel;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
+import org.wpilib.smartdashboard.Field2d;
+import org.wpilib.smartdashboard.SmartDashboard;
+import org.wpilib.system.Timer;
+import org.wpilib.vision.apriltag.AprilTag;
+import org.wpilib.vision.apriltag.AprilTagFieldLayout;
 
 /**
  * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot

photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java

@@ -24,10 +24,10 @@
 
 package org.photonvision.simulation;
 
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Translation3d;
 import java.util.List;
 import org.photonvision.estimation.TargetModel;
+import org.wpilib.math.geometry.Pose3d;
+import org.wpilib.math.geometry.Translation3d;
 
 /** Describes a vision target located somewhere on the field that your vision system can detect. */
 public class VisionTargetSim {

photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java

@@ -24,9 +24,9 @@
 
 package org.photonvision.timesync;
 
-import edu.wpi.first.util.RuntimeLoader;
 import java.io.IOException;
 import org.photonvision.jni.TimeSyncServer;
+import org.wpilib.util.runtime.RuntimeLoader;
 
 /** Helper to hold a single TimeSyncServer instance with some default config */
 public class TimeSyncSingleton {

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -29,20 +29,21 @@
 #include <string_view>
 #include <vector>
 
-#include <frc/Errors.h>
+#include <wpi/hal/UsageReporting.h>
-#include <frc/RobotController.h>
-#include <frc/Timer.h>
-#include <hal/FRCUsageReporting.h>
 #include <net/TimeSyncServer.h>
 #include <opencv2/core.hpp>
 #include <opencv2/core/utility.hpp>
-#include <wpi/json.h>
+#include <wpi/system/Errors.hpp>
+#include <wpi/system/RobotController.hpp>
+#include <wpi/system/Timer.hpp>
+#include <wpi/system/WPILibVersion.hpp>
+#include <wpi/util/json.hpp>
 
 #include "PhotonVersion.h"
 #include "photon/dataflow/structures/Packet.h"
 
-static constexpr units::second_t WARN_DEBOUNCE_SEC = 5_s;
+static constexpr wpi::units::second_t WARN_DEBOUNCE_SEC = 5_s;
-static constexpr units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
+static constexpr wpi::units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
 
 inline void verifyDependencies() {
   if (!(std::string_view{cv::getVersionString()} ==
@@ -83,8 +84,8 @@ inline void verifyDependencies() {
         ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
         ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n";
 
-    FRC_ReportWarning(bfw);
+    WPILIB_ReportWarning(bfw);
-    FRC_ReportError(frc::err::Error, bfw);
+    WPILIB_ReportError(wpi::err::Error, bfw);
     throw new std::runtime_error(std::string{bfw});
   }
 }
@@ -107,7 +108,7 @@ static void InitTspServer() {
 
 namespace photon {
 
-constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
+constexpr const wpi::units::second_t VERSION_CHECK_INTERVAL = 5_s;
 static const std::vector<std::string_view> PHOTON_PREFIX = {"/photonvision/"};
 static const std::string PHOTON_ALERT_GROUP{"PhotonAlerts"};
 bool PhotonCamera::VERSION_CHECK_ENABLED = true;
@@ -120,7 +121,7 @@ static const std::string TYPE_STRING =
     std::string{"photonstruct:PhotonPipelineResult:"} +
     std::string{SerdeType<PhotonPipelineResult>::GetSchemaHash()};
 
-PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
+PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance,
                            const std::string_view cameraName)
     : mainTable(instance.GetTable("photonvision")),
       rootTable(mainTable->GetSubTable(cameraName)),
@@ -163,20 +164,20 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
       disconnectAlert(PHOTON_ALERT_GROUP,
                       std::string{"PhotonCamera '"} + std::string{cameraName} +
                           "' is disconnected.",
-                      frc::Alert::AlertType::kWarning),
+                      wpi::Alert::AlertType::kWarning),
-      timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) {
+      timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::AlertType::kWarning) {
   verifyDependencies();
   InstanceCount++;
   HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, "");
 
   // The Robot class is actually created here:
-  // https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/frc/RobotBase.h#L33
+  // https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/wpi/opmode/RobotBase.hpp#L33
   // so we should be fine to call this from the ctor
   InitTspServer();
 }
 
 PhotonCamera::PhotonCamera(const std::string_view cameraName)
-    : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
+    : PhotonCamera(wpi::nt::NetworkTableInstance::GetDefault(), cameraName) {}
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
   if (test) {
@@ -190,8 +191,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   VerifyVersion();
 
   // Fill the packet with latest data and populate result.
-  units::microsecond_t now =
+  wpi::units::microsecond_t now =
-      units::microsecond_t(frc::RobotController::GetFPGATime());
+      wpi::units::microsecond_t(wpi::RobotController::GetFPGATime());
   const auto value = rawBytesEntry.Get();
   if (!value.size()) return PhotonPipelineResult{};
 
@@ -223,7 +224,7 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
   ret.reserve(changes.size());
 
   for (size_t i = 0; i < changes.size(); i++) {
-    const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
+    const wpi::nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
 
     if (!value.value.size() || value.time == 0) {
       continue;
@@ -237,7 +238,7 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
 
     // TODO: NT4 timestamps are still not to be trusted. But it's the best we
     // can do until we can make time sync more reliable.
-    result.SetReceiveTimestamp(units::microsecond_t(value.time) -
+    result.SetReceiveTimestamp(wpi::units::microsecond_t(value.time) -
                                result.GetLatency());
 
     ret.push_back(result);
@@ -261,11 +262,11 @@ void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) {
     timesyncAlert.SetText(warningText);
     timesyncAlert.Set(true);
 
-    if (frc::Timer::GetFPGATimestamp() <
+    if (wpi::Timer::GetFPGATimestamp() <
         (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
-      prevTimeSyncWarnTime = frc::Timer::GetFPGATimestamp();
+      prevTimeSyncWarnTime = wpi::Timer::GetFPGATimestamp();
 
-      FRC_ReportWarning(
+      WPILIB_ReportWarning(
           warningText +
           "\n\nCheck /photonvision/.timesync/{{COPROCESSOR_HOSTNAME}} for more "
           "information.");
@@ -317,7 +318,7 @@ const std::string_view PhotonCamera::GetCameraName() const {
 
 bool PhotonCamera::IsConnected() {
   auto currentHeartbeat = heartbeatSubscriber.Get();
-  auto now = frc::Timer::GetFPGATimestamp();
+  auto now = wpi::Timer::GetFPGATimestamp();
 
   if (currentHeartbeat < 0) {
     // we have never heard from the camera
@@ -365,10 +366,10 @@ void PhotonCamera::VerifyVersion() {
     return;
   }
 
-  if ((frc::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
+  if ((wpi::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
       VERSION_CHECK_INTERVAL)
     return;
-  this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp();
+  this->lastVersionCheckTime = wpi::Timer::GetFPGATimestamp();
 
   const std::string& versionString = versionEntry.Get("");
   if (versionString.empty()) {
@@ -376,13 +377,13 @@ void PhotonCamera::VerifyVersion() {
     std::vector<std::string> cameraNames =
         rootTable->GetInstance().GetTable("photonvision")->GetSubTables();
     if (cameraNames.empty()) {
-      FRC_ReportError(frc::warn::Warning,
+      WPILIB_ReportError(wpi::warn::Warning,
-                      "Could not find any PhotonVision coprocessors on "
+                         "Could not find any PhotonVision coprocessors on "
-                      "NetworkTables. Double check that PhotonVision is "
+                         "NetworkTables. Double check that PhotonVision is "
-                      "running, and that your camera is connected!");
+                         "running, and that your camera is connected!");
     } else {
-      FRC_ReportError(
+      WPILIB_ReportError(
-          frc::warn::Warning,
+          wpi::warn::Warning,
           "PhotonVision coprocessor at path {} not found on NetworkTables. "
           "Double check that your camera names match!",
           path_);
@@ -391,8 +392,8 @@ void PhotonCamera::VerifyVersion() {
       for (unsigned int i = 0; i < cameraNames.size(); i++) {
         cameraNameOutString += ("\n" + cameraNames[i]);
       }
-      FRC_ReportError(
+      WPILIB_ReportError(
-          frc::warn::Warning,
+          wpi::warn::Warning,
           "Found the following PhotonVision cameras on NetworkTables:\n{}",
           cameraNameOutString);
     }
@@ -400,12 +401,12 @@ void PhotonCamera::VerifyVersion() {
     std::string local_uuid{SerdeType<PhotonPipelineResult>::GetSchemaHash()};
 
     // implicit conversion here might throw an exception, so be careful of that
-    wpi::json remote_uuid_json =
+    wpi::util::json remote_uuid_json =
         rawBytesEntry.GetTopic().GetProperty("message_uuid");
     if (!remote_uuid_json.is_string()) {
-      FRC_ReportError(frc::warn::Warning,
+      WPILIB_ReportError(
-                      "Cannot find property message_uuid for PhotonCamera {}",
+          wpi::warn::Warning,
-                      path);
+          "Cannot find property message_uuid for PhotonCamera {}", path);
       return;
     }
     std::string remote_uuid{remote_uuid_json};
@@ -429,12 +430,12 @@ void PhotonCamera::VerifyVersion() {
           ">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
           ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
           "\n\n";
-      FRC_ReportWarning(bfw);
+      WPILIB_ReportWarning(bfw);
       std::string error_str = fmt::format(
           "Photonlib version {} (message definition version {}) does not match "
           "coprocessor version {} (message definition version {})!",
           PhotonVersion::versionString, local_uuid, versionString, remote_uuid);
-      FRC_ReportError(frc::err::Error, "{}", error_str);
+      WPILIB_ReportError(wpi::err::Error, "{}", error_str);
       throw std::runtime_error(error_str);
     }
   }

photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp

@@ -30,18 +30,18 @@
 #include <vector>
 
 #include <Eigen/Core>
-#include <frc/Errors.h>
-#include <frc/geometry/Pose3d.h>
-#include <frc/geometry/Rotation3d.h>
-#include <frc/geometry/Transform3d.h>
-#include <hal/FRCUsageReporting.h>
 #include <opencv2/calib3d.hpp>
 #include <opencv2/core/mat.hpp>
 #include <opencv2/core/types.hpp>
-#include <units/angle.h>
-#include <units/math.h>
-#include <units/time.h>
 #include <wpi/deprecated.h>
+#include <wpi/hal/UsageReporting.h>
+#include <wpi/math/geometry/Pose3d.hpp>
+#include <wpi/math/geometry/Rotation3d.hpp>
+#include <wpi/math/geometry/Transform3d.hpp>
+#include <wpi/system/Errors.hpp>
+#include <wpi/units/angle.hpp>
+#include <wpi/units/math.hpp>
+#include <wpi/units/time.hpp>
 
 #include "photon/PhotonCamera.h"
 #include "photon/estimation/TargetModel.h"
@@ -55,37 +55,41 @@ WPI_IGNORE_DEPRECATED
 namespace photon {
 
 namespace detail {
-cv::Point3d ToPoint3d(const frc::Translation3d& translation);
+cv::Point3d ToPoint3d(const wpi::math::Translation3d& translation);
 std::optional<std::array<cv::Point3d, 4>> CalcTagCorners(
-    int tagID, const frc::AprilTagFieldLayout& aprilTags);
+    int tagID, const wpi::apriltag::AprilTagFieldLayout& aprilTags);
-frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
+wpi::math::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
-cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
+cv::Point3d TagCornerToObjectPoint(wpi::units::meter_t cornerX,
-                                   units::meter_t cornerY, frc::Pose3d tagPose);
+                                   wpi::units::meter_t cornerY,
+                                   wpi::math::Pose3d tagPose);
 }  // namespace detail
 
-PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+PhotonPoseEstimator::PhotonPoseEstimator(
-                                         frc::Transform3d robotToCamera)
+    wpi::apriltag::AprilTagFieldLayout tags,
+    wpi::math::Transform3d robotToCamera)
     : aprilTags(tags),
       m_robotToCamera(robotToCamera),
-      lastPose(frc::Pose3d()),
+      lastPose(wpi::math::Pose3d()),
-      referencePose(frc::Pose3d()),
+      referencePose(wpi::math::Pose3d()),
       poseCacheTimestamp(-1_s),
-      headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
+      headingBuffer(
+          wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d>(1_s)) {
   HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
              InstanceCount);
   InstanceCount++;
 }
 
-PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+PhotonPoseEstimator::PhotonPoseEstimator(
-                                         PoseStrategy strat,
+    wpi::apriltag::AprilTagFieldLayout tags, PoseStrategy strat,
-                                         frc::Transform3d robotToCamera)
+    wpi::math::Transform3d robotToCamera)
     : aprilTags(tags),
       strategy(strat),
       m_robotToCamera(robotToCamera),
-      lastPose(frc::Pose3d()),
+      lastPose(wpi::math::Pose3d()),
-      referencePose(frc::Pose3d()),
+      referencePose(wpi::math::Pose3d()),
       poseCacheTimestamp(-1_s),
-      headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
+      headingBuffer(
+          wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d>(1_s)) {
   InstanceCount++;
   HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
 }
@@ -93,8 +97,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
 void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
   if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
       strategy == MULTI_TAG_PNP_ON_RIO) {
-    FRC_ReportError(
+    WPILIB_ReportError(
-        frc::warn::Warning,
+        wpi::warn::Warning,
         "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
         "");
     strategy = LOWEST_AMBIGUITY;
@@ -112,15 +116,16 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
     std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
   // Time in the past -- give up, since the following if expects times > 0
   if (result.GetTimestamp() < 0_s) {
-    FRC_ReportError(frc::warn::Warning,
+    WPILIB_ReportError(wpi::warn::Warning,
-                    "Result timestamp was reported in the past!");
+                       "Result timestamp was reported in the past!");
     return std::nullopt;
   }
 
   // If the pose cache timestamp was set, and the result is from the same
   // timestamp, return an empty result
   if (poseCacheTimestamp > 0_s &&
-      units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < 0.001_ms) {
+      wpi::units::math::abs(poseCacheTimestamp - result.GetTimestamp()) <
+          0.001_ms) {
     return std::nullopt;
   }
 
@@ -170,9 +175,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
       break;
     case MULTI_TAG_PNP_ON_RIO:
       if (!cameraMatrixData && !cameraDistCoeffs) {
-        FRC_ReportError(frc::warn::Warning,
+        WPILIB_ReportError(
-                        "No camera calibration provided to multi-tag-on-rio!",
+            wpi::warn::Warning,
-                        "");
+            "No camera calibration provided to multi-tag-on-rio!", "");
         ret = Update(result, this->multiTagFallbackStrategy);
       }
       ret =
@@ -185,8 +190,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
       using namespace frc;
 
       if (!cameraMatrixData || !cameraDistCoeffs) {
-        FRC_ReportError(
+        WPILib_ReportError(
-            frc::warn::Warning,
+            wpi::warn::Warning,
             "No camera calibration data provided for Constrained SolvePnP!");
         ret = Update(result, this->multiTagFallbackStrategy);
         break;
@@ -203,12 +208,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
         break;
       }
 
-      frc::Pose3d fieldToRobotSeed;
+      wpi::math::Pose3d fieldToRobotSeed;
 
       if (result.MultiTagResult().has_value()) {
         fieldToRobotSeed =
-            frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
+            wpi::math::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
-                             m_robotToCamera.Inverse());
+                                   m_robotToCamera.Inverse());
       } else {
         std::optional<EstimatedRobotPose> nestedUpdate =
             Update(result, cameraMatrixData, cameraDistCoeffs, {},
@@ -236,8 +241,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
       ret = EstimatePnpDistanceTrigSolvePose(result);
       break;
     default:
-      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
+      WPILIB_ReportError(wpi::warn::Warning, "Invalid Pose Strategy selected!",
-                      "");
+                         "");
       ret = std::nullopt;
   }
 
@@ -250,8 +255,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
 bool ShouldEstimate(const PhotonPipelineResult& result) {
   // Time in the past -- give up, since the following if expects times > 0
   if (result.GetTimestamp() < 0_s) {
-    FRC_ReportError(frc::warn::Warning,
+    WPILib_ReportError(wpi::warn::Warning,
-                    "Result timestamp was reported in the past!");
+                       "Result timestamp was reported in the past!");
     return false;
   }
 
@@ -281,12 +286,12 @@ PhotonPoseEstimator::EstimateLowestAmbiguityPose(
 
   auto& bestTarget = *foundIt;
 
-  std::optional<frc::Pose3d> fiducialPose =
+  std::optional<wpi::math::Pose3d> fiducialPose =
       aprilTags.GetTagPose(bestTarget.GetFiducialId());
   if (!fiducialPose) {
-    FRC_ReportError(frc::warn::Warning,
+    WPILIB_ReportError(wpi::warn::Warning,
-                    "Tried to get pose of unknown April Tag: {}",
+                       "Tried to get pose of unknown April Tag: {}",
-                    bestTarget.GetFiducialId());
+                       bestTarget.GetFiducialId());
     return std::nullopt;
   }
 
@@ -302,28 +307,28 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
   if (!ShouldEstimate(cameraResult)) {
     return std::nullopt;
   }
-  units::meter_t smallestHeightDifference =
+  wpi::units::meter_t smallestHeightDifference =
-      units::meter_t(std::numeric_limits<double>::infinity());
+      wpi::units::meter_t(std::numeric_limits<double>::infinity());
 
   std::optional<EstimatedRobotPose> pose = std::nullopt;
 
   for (auto& target : cameraResult.GetTargets()) {
-    std::optional<frc::Pose3d> fiducialPose =
+    std::optional<wpi::math::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
-      FRC_ReportError(frc::warn::Warning,
+      WPILIB_ReportError(wpi::warn::Warning,
-                      "Tried to get pose of unknown April Tag: {}",
+                         "Tried to get pose of unknown April Tag: {}",
-                      target.GetFiducialId());
+                         target.GetFiducialId());
       continue;
     }
-    frc::Pose3d const targetPose = *fiducialPose;
+    wpi::math::Pose3d const targetPose = *fiducialPose;
 
-    units::meter_t const alternativeDifference = units::math::abs(
+    wpi::units::meter_t const alternativeDifference = wpi::units::math::abs(
         m_robotToCamera.Z() -
         targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
             .Z());
 
-    units::meter_t const bestDifference = units::math::abs(
+    wpi::units::meter_t const bestDifference = wpi::units::math::abs(
         m_robotToCamera.Z() -
         targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
 
@@ -350,26 +355,26 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
 
 std::optional<EstimatedRobotPose>
 PhotonPoseEstimator::EstimateClosestToReferencePose(
-    PhotonPipelineResult cameraResult, frc::Pose3d referencePose) {
+    PhotonPipelineResult cameraResult, wpi::math::Pose3d referencePose) {
   if (!ShouldEstimate(cameraResult)) {
     return std::nullopt;
   }
-  units::meter_t smallestDifference =
+  wpi::units::meter_t smallestDifference =
-      units::meter_t(std::numeric_limits<double>::infinity());
+      wpi::units::meter_t(std::numeric_limits<double>::infinity());
-  units::second_t stateTimestamp = units::second_t(0);
+  wpi::units::second_t stateTimestamp = wpi::units::second_t(0);
-  frc::Pose3d pose = lastPose;
+  wpi::math::Pose3d pose = lastPose;
 
   auto targets = cameraResult.GetTargets();
   for (auto& target : targets) {
-    std::optional<frc::Pose3d> fiducialPose =
+    std::optional<wpi::math::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
-      FRC_ReportError(frc::warn::Warning,
+      WPILIB_ReportError(wpi::warn::Warning,
-                      "Tried to get pose of unknown April Tag: {}",
+                         "Tried to get pose of unknown April Tag: {}",
-                      target.GetFiducialId());
+                         target.GetFiducialId());
       continue;
     }
-    frc::Pose3d targetPose = fiducialPose.value();
+    wpi::math::Pose3d targetPose = fiducialPose.value();
 
     const auto altPose =
         targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
@@ -378,9 +383,9 @@ PhotonPoseEstimator::EstimateClosestToReferencePose(
         targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
             .TransformBy(m_robotToCamera.Inverse());
 
-    units::meter_t const alternativeDifference = units::math::abs(
+    wpi::units::meter_t const alternativeDifference = wpi::units::math::abs(
         referencePose.Translation().Distance(altPose.Translation()));
-    units::meter_t const bestDifference = units::math::abs(
+    wpi::units::meter_t const bestDifference = wpi::units::math::abs(
         referencePose.Translation().Distance(bestPose.Translation()));
     if (alternativeDifference < smallestDifference) {
       smallestDifference = alternativeDifference;
@@ -400,7 +405,7 @@ PhotonPoseEstimator::EstimateClosestToReferencePose(
 }
 
 std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
-    int tagID, const frc::AprilTagFieldLayout& aprilTags) {
+    int tagID, const wpi::apriltag::AprilTagFieldLayout& aprilTags) {
   if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) {
     return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose),
                       TagCornerToObjectPoint(+3_in, -3_in, *tagPose),
@@ -411,23 +416,23 @@ std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
   }
 }
 
-cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) {
+cv::Point3d detail::ToPoint3d(const wpi::math::Translation3d& translation) {
   return cv::Point3d(-translation.Y().value(), -translation.Z().value(),
                      +translation.X().value());
 }
 
-cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX,
+cv::Point3d detail::TagCornerToObjectPoint(wpi::units::meter_t cornerX,
-                                           units::meter_t cornerY,
+                                           wpi::units::meter_t cornerY,
-                                           frc::Pose3d tagPose) {
+                                           wpi::math::Pose3d tagPose) {
-  frc::Translation3d cornerTrans =
+  wpi::math::Translation3d cornerTrans =
-      tagPose.Translation() +
+      tagPose.Translation() + wpi::math::Translation3d(0.0_m, cornerX, cornerY)
-      frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation());
+                                  .RotateBy(tagPose.Rotation());
   return ToPoint3d(cornerTrans);
 }
 
-frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
+wpi::math::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
-  using namespace frc;
+  using namespace wpi::math;
-  using namespace units;
+  using namespace wpi::units;
 
   cv::Mat R;
   cv::Rodrigues(rvec, R);  // R is 3x3
@@ -458,7 +463,7 @@ PhotonPoseEstimator::EstimateCoprocMultiTagPose(
   const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best;
 
   const auto fieldToRobot =
-      frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
+      wpi::math::Pose3d() + field2camera + m_robotToCamera.Inverse();
   return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(),
                                     cameraResult.GetTargets(),
                                     MULTI_TAG_PNP_ON_COPROCESSOR);
@@ -512,7 +517,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::EstimateRioMultiTagPose(
                  tvec, false, cv::SOLVEPNP_SQPNP);
   }
 
-  const frc::Pose3d pose = detail::ToPose3d(tvec, rvec);
+  const wpi::math::Pose3d pose = detail::ToPose3d(tvec, rvec);
 
   return photon::EstimatedRobotPose(
       pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(),
@@ -526,47 +531,50 @@ PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose(
     return std::nullopt;
   }
   PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget();
-  std::optional<frc::Rotation2d> headingSampleOpt =
+  std::optional<wpi::math::Rotation2d> headingSampleOpt =
       headingBuffer.Sample(cameraResult.GetTimestamp());
   if (!headingSampleOpt) {
-    FRC_ReportError(frc::warn::Warning,
+    WPILIB_ReportError(
-                    "There was no heading data! Use AddHeadingData to add it!");
+        wpi::warn::Warning,
+        "There was no heading data! Use AddHeadingData to add it!");
     return std::nullopt;
   }
 
-  frc::Rotation2d headingSample = headingSampleOpt.value();
+  wpi::math::Rotation2d headingSample = headingSampleOpt.value();
 
-  frc::Translation2d camToTagTranslation =
+  wpi::math::Translation2d camToTagTranslation =
-      frc::Translation3d(
+      wpi::math::Translation3d(
           bestTarget.GetBestCameraToTarget().Translation().Norm(),
-          frc::Rotation3d(0_rad, -units::degree_t(bestTarget.GetPitch()),
+          wpi::math::Rotation3d(0_rad,
-                          -units::degree_t(bestTarget.GetYaw())))
+                                -wpi::units::degree_t(bestTarget.GetPitch()),
+                                -wpi::units::degree_t(bestTarget.GetYaw())))
           .RotateBy(m_robotToCamera.Rotation())
           .ToTranslation2d()
           .RotateBy(headingSample);
 
-  std::optional<frc::Pose3d> fiducialPose =
+  std::optional<wpi::math::Pose3d> fiducialPose =
       aprilTags.GetTagPose(bestTarget.GetFiducialId());
   if (!fiducialPose) {
-    FRC_ReportError(frc::warn::Warning,
+    WPILIB_ReportError(wpi::warn::Warning,
-                    "Tried to get pose of unknown April Tag: {}",
+                       "Tried to get pose of unknown April Tag: {}",
-                    bestTarget.GetFiducialId());
+                       bestTarget.GetFiducialId());
     return std::nullopt;
   }
 
-  frc::Pose2d tagPose = fiducialPose.value().ToPose2d();
+  wpi::math::Pose2d tagPose = fiducialPose.value().ToPose2d();
 
-  frc::Translation2d fieldToCameraTranslation =
+  wpi::math::Translation2d fieldToCameraTranslation =
       tagPose.Translation() - camToTagTranslation;
 
-  frc::Translation2d camToRobotTranslation =
+  wpi::math::Translation2d camToRobotTranslation =
       (-m_robotToCamera.Translation().ToTranslation2d())
           .RotateBy(headingSample);
 
-  frc::Pose2d robotPose = frc::Pose2d(
+  wpi::math::Pose2d robotPose = wpi::math::Pose2d(
       fieldToCameraTranslation + camToRobotTranslation, headingSample);
 
-  return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(),
+  return EstimatedRobotPose{wpi::math::Pose3d(robotPose),
+                            cameraResult.GetTimestamp(),
                             cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
 }
 
@@ -576,22 +584,23 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose(
   if (!ShouldEstimate(cameraResult)) {
     return std::nullopt;
   }
-  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
+  std::vector<
+      std::pair<wpi::math::Pose3d, std::pair<double, wpi::units::second_t>>>
       tempPoses;
   double totalAmbiguity = 0;
 
   auto targets = cameraResult.GetTargets();
   for (auto& target : targets) {
-    std::optional<frc::Pose3d> fiducialPose =
+    std::optional<wpi::math::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
-      FRC_ReportError(frc::warn::Warning,
+      WPILIB_ReportError(wpi::warn::Warning,
-                      "Tried to get pose of unknown April Tag: {}",
+                         "Tried to get pose of unknown April Tag: {}",
-                      target.GetFiducialId());
+                         target.GetFiducialId());
       continue;
     }
 
-    frc::Pose3d targetPose = fiducialPose.value();
+    wpi::math::Pose3d targetPose = fiducialPose.value();
     // Ambiguity = 0, use that pose
     if (target.GetPoseAmbiguity() == 0) {
       return EstimatedRobotPose{
@@ -608,17 +617,17 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose(
                        cameraResult.GetTimestamp())));
   }
 
-  frc::Translation3d transform = frc::Translation3d();
+  wpi::math::Translation3d transform = wpi::math::Translation3d();
-  frc::Rotation3d rotation = frc::Rotation3d();
+  wpi::math::Rotation3d rotation = wpi::math::Rotation3d();
 
-  for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
+  for (std::pair<wpi::math::Pose3d, std::pair<double, wpi::units::second_t>>&
-       tempPoses) {
+           pair : tempPoses) {
     double const weight = (1. / pair.second.first) / totalAmbiguity;
     transform = transform + pair.first.Translation() * weight;
     rotation = rotation + pair.first.Rotation() * weight;
   }
 
-  return EstimatedRobotPose{frc::Pose3d(transform, rotation),
+  return EstimatedRobotPose{wpi::math::Pose3d(transform, rotation),
                             cameraResult.GetTimestamp(),
                             cameraResult.GetTargets(), AVERAGE_BEST_TARGETS};
 }
@@ -627,8 +636,8 @@ std::optional<EstimatedRobotPose>
 PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
     photon::PhotonPipelineResult cameraResult,
     photon::PhotonCamera::CameraMatrix cameraMatrix,
-    photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
+    photon::PhotonCamera::DistortionMatrix distCoeffs,
-    bool headingFree, double headingScaleFactor) {
+    wpi::math::Pose3d seedPose, bool headingFree, double headingScaleFactor) {
   if (!ShouldEstimate(cameraResult)) {
     return std::nullopt;
   }
@@ -638,9 +647,9 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
       return std::nullopt;
     } else {
       // If heading fixed, force rotation component
-      seedPose = frc::Pose3d{
+      seedPose = wpi::math::Pose3d{
           seedPose.Translation(),
-          frc::Rotation3d{
+          wpi::math::Rotation3d{
               headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
     }
   }
@@ -651,7 +660,8 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
       VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
           cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
           aprilTags, photon::kAprilTag36h11, headingFree,
-          frc::Rotation2d{
+          wpi::math::Rotation2d{
+
               headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
           headingScaleFactor);
 
@@ -659,7 +669,7 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
     return std::nullopt;
   }
 
-  frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
+  wpi::math::Pose3d best = wpi::math::Pose3d{} + pnpResult->best;
 
   return EstimatedRobotPose{best, cameraResult.GetTimestamp(),
                             cameraResult.GetTargets(),

photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp

@@ -29,8 +29,8 @@
 #include <utility>
 #include <vector>
 
-#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <wpi/apriltag/AprilTagFieldLayout.hpp>
-#include <frc/apriltag/AprilTagFields.h>
+#include <wpi/apriltag/AprilTagFields.hpp>
 
 #include "photon/estimation/CameraTargetRelation.h"
 #include "photon/estimation/RotTrlTransform3d.h"
@@ -40,19 +40,19 @@
 namespace photon {
 PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera)
     : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG(),
-                      frc::AprilTagFieldLayout::LoadField(
+                      wpi::apriltag::AprilTagFieldLayout::LoadField(
-                          frc::AprilTagField::kDefaultField)) {}
+                          wpi::apriltag::AprilTagField::kDefaultField)) {}
 
-PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
+PhotonCameraSim::PhotonCameraSim(
-                                 const SimCameraProperties& props,
+    PhotonCamera* camera, const SimCameraProperties& props,
-                                 const frc::AprilTagFieldLayout& tagLayout)
+    const wpi::apriltag::AprilTagFieldLayout& tagLayout)
     : prop{props}, cam{camera}, tagLayout{tagLayout} {
   SetMinTargetAreaPixels(kDefaultMinAreaPx);
   videoSimRaw =
-      frc::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
+      wpi::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
                                   prop.GetResWidth(), prop.GetResHeight());
-  videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray);
+  videoSimRaw.SetPixelFormat(wpi::cs::VideoMode::PixelFormat::kGray);
-  videoSimProcessed = frc::CameraServer::PutVideo(
+  videoSimProcessed = wpi::CameraServer::PutVideo(
       std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
       prop.GetResHeight());
   ts.subTable = cam->GetCameraTable();
@@ -62,21 +62,21 @@ PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
 PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
                                  const SimCameraProperties& props,
                                  double minTargetAreaPercent,
-                                 units::meter_t maxSightRange)
+                                 wpi::units::meter_t maxSightRange)
     : PhotonCameraSim(camera, props) {
   this->minTargetAreaPercent = minTargetAreaPercent;
   this->maxSightRange = maxSightRange;
 }
 
-bool PhotonCameraSim::CanSeeTargetPose(const frc::Pose3d& camPose,
+bool PhotonCameraSim::CanSeeTargetPose(const wpi::math::Pose3d& camPose,
                                        const VisionTargetSim& target) {
   CameraTargetRelation rel{camPose, target.GetPose()};
-  return ((units::math::abs(rel.camToTargYaw.Degrees()) <
+  return ((wpi::units::math::abs(rel.camToTargYaw.Degrees()) <
            prop.GetHorizFOV().Degrees() / 2.0) &&
-          (units::math::abs(rel.camToTargPitch.Degrees()) <
+          (wpi::units::math::abs(rel.camToTargPitch.Degrees()) <
            prop.GetVertFOV().Degrees() / 2.0) &&
           (!target.GetModel().GetIsPlanar() ||
-           units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
+           wpi::units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
           (rel.camToTarg.Translation().Norm() <= maxSightRange));
 }
 bool PhotonCameraSim::CanSeeCorner(const std::vector<cv::Point2f>& points) {
@@ -89,13 +89,13 @@ bool PhotonCameraSim::CanSeeCorner(const std::vector<cv::Point2f>& points) {
   return true;
 }
 std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
-  uint64_t now = wpi::Now();
+  uint64_t now = wpi::util::Now();
   uint64_t timestamp{};
   int iter = 0;
   while (now >= nextNTEntryTime) {
     timestamp = nextNTEntryTime;
     uint64_t frameTime = prop.EstSecUntilNextFrame()
-                             .convert<units::microseconds>()
+                             .convert<wpi::units::microseconds>()
                              .to<uint64_t>();
     nextNTEntryTime += frameTime;
 
@@ -113,13 +113,13 @@ std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
   }
 }
 PhotonPipelineResult PhotonCameraSim::Process(
-    units::second_t latency, const frc::Pose3d& cameraPose,
+    wpi::units::second_t latency, const wpi::math::Pose3d& cameraPose,
     std::vector<VisionTargetSim> targets) {
   std::sort(targets.begin(), targets.end(),
             [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) {
-              units::meter_t dist1 =
+              wpi::units::meter_t dist1 =
                   t1.GetPose().Translation().Distance(cameraPose.Translation());
-              units::meter_t dist2 =
+              wpi::units::meter_t dist2 =
                   t2.GetPose().Translation().Distance(cameraPose.Translation());
               return dist1 > dist2;
             });
@@ -144,7 +144,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
       continue;
     }
 
-    std::vector<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
+    std::vector<wpi::math::Translation3d> fieldCorners = tgt.GetFieldVertices();
     if (tgt.GetModel().GetIsSpherical()) {
       TargetModel model = tgt.GetModel();
       fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
@@ -186,11 +186,12 @@ PhotonPipelineResult PhotonCameraSim::Process(
           r = i;
         }
       }
-      cv::RotatedRect rect{
+      cv::RotatedRect rect{cv::Point2d{center.x, center.y},
-          cv::Point2d{center.x, center.y},
+                           cv::Size2d{imagePoints[r].x - lc.x,
-          cv::Size2d{imagePoints[r].x - lc.x,
+                                      imagePoints[b].y - imagePoints[t].y},
-                     imagePoints[b].y - imagePoints[t].y},
+                           wpi::units::radian_t{-angles[r]}
-          units::radian_t{-angles[r]}.convert<units::degrees>().to<float>()};
+                               .convert<wpi::units::degrees>()
+                               .to<float>()};
       std::vector<cv::Point2f> points{};
       rect.points(points);
 
@@ -210,7 +211,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
     minAreaRectPts.reserve(4);
     minAreaRect.points(minAreaRectPts);
     cv::Point2d centerPt = minAreaRect.center;
-    frc::Rotation3d centerRot = prop.GetPixelRot(centerPt);
+    wpi::math::Rotation3d centerRot = prop.GetPixelRot(centerPt);
     double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners);
 
     if (!(CanSeeCorner(noisyTargetCorners) &&
@@ -250,12 +251,12 @@ PhotonPipelineResult PhotonCameraSim::Process(
     std::vector<TargetCorner> cornersDouble{cornersFloat.begin(),
                                             cornersFloat.end()};
     detectableTgts.emplace_back(
-        -centerRot.Z().convert<units::degrees>().to<double>(),
+        -centerRot.Z().convert<wpi::units::degrees>().to<double>(),
-        -centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
+        -centerRot.Y().convert<wpi::units::degrees>().to<double>(), areaPercent,
-        centerRot.X().convert<units::degrees>().to<double>(),
+        centerRot.X().convert<wpi::units::degrees>().to<double>(),
         tgt.GetFiducialId(), classId, conf,
-        pnpSim ? pnpSim->best : frc::Transform3d{},
+        pnpSim ? pnpSim->best : wpi::math::Transform3d{},
-        pnpSim ? pnpSim->alt : frc::Transform3d{},
+        pnpSim ? pnpSim->alt : wpi::math::Transform3d{},
         pnpSim ? pnpSim->ambiguity : -1, smallVec, cornersDouble);
   }
 
@@ -288,7 +289,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
     videoSimRaw.PutFrame(videoSimFrameRaw);
   } else {
     videoSimRaw.SetConnectionStrategy(
-        cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
+        wpi::cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
   }
 
   if (videoSimProcEnabled) {
@@ -333,19 +334,19 @@ PhotonPipelineResult PhotonCameraSim::Process(
     videoSimProcessed.PutFrame(videoSimFrameProcessed);
   } else {
     videoSimProcessed.SetConnectionStrategy(
-        cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
+        wpi::cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
   }
 
   std::optional<MultiTargetPNPResult> multiTagResults = std::nullopt;
 
-  std::vector<frc::AprilTag> visibleLayoutTags =
+  std::vector<wpi::apriltag::AprilTag> visibleLayoutTags =
       VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
   if (visibleLayoutTags.size() > 1) {
     std::vector<int16_t> usedIds{};
     usedIds.resize(visibleLayoutTags.size());
     std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
                    usedIds.begin(),
-                   [](const frc::AprilTag& tag) { return tag.ID; });
+                   [](const wpi::apriltag::AprilTag& tag) { return tag.ID; });
     std::sort(usedIds.begin(), usedIds.end());
     auto pnpResult = VisionEstimation::EstimateCamPosePNP(
         prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
@@ -357,17 +358,17 @@ PhotonPipelineResult PhotonCameraSim::Process(
 
   return PhotonPipelineResult{
       PhotonPipelineMetadata{heartbeatCounter, 0,
-                             units::microsecond_t{latency}.to<int64_t>(),
+                             wpi::units::microsecond_t{latency}.to<int64_t>(),
                              1000000},
       detectableTgts, multiTagResults};
 }
 void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
-  SubmitProcessedFrame(result, wpi::Now());
+  SubmitProcessedFrame(result, wpi::util::Now());
 }
 void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
                                            uint64_t ReceiveTimestamp) {
   ts.latencyMillisEntry.Set(
-      result.GetLatency().convert<units::milliseconds>().to<double>(),
+      result.GetLatency().convert<wpi::units::milliseconds>().to<double>(),
       ReceiveTimestamp);
 
   Packet newPacket{};
@@ -381,7 +382,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
     ts.targetPitchEntry.Set(0.0, ReceiveTimestamp);
     ts.targetYawEntry.Set(0.0, ReceiveTimestamp);
     ts.targetAreaEntry.Set(0.0, ReceiveTimestamp);
-    ts.targetPoseEntry.Set(frc::Transform3d{}, ReceiveTimestamp);
+    ts.targetPoseEntry.Set(wpi::math::Transform3d{}, ReceiveTimestamp);
     ts.targetSkewEntry.Set(0.0, ReceiveTimestamp);
   } else {
     PhotonTrackedTarget bestTarget = result.GetBestTarget();