From 3de878c510ba0f6f6b4bc7aef294ada31614449b Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 16 Dec 2023 13:00:02 -0500 Subject: [PATCH 1/2] update download all artifacts (#1044) * Update build.yml * Update build.yml --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 6d329f400c..58d2356305 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -301,7 +301,7 @@ jobs: steps: # Download literally every single artifact. This also downloads client and docs, # but the filtering below won't pick these up (I hope) - - uses: actions/download-artifact@v2 + - uses: actions/download-artifact@v4 - run: find # Push to dev release - uses: pyTooling/Actions/releaser@r0 From 2e39549771eced81962d8a7235bcba8832d3d2ca Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 16 Dec 2023 13:14:52 -0500 Subject: [PATCH 2/2] Bump wpilib to 2024-beta-4 and report resource photon usage ids from 2024v2 image (#1042) --- build.gradle | 6 +- .../java/org/photonvision/PhotonCamera.java | 10 +- .../org/photonvision/PhotonPoseEstimator.java | 12 +- .../simulation/SimCameraProperties.java | 126 ++++++++++-------- .../photonvision/estimation/OpenCVHelp.java | 5 +- .../targeting/PhotonPipelineResult.java | 2 +- photonlib-cpp-examples/build.gradle | 2 +- photonlib-java-examples/build.gradle | 2 +- shared/common.gradle | 1 + shared/config.gradle | 2 +- shared/javacommon.gradle | 1 + 11 files changed, 96 insertions(+), 73 deletions(-) diff --git a/build.gradle b/build.gradle index f772c89e32..3c00130fd5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,8 @@ plugins { id "com.diffplug.spotless" version "6.22.0" - id "edu.wpi.first.NativeUtils" version "2024.3.2" apply false + id "edu.wpi.first.NativeUtils" version "2024.6.1" apply false id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" id 'edu.wpi.first.WpilibTools' version '1.3.0' } @@ -20,7 +20,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3" + wpilibVersion = "2024.1.1-beta-4" wpimathVersion = wpilibVersion openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 8b46a2c372..faa03ebc54 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,6 +24,8 @@ package org.photonvision; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -52,6 +54,7 @@ /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { + private static int InstanceCount = 0; public static final String kTableName = "photonvision"; private final NetworkTable cameraTable; @@ -152,6 +155,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { MultiSubscriber m_topicNameSubscriber = new MultiSubscriber( instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); + + HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount); + InstanceCount++; } /** @@ -321,14 +327,14 @@ public boolean isConnected() { public Optional> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { - return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix)); + return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); } else return Optional.empty(); } public Optional> getDistCoeffs() { var distCoeffs = cameraDistortionSubscriber.get(); if (distCoeffs != null && distCoeffs.length == 5) { - return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs)); + return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs)); } else return Optional.empty(); } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index dbf36292c2..f93c499140 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -25,6 +25,8 @@ package org.photonvision; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; @@ -52,6 +54,8 @@ * below. Example usage can be found in our apriltagExample example project. */ public class PhotonPoseEstimator { + private static int InstanceCount = 0; + /** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */ public enum PoseStrategy { /** Choose the Pose with the lowest ambiguity. */ @@ -118,14 +122,14 @@ public PhotonPoseEstimator( this.primaryStrategy = strategy; this.camera = camera; this.robotToCamera = robotToCamera; + + HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); + InstanceCount++; } public PhotonPoseEstimator( AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = null; - this.robotToCamera = robotToCamera; + this(fieldTags, strategy, null, robotToCamera); } /** Invalidates the pose cache. */ diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 6d157fb252..f7df72487a 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -25,6 +25,7 @@ package org.photonvision.simulation; import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -146,8 +147,8 @@ public SimCameraProperties(Path path, int width, int height) throws IOException setCalibration( jsonWidth, jsonHeight, - Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics), - Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion)); + MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics), + MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion)); setCalibError(jsonAvgError, jsonErrorStdDev); success = true; } @@ -184,7 +185,7 @@ public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { double fy = cy / Math.tan(fovHeight.getRadians() / 2.0); // create camera intrinsics matrix - var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1); + var camIntrinsics = MatBuilder.fill(Nat.N3(), Nat.N3(), fx, 0, cx, 0, fy, cy, 0, 0, 1); setCalibration(resWidth, resHeight, camIntrinsics, distCoeff); } @@ -597,17 +598,19 @@ public static SimCameraProperties PI4_LIFECAM_320_240() { prop.setCalibration( 320, 240, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 328.2733242048587, - 0.0, - 164.8190261141906, - 0.0, - 318.0609794305216, - 123.8633838438093, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 328.2733242048587, + 0.0, + 164.8190261141906, + 0.0, + 318.0609794305216, + 123.8633838438093, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.09957946553445934, -0.9166265114485799, @@ -626,17 +629,19 @@ public static SimCameraProperties PI4_LIFECAM_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 669.1428078983059, - 0.0, - 322.53377249329213, - 0.0, - 646.9843137061716, - 241.26567383784163, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 669.1428078983059, + 0.0, + 322.53377249329213, + 0.0, + 646.9843137061716, + 241.26567383784163, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.12788470750464645, -1.2350335805796528, @@ -655,17 +660,18 @@ public static SimCameraProperties LL2_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 511.22843367007755, - 0.0, - 323.62049380211096, - 0.0, - 514.5452336723849, - 261.8827920543568, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), // intrinsic + 511.22843367007755, + 0.0, + 323.62049380211096, + 0.0, + 514.5452336723849, + 261.8827920543568, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.1917469998873756, -0.5142936883324216, @@ -684,17 +690,19 @@ public static SimCameraProperties LL2_960_720() { prop.setCalibration( 960, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 769.6873145148892, - 0.0, - 486.1096609458122, - 0.0, - 773.8164483705323, - 384.66071662358354, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 769.6873145148892, + 0.0, + 486.1096609458122, + 0.0, + 773.8164483705323, + 384.66071662358354, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.189462064814501, -0.49903003669627627, @@ -713,17 +721,19 @@ public static SimCameraProperties LL2_1280_720() { prop.setCalibration( 1280, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 1011.3749416937393, - 0.0, - 645.4955139388737, - 0.0, - 1008.5391755084075, - 508.32877656020196, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 1011.3749416937393, + 0.0, + 645.4955139388737, + 0.0, + 1008.5391755084075, + 508.32877656020196, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.13730101577061535, -0.2904345656989261, diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index ed53a17b80..e2413a4d22 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -18,6 +18,7 @@ package org.photonvision.estimation; import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -63,8 +64,8 @@ public static void forceLoadOpenCV() { } static { - NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)); - EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)); + NWU_TO_EDN = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, -1, 0, 0, 0, -1, 1, 0, 0)); + EDN_TO_NWU = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, 0, 1, -1, 0, 0, 0, -1, 0)); } public static Mat matrixToMat(SimpleMatrix matrix) { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 9a8ce65db9..7047ee46aa 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -88,7 +88,7 @@ public PhotonTrackedTarget getBestTarget() { String errStr = "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " + "before calling this method. For more information, please review the PhotonLib " - + "documentation at http://docs.photonvision.org"; + + "documentation at https://docs.photonvision.org"; System.err.println(errStr); new Exception().printStackTrace(); HAS_WARNED = true; diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 881e04a114..51263e12bc 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" apply false + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" apply false } allprojects { diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 1c92e043f9..fb2207b6c5 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" apply false + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" apply false } apply from: "examples.gradle" diff --git a/shared/common.gradle b/shared/common.gradle index faeaa1f694..7cbf858b22 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -32,6 +32,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() diff --git a/shared/config.gradle b/shared/config.gradle index b3c7781c84..4044d7e3ff 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -14,7 +14,7 @@ nativeUtils.wpi.configureDependencies { opencvVersion = openCVversion googleTestYear = "frc2024" googleTestVersion = "1.14.0-1" - niLibVersion = "2024.1.1" + niLibVersion = "2024.2.1" } // Configure warnings and errors diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index c4e0ad3b56..55f72d0f60 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -113,6 +113,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get()