From 399f1cc19235e9273901955fde999c2ffa0afb6c Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 23 Mar 2023 18:06:15 -0400 Subject: [PATCH] April Tag changes --- lib/build.gradle | 19 +++- lib/publish.gradle | 4 + .../CameraTracking/RobotFieldPosition.java | 13 +++ .../WPILIB/WPI_AprilTagPoseEstimator.java | 92 +++++++++++++++++++ 4 files changed, 127 insertions(+), 1 deletion(-) create mode 100644 lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java diff --git a/lib/build.gradle b/lib/build.gradle index 689664a..5b2288f 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -9,7 +9,7 @@ plugins { // Apply the java-library plugin for API and implementation separation. id 'java' - id 'edu.wpi.first.GradleRIO' version '2023.2.1' + id 'edu.wpi.first.GradleRIO' version '2023.+' } repositories { @@ -21,6 +21,14 @@ repositories { maven {url "https://maven.photonvision.org/repository/snapshots"} maven {url "https://maven.photonvision.org/repository/internal"} maven {url "https://maven.ctr-electronics.com/release/"} + + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } } dependencies { @@ -43,10 +51,19 @@ dependencies { implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" implementation "com.fasterxml.jackson.core:jackson-databind:2.10.0" implementation 'edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:4.5.2-1' + + // CTRE Phoenix implementation 'com.ctre.phoenix:api-java:5.30.3' implementation 'com.ctre.phoenix:wpiapi-java:5.30.3' + + // PhotonVision implementation "org.photonvision:PhotonLib-java:v2023.+" implementation "org.photonvision:PhotonTargeting-java:v2023.+" + + // AdvantageKit + implementation "org.littletonrobotics.akit.junction:wpilib-shim:2.2.1" + implementation "org.littletonrobotics.akit.junction:junction-core:2.2.1" + implementation "org.littletonrobotics.akit.conduit:conduit-api:2.2.1" } tasks.named('test') { diff --git a/lib/publish.gradle b/lib/publish.gradle index 2a8f4cd..e64616e 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,11 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") +<<<<<<< Updated upstream def pubVersion = '2023.2.1' +======= +def pubVersion = '2023.3.0' +>>>>>>> Stashed changes def outputsFolder = file("$buildDir/allOutputs") diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java index d64f680..a54535f 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java @@ -10,7 +10,11 @@ import org.photonvision.EstimatedRobotPose; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +<<<<<<< Updated upstream import edu.wpi.first.math.geometry.Pose2d; +======= +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +>>>>>>> Stashed changes import edu.wpi.first.math.geometry.Transform3d; public class RobotFieldPosition { @@ -44,6 +48,8 @@ public class RobotFieldPosition { public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException { layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); + layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + PhotonCamera cam = new PhotonCamera(camera); photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); } @@ -55,6 +61,13 @@ public class RobotFieldPosition { photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); } + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFieldLayout layout, PoseStrategy strategy) throws IOException { + this.layout = layout; + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + public Optional getEstimatedGlobalPose() { return frozen ? frozenPose : photonPoseEstimator.update(); } diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java new file mode 100644 index 0000000..c275981 --- /dev/null +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/WPILIB/WPI_AprilTagPoseEstimator.java @@ -0,0 +1,92 @@ +package com.fairportrobotics.frc.poe.CameraTracking.WPILIB; + +import java.io.IOException; +import java.util.Optional; + +import org.opencv.core.Mat; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import edu.wpi.first.cscore.VideoCamera; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.vision.VisionPipeline; +import edu.wpi.first.vision.VisionRunner; + +public class WPI_AprilTagPoseEstimator { + + private final AprilTagDetector aprilTagDetector; + private final AprilTagPoseEstimator aprilTagPoseEstimator; + private AprilTagFieldLayout aprilTagFieldLayout; + private Pose3d estimatedPose = new Pose3d(); + + private VisionRunner visionRunner; + + public WPI_AprilTagPoseEstimator(VideoCamera imageSource) { + aprilTagDetector = new AprilTagDetector(); + aprilTagDetector.addFamily("tag16h5"); + AprilTagPoseEstimator.Config poseEstimatorConfig = new Config(0.203, 0, 0, 0, 0); + aprilTagPoseEstimator = new AprilTagPoseEstimator(poseEstimatorConfig); + + try { + aprilTagFieldLayout = new AprilTagFieldLayout(AprilTagFields.kDefaultField.m_resourceFile); + } catch (IOException e) { + e.printStackTrace(); + } + + visionRunner = new VisionRunner(imageSource, new AprilTagsDetectorPipeline(), new VisionSystemCallback()); + } + + public void runDetectorPipeline(){ + new Thread(() -> { + try{ + visionRunner.runForever(); + } catch (Exception e){ + e.printStackTrace(); + } + }).start(); + } + + public Pose3d getEstimatedGlobalPose() { + return estimatedPose; + } + + private final class AprilTagsDetectorPipeline implements VisionPipeline { + + private Pose3d estPose = new Pose3d(); + + @Override + public void process(Mat image) { + AprilTagDetection[] aprilTags = aprilTagDetector.detect(image); + + for (AprilTagDetection aprilTag : aprilTags) { + Transform3d tagRelativeToRobot = aprilTagPoseEstimator.estimate(aprilTag); + Optional tagRelativeToFieldOptional = aprilTagFieldLayout.getTagPose(aprilTag.getId()); + + if (tagRelativeToFieldOptional.isPresent()) { + Pose3d tagRelativeToFieldPose = tagRelativeToFieldOptional.get(); + Pose3d estimatedFieldPos = tagRelativeToFieldPose.plus(tagRelativeToRobot); + // Logger.getInstance().recordOutput("April Tag Pose", tagRelativeToFieldPose); + estPose = estimatedFieldPos; + } + } + } + + public Pose3d getEstimatedPose(){ + return estPose; + } + + } + + private final class VisionSystemCallback implements VisionRunner.Listener { + @Override + public void copyPipelineOutputs(AprilTagsDetectorPipeline pipeline) { + estimatedPose = pipeline.getEstimatedPose(); + } + } + +}