From ac81af87296e01d17a2f02825e0140d5c153a238 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Tue, 28 Feb 2023 11:02:33 -0500 Subject: [PATCH 01/14] April tag changes --- lib/publish.gradle | 2 +- .../CameraTracking/RobotFieldPosition.java | 44 ++++++++++++++----- 2 files changed, 34 insertions(+), 12 deletions(-) diff --git a/lib/publish.gradle b/lib/publish.gradle index bd49a07..94f7526 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.0' +def pubVersion = '2023.1.1-SNAPSHOT' 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 2c62ef5..7ce0ffc 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 @@ -9,30 +9,52 @@ 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 { +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. - */ + * 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, String layoutResourceFile) throws IOException { - layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); - + PhotonCamera cam = new PhotonCamera(camera); photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); - } + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile, PoseStrategy strategy) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields, PoseStrategy strategy) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } public Optional getEstimatedGlobalPose() { return photonPoseEstimator.update(); } - -} \ No newline at end of file +} \ No newline at end of file From da4f21ffbb5adf65c0bb03c3c664ccff4a9f3fcb Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 2 Mar 2023 18:06:11 -0500 Subject: [PATCH 02/14] more constructors for field position --- .../CameraTracking/RobotFieldPosition.java | 1 - vendordeps/AdvantageKit.json | 41 +++++++++++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) create mode 100644 vendordeps/AdvantageKit.json 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 7ce0ffc..5f09614 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,6 @@ 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 { diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..b5c210c --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,41 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "2.2.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "2.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "2.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "2.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "2.2.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file From 16196bc980e524c2dd3744ae26e83169a17aa1ac Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 2 Mar 2023 22:46:26 -0500 Subject: [PATCH 03/14] Some minor vision changes and auto package for main builds --- .github/workflows/gradle-publish.yml | 6 ++++-- lib/publish.gradle | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/.github/workflows/gradle-publish.yml b/.github/workflows/gradle-publish.yml index 8ec3ee3..3feef94 100644 --- a/.github/workflows/gradle-publish.yml +++ b/.github/workflows/gradle-publish.yml @@ -10,6 +10,8 @@ name: Gradle Package on: release: types: [created] + push: + branches: ["main"] jobs: build: @@ -22,10 +24,10 @@ jobs: steps: - uses: actions/checkout@v3 - - name: Set up JDK 11 + - name: Set up JDK 17 uses: actions/setup-java@v3 with: - java-version: '11' + java-version: '17' distribution: 'temurin' server-id: github # Value of the distributionManagement/repository/id field of the pom.xml settings-path: ${{ github.workspace }} # location for the settings.xml file diff --git a/lib/publish.gradle b/lib/publish.gradle index 94f7526..58b7f22 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.1-SNAPSHOT' +def pubVersion = '2023.1.2' def outputsFolder = file("$buildDir/allOutputs") From 42c00e2cdd00af02aac404927c10356f2ff3687f Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 2 Mar 2023 22:48:21 -0500 Subject: [PATCH 04/14] Remove vendordeps --- vendordeps/AdvantageKit.json | 41 ------------------------------------ 1 file changed, 41 deletions(-) delete mode 100644 vendordeps/AdvantageKit.json diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json deleted file mode 100644 index b5c210c..0000000 --- a/vendordeps/AdvantageKit.json +++ /dev/null @@ -1,41 +0,0 @@ -{ - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "2.2.1", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "mavenUrls": [], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "wpilib-shim", - "version": "2.2.1" - }, - { - "groupId": "org.littletonrobotics.akit.junction", - "artifactId": "junction-core", - "version": "2.2.1" - }, - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-api", - "version": "2.2.1" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit.conduit", - "artifactId": "conduit-wpilibio", - "version": "2.2.1", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [] -} \ No newline at end of file From 198b4641eb5f4c2c20c8d96567280128faf595de Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Tue, 7 Mar 2023 19:17:30 -0500 Subject: [PATCH 05/14] Fix class visibility for color sensor --- lib/publish.gradle | 2 +- .../fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/publish.gradle b/lib/publish.gradle index 58b7f22..8e3572a 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.2' +def pubVersion = '2023.1.3' def outputsFolder = file("$buildDir/allOutputs") diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java b/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java index 0f76a61..15b34d8 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/sensors/colorsensors/TCS34725.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.I2C; public class TCS34725 { /** */ - class TCS34725_RGB { + public class TCS34725_RGB { private int r, g, b; public TCS34725_RGB(int r, int g, int b) { From ad268932b66bc73c14aef0ad7b4fe121a5501691 Mon Sep 17 00:00:00 2001 From: theduckbutler <109929126+theduckbutler@users.noreply.github.com> Date: Tue, 7 Mar 2023 20:04:47 -0500 Subject: [PATCH 06/14] Oops, can now do individual LEDs --- .../frc/poe/controllers/lighting/ArduinoLightingController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java b/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java index d42bfcc..bc3b121 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/controllers/lighting/ArduinoLightingController.java @@ -23,7 +23,7 @@ public class ArduinoLightingController { Matcher m = colorPattern.matcher(colorValue); return m.find(); } - private int executeColor(String colorValue, String command) { + public int executeColor(String colorValue, String command) { if ( validateColor(colorValue) ) { return bus.writeString( colorValue + command ); } From 91ffc202ebcde109059cc4640bd6a532042bb34d Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Tue, 7 Mar 2023 20:36:36 -0500 Subject: [PATCH 07/14] Bump version number --- lib/publish.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/publish.gradle b/lib/publish.gradle index 8e3572a..f435e7b 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.3' +def pubVersion = '2023.1.4' def outputsFolder = file("$buildDir/allOutputs") From b51d40782265705b77d4862c22d5ae904b34db30 Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Thu, 9 Mar 2023 16:27:57 -0800 Subject: [PATCH 08/14] Fix Repeated PoseStrategy and Added Some Extra Stuff --- .../CameraTracking/RobotFieldPosition.java | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) 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 5f09614..d64f680 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,11 +10,14 @@ 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; + Optional frozenPose; + boolean frozen = false; /* * Example of Transform3d: @@ -35,7 +38,7 @@ public class RobotFieldPosition { layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); PhotonCamera cam = new PhotonCamera(camera); - photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); } public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException { @@ -49,11 +52,26 @@ public class RobotFieldPosition { layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); PhotonCamera cam = new PhotonCamera(camera); - photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); } public Optional getEstimatedGlobalPose() { - return photonPoseEstimator.update(); + return frozen ? frozenPose : photonPoseEstimator.update(); } + public void setLastPose(Pose2d pose) { + photonPoseEstimator.setLastPose(pose); + } + + // For when you want it to stay the same across lines + public void freeze() { + frozen = true; + frozenPose = photonPoseEstimator.update(); + } + + public void unfreeze() { + frozen = false; + } + + } \ No newline at end of file From 5ae94879f7963bac057f064291649478b2d348e2 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 12 Mar 2023 13:42:26 -0400 Subject: [PATCH 09/14] Artifact version --- lib/publish.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/publish.gradle b/lib/publish.gradle index bd49a07..ae17b72 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.0' +def pubVersion = '2023.2.0' def outputsFolder = file("$buildDir/allOutputs") From 708534f9400546141fe347495ab9c768e54b24ba Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Tue, 14 Mar 2023 16:12:30 -0700 Subject: [PATCH 10/14] Bump Version --- lib/publish.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/publish.gradle b/lib/publish.gradle index 8e3572a..f435e7b 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.3' +def pubVersion = '2023.1.4' def outputsFolder = file("$buildDir/allOutputs") From cfa1d8a2b7ebcc1f021dca9c017b75288667aaec Mon Sep 17 00:00:00 2001 From: Declipsonator <96392121+Declipsonator@users.noreply.github.com> Date: Tue, 14 Mar 2023 21:59:29 -0400 Subject: [PATCH 11/14] Fix Bump --- lib/publish.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/publish.gradle b/lib/publish.gradle index f435e7b..2a8f4cd 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.4' +def pubVersion = '2023.2.1' def outputsFolder = file("$buildDir/allOutputs") From 399f1cc19235e9273901955fde999c2ffa0afb6c Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 23 Mar 2023 18:06:15 -0400 Subject: [PATCH 12/14] 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(); + } + } + +} From c8af3ee673e6094f42026111787a9fe4582c04fd Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 23 Mar 2023 18:06:15 -0400 Subject: [PATCH 13/14] April Tag changes --- lib/build.gradle | 19 +++- lib/publish.gradle | 2 +- .../CameraTracking/RobotFieldPosition.java | 10 ++ .../WPILIB/WPI_AprilTagPoseEstimator.java | 92 +++++++++++++++++++ 4 files changed, 121 insertions(+), 2 deletions(-) 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..30ea7f9 100644 --- a/lib/publish.gradle +++ b/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") 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..2625ce5 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 @@ -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 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(); + } + } + +} From 3385dfcd614055b02acb96893272b4b163425a3c Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 25 Mar 2023 17:51:22 -0400 Subject: [PATCH 14/14] Changes some constructors --- lib/publish.gradle | 2 +- .../frc/poe/CameraTracking/RobotFieldPosition.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/publish.gradle b/lib/publish.gradle index 30ea7f9..76eaee3 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.3.1' +def pubVersion = '2023.3.2' 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 a4726ba..45a8566 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 @@ -63,7 +63,7 @@ public class RobotFieldPosition { this.layout = layout; PhotonCamera cam = new PhotonCamera(camera); - photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); } public Optional getEstimatedGlobalPose() {