Skip to content

Commit

Permalink
Bump wpilib to 2024-beta-4 and report resource photon usage ids from …
Browse files Browse the repository at this point in the history 
…2024v2 image (#1042)
  • Loading branch information
@srimanachanta
srimanachanta committed Dec 16, 2023
1 parent 3de878c commit 2e39549
Show file tree
Hide file tree
Showing 11 changed files with 96 additions and 73 deletions.
6 changes: 3 additions & 3 deletions build.gradle
View file
Original file line number Diff line number Diff line change
@@ -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'
}

Expand All @@ -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"
Expand Down
10 changes: 8 additions & 2 deletions photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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++;
}

/**
Expand Down Expand Up @@ -321,14 +327,14 @@ public boolean isConnected() {
public Optional<Matrix<N3, N3>> 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<Matrix<N5, N1>> 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();
}

Expand Down
12 changes: 8 additions & 4 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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. */
Expand Down Expand Up @@ -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. */
Expand Down
126 changes: 68 additions & 58 deletions photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand Down
5 changes: 3 additions & 2 deletions photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down
2 changes: 1 addition & 1 deletion photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion photonlib-cpp-examples/build.gradle
View file
Original file line number Diff line number Diff line change
@@ -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 {
Expand Down
2 changes: 1 addition & 1 deletion photonlib-java-examples/build.gradle
View file
Original file line number Diff line number Diff line change
@@ -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"
Expand Down
1 change: 1 addition & 0 deletions shared/common.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
2 changes: 1 addition & 1 deletion shared/config.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions shared/javacommon.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down

0 comments on commit 2e39549

Please sign in to comment.