April Tag changes

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

lib/publish.gradle

@@ -4,7 +4,7 @@ apply plugin: 'maven-publish'
 
 ext.licenseFile = files("$rootDir/LICENSE.txt")
 
-def pubVersion = '2023.2.1'
+def pubVersion = '2023.3.1'
 
 def outputsFolder = file("$buildDir/allOutputs")
 

lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java

@@ -11,6 +11,7 @@ import org.photonvision.EstimatedRobotPose;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
 import edu.wpi.first.math.geometry.Transform3d;
 
 public class RobotFieldPosition {
@@ -44,6 +45,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 +58,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<EstimatedRobotPose> getEstimatedGlobalPose() {
         return frozen ? frozenPose : photonPoseEstimator.update();
     }

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<AprilTagsDetectorPipeline> 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<AprilTagsDetectorPipeline>(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<Pose3d> 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<AprilTagsDetectorPipeline> {
+        @Override
+        public void copyPipelineOutputs(AprilTagsDetectorPipeline pipeline) {
+            estimatedPose = pipeline.getEstimatedPose();
+        }
+    }
+
+}