diff --git a/.gitignore b/.gitignore index e519109..b7e94a2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,6 @@ .gradle -build/ - -lib/bin +/build/ +/lib/bin # Ignore Gradle GUI config gradle-app.setting @@ -17,3 +16,4 @@ gradle-app.setting # Ignore Gradle build output directory build + diff --git a/lib/build.gradle b/lib/build.gradle index 409832f..95ce996 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -17,15 +17,10 @@ repositories { mavenCentral() mavenLocal() gradlePluginPortal() - 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 "https://maven.photonvision.org/repository/snapshots"} + maven {url "https://maven.photonvision.org/repository/internal"} + maven {url "https://maven.ctr-electronics.com/release/"} } dependencies { @@ -34,14 +29,15 @@ dependencies { // This dependency is used internally, and not exposed to consumers on their own compile classpath. implementation 'com.google.guava:guava:30.1.1-jre' - implementation 'edu.wpi.first.cscore:cscore-java:2022.+' - implementation 'edu.wpi.first.cameraserver:cameraserver-java:2022.+' - implementation 'edu.wpi.first.ntcore:ntcore-java:2022.+' - implementation 'edu.wpi.first.wpilibj:wpilibj-java:2022.+' - implementation 'edu.wpi.first.wpiutil:wpiutil-java:2022.+' - implementation 'edu.wpi.first.wpimath:wpimath-java:2022.+' - implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2022.+' - implementation 'edu.wpi.first.hal:hal-java:2022.+' + implementation 'edu.wpi.first.cscore:cscore-java:2023.2.1' + implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.2.1' + implementation 'edu.wpi.first.ntcore:ntcore-java:2023.2.1' + implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.2.1' + implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.2.1' + implementation 'edu.wpi.first.wpimath:wpimath-java:2023.2.1' + implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.2.1' + implementation 'edu.wpi.first.hal:hal-java:2023.2.1' + implementation "edu.wpi.first.apriltag:apriltag-java:2023.2.1" implementation "org.ejml:ejml-simple:0.38" implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0" implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java new file mode 100644 index 0000000..c05d7ff --- /dev/null +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java @@ -0,0 +1,78 @@ +package com.fairportrobotics.frc.poe.CameraTracking; + +import java.util.ArrayList; +import java.util.List; + +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class AprilTags { + // an ArrayList to store the cameras + public final ArrayList cameras = new ArrayList<>(); + + public AprilTags(String...cameraNames) { + for(String cameraName : cameraNames) { + cameras.add(new PhotonCamera(cameraName)); + } + } + + // Method to get a list of all targets from all cameras + public List getTargets() { + List targets = new ArrayList<>(); + + for(PhotonCamera camera : cameras) { + for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { + boolean toAdd = true; + // check if the target is already in the list and if it has a higher pose ambiguity + for(PhotonTrackedTarget arTarget : targets) { + if(target.getFiducialId() == arTarget.getFiducialId() && target.getPoseAmbiguity() < arTarget.getPoseAmbiguity()) { + // if it does, remove it from the list + targets.remove(arTarget); + break; + } else if(target.getFiducialId() == arTarget.getFiducialId()) { + // if the target is already in the list and has the same ID, don't add it + toAdd = false; + break; + } + } + + if(toAdd) targets.add(target); + } + targets.addAll(camera.getLatestResult().getTargets()); + } + + return targets; + } + + // method to check if any camera has any targets + public boolean hasTargets() { + for(PhotonCamera camera : cameras) { + // if a camera has targets, return true + if(camera.getLatestResult().hasTargets()) return true; + } + + return false; + } + + // method to get the closest target + public PhotonTrackedTarget getClosestTarget() { + if(!hasTargets()) return null; + PhotonTrackedTarget closestTarget = null; + for(PhotonTrackedTarget target : getTargets()) { + if(closestTarget == null || Math.abs(target.getBestCameraToTarget().getY()) > Math.abs(target.getBestCameraToTarget().getY()) ) closestTarget = target; + } + return closestTarget; + } + + // method to get the target with the lowest pose ambiguity + public PhotonTrackedTarget getLowestAmbiguityTarget() { + if(!hasTargets()) return null; + PhotonTrackedTarget lowestAmbiguity = null; + for(PhotonTrackedTarget target : getTargets()) { + // check if the target has a lower pose ambiguity than the current lowest ambiguity target + if(lowestAmbiguity == null || lowestAmbiguity.getPoseAmbiguity() > target.getPoseAmbiguity()) lowestAmbiguity = target; + } + return lowestAmbiguity; + } + +} \ No newline at end of file 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 new file mode 100644 index 0000000..81fe7e2 --- /dev/null +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java @@ -0,0 +1,42 @@ +package com.fairportrobotics.frc.poe.CameraTracking; + +import java.io.IOException; +import java.util.Optional; + +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +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.math.geometry.Transform3d; + +public class RobotFieldPosition { + AprilTagFieldLayout layout; + PhotonPoseEstimator photonPoseEstimator; + + /* + Example of Transform3d: + + Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); + Cam mounted facing forward, half a meter forward of center, half a meter up from center. + */ + public RobotFieldPosition(String camera, Transform3d robotToCam) { + try { + layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile); + } catch(IOException exception) { + exception.printStackTrace(); + } + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + + public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { + photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + return photonPoseEstimator.update(); + } + + +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..1112658 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.2.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "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": "v2023.2.1", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.2.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.2.1" + } + ] +} \ No newline at end of file