Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to match new WPILib organization

Browse Source
This commit is contained in:
Gold856
2025-12-29 16:16:56 -05:00
committed by samfreund
parent c34c854583
commit 934eed21d2
264 changed files with 3440 additions and 3299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
build.gradle
View File

@@ -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 "edu.wpi.first.GradleRIO2027" version "2027.0.0-alpha-2"
id "org.wpilib.WPILibRepositoriesPlugin" version "2027.0.0"
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"

2
docs/source/docs/apriltag-pipelines/multitag.md
View File

@@ -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;
}
}

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

@@ -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

10
docs/source/docs/programming/photonlib/getting-target-data.md
View File

@@ -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::SmallVector<std::pair<double, double>, 4> corners = target.GetCorners();
wpi::math::Transform2d pose = target.GetCameraToTarget();
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();
frc::Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget();
wpi::math::Transform3d bestCameraToTarget = target.getBestCameraToTarget();
wpi::math::Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget();
.. code-block:: python

8
docs/source/docs/programming/photonlib/using-target-data.md
View File

@@ -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(
kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, frc::Rotation2d(units::degree_t(-target.GetYaw())), frc::Rotation2d(units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot);
wpi::math::Pose2d robotPose = photonlib::EstimateFieldToRobot(
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(
distance, frc::Rotation2d(units::degree_t(-target.GetYaw())));
wpi::math::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslation(
distance, wpi::math::Rotation2d(wpi::units::degree_t(-target.GetYaw())));
.. code-block:: python

2
docs/source/docs/quick-start/networking.md
View File

@@ -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

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

@@ -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);

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

@@ -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 {

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

@@ -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;

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

@@ -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).

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

@@ -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;

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

@@ -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(

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

@@ -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.

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

@@ -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 =

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

@@ -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

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

@@ -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(

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

@@ -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;

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

@@ -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);

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

@@ -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;

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

@@ -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> {

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

@@ -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 {

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

@@ -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 {

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

@@ -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);

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

@@ -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);

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

@@ -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")

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

@@ -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() {}

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

@@ -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.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.util.ColorHelper;
import org.wpilib.math.geometry.Pose3d;
// Ignore the previous calibration data that was stored in the json file.
@JsonIgnoreProperties(ignoreUnknown = true)

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

@@ -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 {

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

@@ -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;

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

@@ -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)

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

@@ -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.

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

@@ -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) {

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

@@ -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) {

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

@@ -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) {

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

@@ -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 "

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

@@ -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) {

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

@@ -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);

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

@@ -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);

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

@@ -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) {

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

@@ -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.

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

@@ -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;

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

@@ -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");
}

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

@@ -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);

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

@@ -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 {

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

@@ -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);

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

@@ -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;

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

@@ -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(

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

@@ -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);

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

@@ -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.

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

@@ -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<

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

@@ -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<

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

@@ -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<

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

