From 128446e159f8617f0a92a91bb61cbb418e9d545a Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 21 Jan 2023 12:02:25 -0500 Subject: [PATCH 01/11] Added deploy to local --- .gitignore | 4 ++- README.md | 5 +++- .../SwerveDrive/SwerveDriveSubsystem.class | Bin 425 -> 0 bytes .../fairportrobotics/frc/poe/Library.class | Bin 388 -> 0 bytes .../frc/poe/LibraryTest.class | Bin 781 -> 0 bytes lib/publish.gradle | 27 +++++++++++++++++- 6 files changed, 33 insertions(+), 3 deletions(-) delete mode 100644 lib/bin/main/com/fairportrobotics/frc/poe/DriveSystems/SwerveDrive/SwerveDriveSubsystem.class delete mode 100644 lib/bin/main/com/fairportrobotics/frc/poe/Library.class delete mode 100644 lib/bin/test/com/fairportrobotics/frc/poe/LibraryTest.class diff --git a/.gitignore b/.gitignore index 821a4e0..e519109 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,7 @@ .gradle -/build/ +build/ + +lib/bin # Ignore Gradle GUI config gradle-app.setting diff --git a/README.md b/README.md index 5356c5e..d8d0d21 100644 --- a/README.md +++ b/README.md @@ -5,5 +5,8 @@ The official library for Team 578 R3 - Red Raider Robotics To include this library in a Java FRC Robot project. # Building +The project can be built and published to the local Maven repository using `./gradlew pubLocal` -The project can be build and published to the local Maven repository using `./gradlew publishToMavenLocal` + +## Updating version +The version tag can be found in `lib/publish.gradle`. When updating the version match the year to the current year, and increment the major and minor version as needed. diff --git a/lib/bin/main/com/fairportrobotics/frc/poe/DriveSystems/SwerveDrive/SwerveDriveSubsystem.class b/lib/bin/main/com/fairportrobotics/frc/poe/DriveSystems/SwerveDrive/SwerveDriveSubsystem.class deleted file mode 100644 index 47dec288b38da8c6902782cd156c69bcc4d4eb96..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 425 zcmb_Yu}Z{16r9cFE?zu6v9q*O5f`+uP_9t1kYE$Jev@qQ9Z5FqCOP~pE5X7K@T0_+ zAjf@x&CI-EmiK1Ae?GqeT;m{sC-9V-Qs#l7e3xMkZCoQd#QfKhtks$q7W!|FyXh2xHUQR6kO` z_h_sBro_ccbqdGe`}t47Ec&|{1D>vi85o~^VCS6;4SkI%Pv04EqHhzPsNR$?ZNTUzIxEvz@H5i_U6(rS^L!b$g(ASMhK z@==OX)(_&QSZL)5y)#oAe@^J^A4~}Gg`MgY9c*OiqDL6yrqoF6a1S_$24`0!LS+7%OSO;!0vu^|I7Egq2445HS22{2YkiWdBbx`biGguF)4- Ca!wln diff --git a/lib/bin/test/com/fairportrobotics/frc/poe/LibraryTest.class b/lib/bin/test/com/fairportrobotics/frc/poe/LibraryTest.class deleted file mode 100644 index e0216bfb997c8fab3f86ca2b8a18765e48518468..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 781 zcma)4O>Yx15PeQVH`#_X2_;Ynbb*S7R>DUPK}Z!6r=>`3h(vGRjhi^y*pb(Z#Lw!b zMIiA5_)&=QCQ^~gAqUTRKK$m*jDP?6@e9Bk><6eY^iwnE7ed*EvCf*rIF;u7!lry- zB#%{Mh5a^^xeHKbXwAe|!84&J{8KWMsbkoDtF&_O7&iK^Mhw;WW-LQ&px(e8Y%*-e zO3RaCo=7_sNhTROu}MWX5>|P8<*d4?${7yhb@>b!dbyd)6~&LzP0jdJy29#wXbY;d zds=9x=5nNRMTL&FHcmKYG}YQj{BtB~mn+6@8nZl;>gYxTZ{qrea`$C97P12tN zDs*V;WCwKPq?z{|UZZwdhC`A*4Aikjvaw7ceU~htNzn?n>E^Rqq&?6H`8;&zC)!`G saW6i)!cOPmcRad5ZI%?aP$R51p?UP~^3@8xhLET}BV2~(6s_RJUke( Date: Sat, 28 Jan 2023 14:00:58 -0500 Subject: [PATCH 02/11] Create gradle.yml --- .github/workflows/gradle.yml | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 .github/workflows/gradle.yml diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml new file mode 100644 index 0000000..f5e6591 --- /dev/null +++ b/.github/workflows/gradle.yml @@ -0,0 +1,34 @@ +# This workflow uses actions that are not certified by GitHub. +# They are provided by a third-party and are governed by +# separate terms of service, privacy policy, and support +# documentation. +# This workflow will build a Java project with Gradle and cache/restore any dependencies to improve the workflow execution time +# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-java-with-gradle + +name: Java CI with Gradle + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +permissions: + contents: read + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v3 + - name: Set up JDK 17 + uses: actions/setup-java@v3 + with: + java-version: '17' + distribution: 'temurin' + - name: Build with Gradle + uses: gradle/gradle-build-action@67421db6bd0bf253fb4bd25b31ebb98943c375e1 + with: + arguments: build From 3b3fdfc85d789c734b16e818121d9a1b63a6d237 Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Sat, 21 Jan 2023 07:59:40 -0800 Subject: [PATCH 03/11] Add AprilTags and RobotFieldPosition --- .gitignore | 6 +- lib/build.gradle | 30 ++++--- .../frc/poe/CameraTracking/AprilTags.java | 78 +++++++++++++++++++ .../CameraTracking/RobotFieldPosition.java | 42 ++++++++++ vendordeps/photonlib.json | 41 ++++++++++ 5 files changed, 177 insertions(+), 20 deletions(-) create mode 100644 lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java create mode 100644 lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java create mode 100644 vendordeps/photonlib.json 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 From 57d3ef677003c0aa14c3e4c016725db78bef32a6 Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Sat, 21 Jan 2023 08:02:33 -0800 Subject: [PATCH 04/11] oops --- .gitignore | 1 - .../frc/poe/CameraTracking/AprilTags.class | Bin 0 -> 3246 bytes .../poe/CameraTracking/RobotFieldPosition.class | Bin 0 -> 2269 bytes 3 files changed, 1 deletion(-) create mode 100644 lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.class create mode 100644 lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.class diff --git a/.gitignore b/.gitignore index b7e94a2..6e77973 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,5 @@ .gradle /build/ -/lib/bin # Ignore Gradle GUI config gradle-app.setting diff --git a/lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.class b/lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.class new file mode 100644 index 0000000000000000000000000000000000000000..f3b46d5bf431b6b3afec8086f045b1c608283e15 GIT binary patch literal 3246 zcmbVN-E$LF6#v~O>Gos$L1`(W6e^0eO<|#h0_hi}A6Q9CrL|b_YnyJVD>IC~sQBWHug>_S|AzHY#3#zMZI*rlc_crib49^ zrJ@15dAUA2yGhSOtgGdsKvl#{8e^HnjA2jeGjRe_6S1Otd`h=Xxz7sy&b&!rk4K)V zVnnUnE6x~8L_gaLASY0^t|P-(uW%Myfr<%-Y#7y3ZdeNT(I{p(le*27eruXrHk376 z^}#K|0%dkw7m{aQ!Cl%Jvz}+xT_PG%(T0NpC8k4SI+iUtu<^o?th}on8jdJ9%!t-2 zo;y|1jt;UUX~YjYkpaGAH&;+Tp~9A|}?T10n@v~%7_XW}&Jpve}3Y))X@Oc`<3 z)-$5Vlju?q7SR5usfq(=E@#h%6p;0m>j_DG6`T?{^hB3=aF(GDr&SDKkQHTi!{%%z zYU=S3@|3MIBF~3aoWV=%bDob|X`??eW5zP3v*@Y}%6MK@aaIz%*ftW@f*~;3E?NZ9mlXDA+`WL5Q6P8EAp z71+DA5_uDYmwA2!4sTW6T(mrx*fjyi>X+Rs*r_EtJ5=(VtjllF#Z<0MPj8C!8Hmn~ zB6!|SX~~)y6>m#SWhDmUEJZroZvyR4L&W@?$~fp|Y8*O_|!9X{Q}6p*!*-aFCHxbcRguHxIT*(nj-f*hvo&>namiqiHUKs(8Mf@J?76 zJ8BG@a@ba`y=sSKU*S0-wT!L4^TCI zAKSh~O=vnCc-ZOp^)8_y7-(!CZT$u{4^TVZb{{*I5xnrePc+u~@2vj)6|H?}CmF*Yo;SG>g|U}k@)u^5`xr+Ps_{HtV08N_-GQQ2J~({8gn}0# z+!giIHt>jyav7~-OZf8O3JwYUfZEZvWgK0`i4}AUgab>s z9t>p9`oeyHXK~0oDOtiuuw?B-3Hq1OA5^$}hDNy=lAEEMpP(ufR8}xjgbPb(+Nf6Q z>nvVEh4c~;xV`#yUGXibp`d>S?Ygx;2L4PPUt=$gdY;Vk9c;I5PeSFduYK!+WpLDMgrgK7UoeF_!9hx zS20CE8*v90@ESo4lk?NK$h&*kNSE+BEhZ^_gE9qg(uYcY&+;vP$wK{tl2vSn-z`)R z)ihvLlE4!vsFk9uIEd1XVg-FXkfd~t+6$g~O44af!SkGTDU?FKX;)D=nEL#nnGXjO zSJ@En@n1GhTb4?rO9-~*s1ymgRMKxD;J-_$TeZJ@cd75SKn)QDj<9YWB=-@t^68Zo zAgQI#Yz69wro`RDahoK3Ieg5)%czbYQOt6c0sTRj(&^dOWf->+5cfocpK~s0N%T;T z=zXK1pU^;J#g=j9JEYdXKK>Mewn&a;zq>5k*s@)0qHbQ+LoW5=6#qxKuD9&jY~1CP zE|3v|dV5^HcrAMZ{?b)8x<`e+TwV6(1nY(5&f-4C3lof!4%nYk~|z4y6y{`&imR{-we2MZB~g|ezSJ6!6z z(t%dnDv)L0+0kXEu7tD9YeMs)=H;i-t2%jezbb`W*;Kv^r1C7p7Dza5NJZ5K8A#B?>beXY9i444#nvC zJ4)APE9r?6Lu^@9L;_KqwGqcThG=r46h{v(SUArx+DTNtjf=R%(EW7g=FCi<*JULy zxJl3WF)XKN#ZMcDeKs!R3d6bDV=a8u&}FeIUBPhSWQYlTf~yujWk_`bu$gun1Gq+2 zbQQ0xYE{c0d0|*QC5sasK|d;J91PcOe1xt9hB0E{Gb(Xd7^$30Zsn)4s2dvD7&STW zCQf)kAt1UL&It?S3b{Lzn4u(QbB|UE#m7tQ_cLn`GsP{2*^bEDVl17&ByO9iK4^kmJK=FdCdOT=o zGQ>aF(EQ1Syn`CP8U{pqAtd&?jSb`|AHE3kVn=A@y-C0On-c5BaZ^ry-}}F z3sQcPZ9?p;Yq56{4o!t)opoy7h){)pP2I~UAHf@DqA}PR&l7st<-RY7(zXfd?RdZt zwMLZQ-3a|?9MTHYC$vWBCXNKHAJe}-=`=>)JE=q1sb}bZj^5wV{{n+Q71v`vP;nk(n6I;}VVAD83*=*T^8~S%~2(oe!XwPWo}3 z?uRf(>z8zUn;?-(+(nwcW;c&3WXbIAVSym_lKxj%B%8yuU&4LbEz^A;BCp9=)B>~c zwS`sl9gdOi17t|=8 Date: Sat, 21 Jan 2023 08:04:42 -0800 Subject: [PATCH 05/11] remove unneeded stuff --- vendordeps/photonlib.json | 41 --------------------------------------- 1 file changed, 41 deletions(-) delete mode 100644 vendordeps/photonlib.json diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 1112658..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,41 +0,0 @@ -{ - "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 From a2b1ecba8a7b38f68add004c47e2b2557b91cb70 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sat, 21 Jan 2023 12:02:25 -0500 Subject: [PATCH 06/11] Added deploy to local --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 6e77973..4b2be17 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,7 @@ .gradle -/build/ +build/ + +lib/bin # Ignore Gradle GUI config gradle-app.setting From a1970666117cdccbd99b0695cc730a564d112eed Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Sat, 21 Jan 2023 07:59:40 -0800 Subject: [PATCH 07/11] Add AprilTags and RobotFieldPosition --- .gitignore | 5 ++--- vendordeps/photonlib.json | 41 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 3 deletions(-) create mode 100644 vendordeps/photonlib.json diff --git a/.gitignore b/.gitignore index 4b2be17..b7e94a2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,6 @@ .gradle -build/ - -lib/bin +/build/ +/lib/bin # Ignore Gradle GUI config gradle-app.setting 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 From 4237078930330acbeb59cdc005f4aed0fb6e5958 Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Sat, 21 Jan 2023 11:23:07 -0800 Subject: [PATCH 08/11] oops --- vendordeps/photonlib.json | 41 --------------------------------------- 1 file changed, 41 deletions(-) delete mode 100644 vendordeps/photonlib.json diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 1112658..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,41 +0,0 @@ -{ - "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 From b39cc9242d3a0c72576d05512773a4efba9a6ae9 Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Sat, 21 Jan 2023 11:31:20 -0800 Subject: [PATCH 09/11] quick fix --- .../frc/poe/CameraTracking/AprilTags.class | Bin 3246 -> 0 bytes .../poe/CameraTracking/RobotFieldPosition.class | Bin 2269 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.class delete mode 100644 lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.class diff --git a/lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.class b/lib/bin/main/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.class deleted file mode 100644 index f3b46d5bf431b6b3afec8086f045b1c608283e15..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3246 zcmbVN-E$LF6#v~O>Gos$L1`(W6e^0eO<|#h0_hi}A6Q9CrL|b_YnyJVD>IC~sQBWHug>_S|AzHY#3#zMZI*rlc_crib49^ zrJ@15dAUA2yGhSOtgGdsKvl#{8e^HnjA2jeGjRe_6S1Otd`h=Xxz7sy&b&!rk4K)V zVnnUnE6x~8L_gaLASY0^t|P-(uW%Myfr<%-Y#7y3ZdeNT(I{p(le*27eruXrHk376 z^}#K|0%dkw7m{aQ!Cl%Jvz}+xT_PG%(T0NpC8k4SI+iUtu<^o?th}on8jdJ9%!t-2 zo;y|1jt;UUX~YjYkpaGAH&;+Tp~9A|}?T10n@v~%7_XW}&Jpve}3Y))X@Oc`<3 z)-$5Vlju?q7SR5usfq(=E@#h%6p;0m>j_DG6`T?{^hB3=aF(GDr&SDKkQHTi!{%%z zYU=S3@|3MIBF~3aoWV=%bDob|X`??eW5zP3v*@Y}%6MK@aaIz%*ftW@f*~;3E?NZ9mlXDA+`WL5Q6P8EAp z71+DA5_uDYmwA2!4sTW6T(mrx*fjyi>X+Rs*r_EtJ5=(VtjllF#Z<0MPj8C!8Hmn~ zB6!|SX~~)y6>m#SWhDmUEJZroZvyR4L&W@?$~fp|Y8*O_|!9X{Q}6p*!*-aFCHxbcRguHxIT*(nj-f*hvo&>namiqiHUKs(8Mf@J?76 zJ8BG@a@ba`y=sSKU*S0-wT!L4^TCI zAKSh~O=vnCc-ZOp^)8_y7-(!CZT$u{4^TVZb{{*I5xnrePc+u~@2vj)6|H?}CmF*Yo;SG>g|U}k@)u^5`xr+Ps_{HtV08N_-GQQ2J~({8gn}0# z+!giIHt>jyav7~-OZf8O3JwYUfZEZvWgK0`i4}AUgab>s z9t>p9`oeyHXK~0oDOtiuuw?B-3Hq1OA5^$}hDNy=lAEEMpP(ufR8}xjgbPb(+Nf6Q z>nvVEh4c~;xV`#yUGXibp`d>S?Ygx;2L4PPUt=$gdY;Vk9c;I5PeSFduYK!+WpLDMgrgK7UoeF_!9hx zS20CE8*v90@ESo4lk?NK$h&*kNSE+BEhZ^_gE9qg(uYcY&+;vP$wK{tl2vSn-z`)R z)ihvLlE4!vsFk9uIEd1XVg-FXkfd~t+6$g~O44af!SkGTDU?FKX;)D=nEL#nnGXjO zSJ@En@n1GhTb4?rO9-~*s1ymgRMKxD;J-_$TeZJ@cd75SKn)QDj<9YWB=-@t^68Zo zAgQI#Yz69wro`RDahoK3Ieg5)%czbYQOt6c0sTRj(&^dOWf->+5cfocpK~s0N%T;T z=zXK1pU^;J#g=j9JEYdXKK>Mewn&a;zq>5k*s@)0qHbQ+LoW5=6#qxKuD9&jY~1CP zE|3v|dV5^HcrAMZ{?b)8x<`e+TwV6(1nY(5&f-4C3lof!4%nYk~|z4y6y{`&imR{-we2MZB~g|ezSJ6!6z z(t%dnDv)L0+0kXEu7tD9YeMs)=H;i-t2%jezbb`W*;Kv^r1C7p7Dza5NJZ5K8A#B?>beXY9i444#nvC zJ4)APE9r?6Lu^@9L;_KqwGqcThG=r46h{v(SUArx+DTNtjf=R%(EW7g=FCi<*JULy zxJl3WF)XKN#ZMcDeKs!R3d6bDV=a8u&}FeIUBPhSWQYlTf~yujWk_`bu$gun1Gq+2 zbQQ0xYE{c0d0|*QC5sasK|d;J91PcOe1xt9hB0E{Gb(Xd7^$30Zsn)4s2dvD7&STW zCQf)kAt1UL&It?S3b{Lzn4u(QbB|UE#m7tQ_cLn`GsP{2*^bEDVl17&ByO9iK4^kmJK=FdCdOT=o zGQ>aF(EQ1Syn`CP8U{pqAtd&?jSb`|AHE3kVn=A@y-C0On-c5BaZ^ry-}}F z3sQcPZ9?p;Yq56{4o!t)opoy7h){)pP2I~UAHf@DqA}PR&l7st<-RY7(zXfd?RdZt zwMLZQ-3a|?9MTHYC$vWBCXNKHAJe}-=`=>)JE=q1sb}bZj^5wV{{n+Q71v`vP;nk(n6I;}VVAD83*=*T^8~S%~2(oe!XwPWo}3 z?uRf(>z8zUn;?-(+(nwcW;c&3WXbIAVSym_lKxj%B%8yuU&4LbEz^A;BCp9=)B>~c zwS`sl9gdOi17t|=8 Date: Sat, 21 Jan 2023 11:31:27 -0800 Subject: [PATCH 10/11] quick fix --- .../fairportrobotics/frc/poe/CameraTracking/AprilTags.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 index c05d7ff..7aa4568 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java @@ -6,6 +6,8 @@ import java.util.List; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonTrackedTarget; +import edu.wpi.first.math.geometry.Transform3d; + public class AprilTags { // an ArrayList to store the cameras public final ArrayList cameras = new ArrayList<>(); @@ -59,7 +61,7 @@ public class AprilTags { 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; + if(closestTarget == null || calculateDistance(target.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget()) ) closestTarget = target; } return closestTarget; } @@ -75,4 +77,7 @@ public class AprilTags { return lowestAmbiguity; } + private double calculateDistance(Transform3d t) { + return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY() + t.getZ()*t.getZ()); + } } \ No newline at end of file From 98ced09d82e94dc33fd459c39724006f076360d3 Mon Sep 17 00:00:00 2001 From: Declipsonator <96392121+Declipsonator@users.noreply.github.com> Date: Mon, 30 Jan 2023 20:52:23 -0500 Subject: [PATCH 11/11] resolved issues --- lib/build.gradle | 22 ++++++++--------- .../frc/poe/CameraTracking/AprilTags.java | 24 ++++++++----------- .../CameraTracking/RobotFieldPosition.java | 11 ++++----- 3 files changed, 25 insertions(+), 32 deletions(-) diff --git a/lib/build.gradle b/lib/build.gradle index 95ce996..689664a 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -29,15 +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: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 'edu.wpi.first.cscore:cscore-java:2023.+' + implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.+' + implementation 'edu.wpi.first.ntcore:ntcore-java:2023.+' + implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.+' + implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.+' + implementation 'edu.wpi.first.wpimath:wpimath-java:2023.+' + implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.+' + implementation 'edu.wpi.first.hal:hal-java:2023.+' + implementation "edu.wpi.first.apriltag:apriltag-java:2023.+" 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" @@ -45,8 +45,8 @@ dependencies { implementation 'edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:4.5.2-1' implementation 'com.ctre.phoenix:api-java:5.30.3' implementation 'com.ctre.phoenix:wpiapi-java:5.30.3' - implementation "org.photonvision:PhotonLib-java:v2023.2.1" - implementation "org.photonvision:PhotonTargeting-java:v2023.2.1" + implementation "org.photonvision:PhotonLib-java:v2023.+" + implementation "org.photonvision:PhotonTargeting-java:v2023.+" } tasks.named('test') { 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 index 7aa4568..270ed39 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java @@ -25,11 +25,12 @@ public class AprilTags { for(PhotonCamera camera : cameras) { for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { boolean toAdd = true; + List tempTargets = targets; // 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); + tempTargets.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 @@ -37,10 +38,10 @@ public class AprilTags { break; } } + if(toAdd) { tempTargets.add(target); } - if(toAdd) targets.add(target); + targets = tempTargets; } - targets.addAll(camera.getLatestResult().getTargets()); } return targets; @@ -48,36 +49,31 @@ public class AprilTags { // 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; + return cameras.stream().anyMatch((camera) -> camera.getLatestResult().hasTargets() == true); } // method to get the closest target public PhotonTrackedTarget getClosestTarget() { - if(!hasTargets()) return null; + if(!hasTargets()) { return null; } PhotonTrackedTarget closestTarget = null; for(PhotonTrackedTarget target : getTargets()) { - if(closestTarget == null || calculateDistance(target.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget()) ) closestTarget = target; + if(closestTarget == null || calculateDistance(closestTarget.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget())) { closestTarget = target; } } return closestTarget; } // method to get the target with the lowest pose ambiguity public PhotonTrackedTarget getLowestAmbiguityTarget() { - if(!hasTargets()) return null; + 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; + if(lowestAmbiguity == null || lowestAmbiguity.getPoseAmbiguity() > target.getPoseAmbiguity()) { lowestAmbiguity = target; } } return lowestAmbiguity; } private double calculateDistance(Transform3d t) { - return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY() + t.getZ()*t.getZ()); + return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY()); } } \ 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 index 81fe7e2..17ed464 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,7 +9,6 @@ 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; @@ -23,12 +22,10 @@ public class RobotFieldPosition { 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(); - } + 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); }