From 7be7a80aac54a7842b5a110157a14d3e12bd337f Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Fri, 12 Jan 2024 22:19:17 -0500 Subject: [PATCH] Adding changes to interfaces AND preliminary PhotonVision interfacing class --- .../robot/interfaces/IAprilTagProvider.java | 5 +- .../robot/interfaces/IVisualPoseProvider.java | 4 +- .../java/frc/robot/subsystems/Drivetrain.java | 10 +- .../frc/robot/utilities/PhotonVision.java | 135 ++++++++++++++++++ vendordeps/photonlib.json | 57 ++++++++ 5 files changed, 203 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/utilities/PhotonVision.java create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java index 09d61dd..3546b53 100644 --- a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java +++ b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java @@ -2,9 +2,8 @@ package frc.robot.interfaces; import java.util.OptionalDouble; -public interface IAprilTagProvider { - public OptionalDouble getTagHorizontalOffsetByID(int id); - public OptionalDouble getTagDistanceFromCameraByID(int id); +public interface IAprilTagProvider { + public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters); public OptionalDouble getTagPitchByID(int id); public OptionalDouble getTagYawByID(int id); } diff --git a/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java b/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java index ff147e6..a645c79 100644 --- a/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java +++ b/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java @@ -1,9 +1,11 @@ package frc.robot.interfaces; +import java.util.Optional; + import edu.wpi.first.math.geometry.Pose2d; public interface IVisualPoseProvider { public record VisualPose(Pose2d visualPose, double timestamp) {} - public VisualPose getVisualPose(); + public Optional getVisualPose(); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 0809087..491c856 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -25,6 +25,7 @@ import frc.robot.constants.DrivetrainConstants; import frc.robot.utilities.MAXSwerveModule; import frc.robot.utilities.SwerveUtils; import frc.robot.interfaces.IVisualPoseProvider; +import frc.robot.interfaces.IVisualPoseProvider.VisualPose; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PIDCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -134,10 +135,11 @@ public class Drivetrain extends SubsystemBase { ); if (m_visualPoseProvider != null) { - m_poseEstimator.addVisionMeasurement( - m_visualPoseProvider.getVisualPose().visualPose(), - m_visualPoseProvider.getVisualPose().timestamp() - ); + Optional vPose = m_visualPoseProvider.getVisualPose(); + + if (vPose.isPresent()) { + m_poseEstimator.addVisionMeasurement(vPose.get().visualPose(), vPose.get().timestamp()); + } } } diff --git a/src/main/java/frc/robot/utilities/PhotonVision.java b/src/main/java/frc/robot/utilities/PhotonVision.java new file mode 100644 index 0000000..591e219 --- /dev/null +++ b/src/main/java/frc/robot/utilities/PhotonVision.java @@ -0,0 +1,135 @@ +package frc.robot.utilities; + +import java.io.IOException; +import java.util.List; +import java.util.Optional; +import java.util.OptionalDouble; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonUtils; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; +import frc.robot.interfaces.IAprilTagProvider; +import frc.robot.interfaces.IVisualPoseProvider; + +public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider { + + private final PhotonCamera camera; + + private final PhotonPoseEstimator photonPoseEstimator; + + private final double cameraHeightMeters; + private final double cameraPitchRadians; + + public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException { + camera = new PhotonCamera(cameraName); + + photonPoseEstimator = new PhotonPoseEstimator( + AprilTagFieldLayout.loadFromResource( + AprilTagFields.k2024Crescendo.m_resourceFile + ), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + robotToCam + ); + + this.cameraHeightMeters = cameraHeightMeters; + this.cameraPitchRadians = cameraPitchRadians; + } + + @Override + public Optional getVisualPose() { + Optional pose = photonPoseEstimator.update(); + + if (pose.isEmpty()) { + return Optional.empty(); + } + + return Optional.of( + new VisualPose( + pose.get().estimatedPose.toPose2d(), + pose.get().timestampSeconds + ) + ); + } + + @Override + public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) { + PhotonPipelineResult result = camera.getLatestResult(); + + if (!result.hasTargets()) { + return OptionalDouble.empty(); + } + + Optional desiredTarget = getTargetFromList(result.getTargets(), id); + + if (desiredTarget.isEmpty()) { + return OptionalDouble.empty(); + } + + return OptionalDouble.of( + PhotonUtils.calculateDistanceToTargetMeters( + cameraHeightMeters, + targetHeightMeters, + cameraPitchRadians, + Units.degreesToRadians(desiredTarget.get().getPitch())) + ); + } + + @Override + public OptionalDouble getTagPitchByID(int id) { + PhotonPipelineResult result = camera.getLatestResult(); + + if (!result.hasTargets()) { + return OptionalDouble.empty(); + } + + Optional desiredTarget = getTargetFromList(result.getTargets(), id); + + if (desiredTarget.isEmpty()) { + return OptionalDouble.empty(); + } + + return OptionalDouble.of( + desiredTarget.get().getPitch() + ); + } + + @Override + public OptionalDouble getTagYawByID(int id) { + PhotonPipelineResult result = camera.getLatestResult(); + + if (!result.hasTargets()) { + return OptionalDouble.empty(); + } + + Optional desiredTarget = getTargetFromList(result.getTargets(), id); + + if (desiredTarget.isEmpty()) { + return OptionalDouble.empty(); + } + + return OptionalDouble.of( + desiredTarget.get().getYaw() + ); + } + + private Optional getTargetFromList(List targets, int id) { + for (PhotonTrackedTarget target : targets) { + if (target.getFiducialId() == id) { + return Optional.of(target); + } + } + + return Optional.empty(); + } + +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..f658f52 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.1.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.1.2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.1.2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.1.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.1.2" + } + ] +} \ No newline at end of file