@@ -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> {

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

@@ -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<

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

@@ -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> {

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

@@ -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> {

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

@@ -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<

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

@@ -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<

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

@@ -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> {

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

@@ -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

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

@@ -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> {

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

@@ -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);

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

@@ -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);

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

@@ -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> {

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

@@ -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;

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

@@ -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> {

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

@@ -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> {

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

@@ -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

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

@@ -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:
* the latency is relative to the time at which this method is called. Waiting to call this method
* will change the latency this method returns.
* Get the latency between now (wpi::util::Now) and the time at which the image was captured.
* FOOTGUN: the latency is relative to the time at which this method is called. Waiting to call
* this method will change the latency this method returns.
*/
@Deprecated
public double getLatencyMillis() {

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

@@ -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

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

@@ -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 {

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

@@ -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

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

@@ -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;

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

@@ -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

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

@@ -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;

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

@@ -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;

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

@@ -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

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

@@ -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;

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

@@ -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

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

@@ -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

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

@@ -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

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

@@ -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";

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

@@ -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. */

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

@@ -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

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

@@ -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

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

@@ -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);

4
photon-lib/build.gradle
View File

@@ -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")

2
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View File

@@ -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 {

40
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -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 {

34
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -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

4
photon-lib/src/main/java/org/photonvision/PhotonUtils.java
View File

@@ -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

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

@@ -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
|| MathUtil.clamp(point.y, 0, prop.getResHeight()) != point.y) {
if (Math.clamp(point.x, 0, prop.getResWidth()) != point.x
|| Math.clamp(point.y, 0, prop.getResHeight()) != point.y) {
return false; // point is outside of resolution
}
}

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

@@ -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);
}

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

@@ -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

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

@@ -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

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

@@ -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 {

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

@@ -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 {

73
photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View File

@@ -29,20 +29,21 @@
#include <string_view>
#include <vector>
#include <frc/Errors.h>
#include <frc/RobotController.h>
#include <frc/Timer.h>
#include <hal/FRCUsageReporting.h>
#include <wpi/hal/UsageReporting.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 units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
static constexpr wpi::units::second_t WARN_DEBOUNCE_SEC = 5_s;
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);
FRC_ReportError(frc::err::Error, bfw);
WPILIB_ReportWarning(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),
timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) {
wpi::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 =
units::microsecond_t(frc::RobotController::GetFPGATime());
wpi::units::microsecond_t now =
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 "
"NetworkTables. Double check that PhotonVision is "
"running, and that your camera is connected!");
} else {
FRC_ReportError(
frc::warn::Warning,
WPILIB_ReportError(
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(
frc::warn::Warning,
WPILIB_ReportError(
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,
"Cannot find property message_uuid for PhotonCamera {}",
path);
WPILIB_ReportError(
wpi::warn::Warning,
"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);
}
}

206
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -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);
frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
int tagID, const wpi::apriltag::AprilTagFieldLayout& aprilTags);
wpi::math::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
cv::Point3d TagCornerToObjectPoint(wpi::units::meter_t cornerX,
wpi::units::meter_t cornerY,
wpi::math::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
frc::Transform3d robotToCamera)
PhotonPoseEstimator::PhotonPoseEstimator(
wpi::apriltag::AprilTagFieldLayout tags,
wpi::math::Transform3d robotToCamera)
: aprilTags(tags),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
lastPose(wpi::math::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,
PoseStrategy strat,
frc::Transform3d robotToCamera)
PhotonPoseEstimator::PhotonPoseEstimator(
wpi::apriltag::AprilTagFieldLayout tags, PoseStrategy strat,
wpi::math::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
lastPose(wpi::math::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(
frc::warn::Warning,
WPILIB_ReportError(
wpi::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
"");
strategy = LOWEST_AMBIGUITY;
@@ -112,7 +116,7 @@ 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!");
return std::nullopt;
}
@@ -120,7 +124,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
// 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,
"No camera calibration provided to multi-tag-on-rio!",
"");
WPILIB_ReportError(
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(
frc::warn::Warning,
WPILib_ReportError(
wpi::warn::Warning,
"No camera calibration data provided for Constrained SolvePnP!");
ret = Update(result, this->multiTagFallbackStrategy);
break;
@@ -203,11 +208,11 @@ 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());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
@@ -236,7 +241,7 @@ 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,7 +255,7 @@ 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!");
return false;
}
@@ -281,10 +286,10 @@ 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: {}",
bestTarget.GetFiducialId());
return std::nullopt;
@@ -302,28 +307,28 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
wpi::units::meter_t smallestHeightDifference =
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: {}",
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 =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
wpi::units::meter_t smallestDifference =
wpi::units::meter_t(std::numeric_limits<double>::infinity());
wpi::units::second_t stateTimestamp = wpi::units::second_t(0);
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: {}",
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,
units::meter_t cornerY,
frc::Pose3d tagPose) {
frc::Translation3d cornerTrans =
tagPose.Translation() +
frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation());
cv::Point3d detail::TagCornerToObjectPoint(wpi::units::meter_t cornerX,
wpi::units::meter_t cornerY,
wpi::math::Pose3d tagPose) {
wpi::math::Translation3d cornerTrans =
tagPose.Translation() + wpi::math::Translation3d(0.0_m, cornerX, cornerY)
.RotateBy(tagPose.Rotation());
return ToPoint3d(cornerTrans);
}
frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
using namespace frc;
using namespace units;
wpi::math::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
using namespace wpi::math;
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(
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 =
frc::Translation3d(
wpi::math::Translation2d camToTagTranslation =
wpi::math::Translation3d(
bestTarget.GetBestCameraToTarget().Translation().Norm(),
frc::Rotation3d(0_rad, -units::degree_t(bestTarget.GetPitch()),
-units::degree_t(bestTarget.GetYaw())))
wpi::math::Rotation3d(0_rad,
-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: {}",
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: {}",
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();
frc::Rotation3d rotation = frc::Rotation3d();
wpi::math::Translation3d transform = wpi::math::Translation3d();
wpi::math::Rotation3d rotation = wpi::math::Rotation3d();
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
for (std::pair<wpi::math::Pose3d, std::pair<double, wpi::units::second_t>>&
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,
bool headingFree, double headingScaleFactor) {
photon::PhotonCamera::DistortionMatrix distCoeffs,
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(),

77
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View File

@@ -29,8 +29,8 @@
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#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(
frc::AprilTagField::kDefaultField)) {}
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::kDefaultField)) {}
PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
const SimCameraProperties& props,
const frc::AprilTagFieldLayout& tagLayout)
PhotonCameraSim::PhotonCameraSim(
PhotonCamera* camera, const SimCameraProperties& props,
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);
videoSimProcessed = frc::CameraServer::PutVideo(
videoSimRaw.SetPixelFormat(wpi::cs::VideoMode::PixelFormat::kGray);
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::Point2d{center.x, center.y},
cv::RotatedRect rect{cv::Point2d{center.x, center.y},
cv::Size2d{imagePoints[r].x - lc.x,
imagePoints[b].y - imagePoints[t].y},
units::radian_t{-angles[r]}.convert<units::degrees>().to<float>()};
wpi::units::radian_t{-angles[r]}
.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.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(),
-centerRot.Z().convert<wpi::units::degrees>().to<double>(),
-centerRot.Y().convert<wpi::units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<wpi::units::degrees>().to<double>(),
tgt.GetFiducialId(), classId, conf,
pnpSim ? pnpSim->best : frc::Transform3d{},
pnpSim ? pnpSim->alt : frc::Transform3d{},
pnpSim ? pnpSim->best : wpi::math::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();

Some files were not shown because too many files have changed in this diff Show More