From b23b2d31d119e5a2645e01060dd0408a89dfeb22 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Wed, 4 Dec 2024 12:42:41 -0500 Subject: [PATCH 1/3] Vision distance (#51) * start adding distance based deviations (failing to build due to static issues) * add method for 2025 commented out and update variances * fix errors related to static attribute in average tag distance * unnest conditionals and add constants * switch to counting tags * start adding distance based deviations (failing to build due to static issues) * add method for 2025 commented out and update variances * fix errors related to static attribute in average tag distance * unnest conditionals and add constants * switch to counting tags * add buildconstants for now --- .../main/java/coppercore/vision/Camera.java | 23 ++++++++- .../coppercore/vision/CameraIOPhoton.java | 49 +++++++++---------- .../vision/CoreVisionConstants.java | 11 +++++ wpilib_interface/src/.gitignore | 1 - 4 files changed, 54 insertions(+), 30 deletions(-) diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java index 08bd7aa..48c6597 100644 --- a/vision/src/main/java/coppercore/vision/Camera.java +++ b/vision/src/main/java/coppercore/vision/Camera.java @@ -2,7 +2,6 @@ import coppercore.vision.VisionLocalizer.CameraMeasurement; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import org.littletonrobotics.junction.Logger; @@ -39,7 +38,27 @@ public CameraMeasurement getLatestMeasurement() { } public Matrix getLatestVariance() { + Matrix stdDev = CoreVisionConstants.singleTagStdDev; // TODO: Actually calculate variances! - return VecBuilder.fill(0.0, 0.0, 0.0); + double avgDistanceFromTarget = inputs.averageTagDistanceM; + int numTags = inputs.nTags; + + if (numTags == 0) { + return stdDev; + } else if (numTags > 1) { + stdDev = CoreVisionConstants.multiTagStdDev; + } else if (numTags == 1 + && avgDistanceFromTarget > CoreVisionConstants.singleTagDistanceCutoff) { + return CoreVisionConstants.rejectionStdDev; + } + + // distance based variance + stdDev = + stdDev.times( + 1 + + (Math.pow(avgDistanceFromTarget, 2) + / CoreVisionConstants.distanceFactor)); + + return stdDev; } } diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java index da313b8..a76c79f 100644 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java @@ -1,10 +1,8 @@ package coppercore.vision; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import java.util.Optional; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -33,6 +31,7 @@ public CameraIOPhoton( poseEstimator = new PhotonPoseEstimator( layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera); + poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } public static CameraIOPhoton fromRealCameraParams( @@ -72,15 +71,12 @@ public void updateInputs(CameraIOInputs inputs) { latestTimestampSeconds = result.getTimestampSeconds(); Optional photonPose = poseEstimator.update(result); - photonPose.filter(CameraIOPhoton::filterPhotonPose); - photonPose.ifPresentOrElse( (pose) -> { + calculateAverageTagDistance(pose, inputs); inputs.latestFieldToRobot = pose.estimatedPose; - inputs.nTags = pose.targetsUsed.size(); inputs.latestTimestampSeconds = this.latestTimestampSeconds; - inputs.averageTagDistanceM = calculateAverageTagDistance(pose); inputs.averageTagYaw = calculateAverageTagYaw(pose); inputs.wasAccepted = true; @@ -90,37 +86,36 @@ public void updateInputs(CameraIOInputs inputs) { }); } - private static boolean filterPhotonPose(EstimatedRobotPose photonPose) { - // TODO: Actual pose rejection - if (photonPose.targetsUsed.size() < 2) { - return false; - } - - Pose3d pose = photonPose.estimatedPose; - // check that the pose isn't insane - if (pose.getZ() > 1 || pose.getZ() < -0.1) { - return false; - } + // NOTE: Can be used in 2025 code just not ready yet + // private Optional getEstimatedPose () { + // Optional visionEstimate = Optional.empty(); - if (calculateAverageTagDistance(photonPose) - > CoreVisionConstants.maxAcceptedDistanceMeters) { - return false; - } + // for (var change : camera.getAllUnreadResults()) { + // visionEstimate = poseEstimator.update(change); + // } - return true; - } + // return visionEstimate; + // } - private static double calculateAverageTagDistance(EstimatedRobotPose pose) { + private void calculateAverageTagDistance(EstimatedRobotPose pose, CameraIOInputs inputs) { double distance = 0.0; + int numTags = 0; for (PhotonTrackedTarget target : pose.targetsUsed) { + var tagPose = poseEstimator.getFieldTags().getTagPose(target.getFiducialId()); + if (tagPose.isEmpty()) { + continue; + } + numTags += 1; distance += - target.getBestCameraToTarget() + tagPose.get() + .toPose2d() .getTranslation() - .getDistance(new Translation3d()); + .getDistance(pose.estimatedPose.toPose2d().getTranslation()); } distance /= pose.targetsUsed.size(); - return distance; + inputs.nTags = numTags; + inputs.averageTagDistanceM = distance; } private static Rotation2d calculateAverageTagYaw(EstimatedRobotPose pose) { diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java index 432f992..47f8390 100644 --- a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java +++ b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java @@ -1,5 +1,10 @@ package coppercore.vision; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + /** * Vision constants for coppercore's internal vision constants. Not to be confused with * VisionConstants, which is likely your robot project's local vision constants that are configured @@ -9,4 +14,10 @@ public final class CoreVisionConstants { // TODO: Tune this value // This value mainly exists so that this file stays named CoreVisionConstants. public static final double maxAcceptedDistanceMeters = 10.0; + public static final Matrix singleTagStdDev = VecBuilder.fill(1, 1, 1); + public static final Matrix multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5); + public static final Matrix rejectionStdDev = + VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + public static final double singleTagDistanceCutoff = 4.0; + public static final double distanceFactor = 30.0; } diff --git a/wpilib_interface/src/.gitignore b/wpilib_interface/src/.gitignore index 8cfe08b..e69de29 100644 --- a/wpilib_interface/src/.gitignore +++ b/wpilib_interface/src/.gitignore @@ -1 +0,0 @@ -frc/ \ No newline at end of file From ae92bd015cfea62f3e15e05ecaa129c58aa316b1 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Fri, 6 Dec 2024 12:09:14 -0500 Subject: [PATCH 2/3] Update coppercore to 2025 (#62) * Update vision to use 2025 libraries * Update wpilib_interface to 2025 wpilib and advantagekit * Update version string to 2025.0.0-beta --- gradle.properties | 2 +- vision/build.gradle | 17 +++++++------ wpilib_interface/build.gradle | 24 +++++++++++-------- .../main/java/frc/robot/BuildConstants.java | 18 +++++++------- 4 files changed, 34 insertions(+), 27 deletions(-) diff --git a/gradle.properties b/gradle.properties index 8c90a0f..fb0289b 100644 --- a/gradle.properties +++ b/gradle.properties @@ -1 +1 @@ -version=2024.11.29-beta \ No newline at end of file +version=2025.0.0-beta \ No newline at end of file diff --git a/vision/build.gradle b/vision/build.gradle index 1961f43..cda7dd4 100644 --- a/vision/build.gradle +++ b/vision/build.gradle @@ -33,18 +33,21 @@ task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { - implementation 'edu.wpi.first.wpilibj:wpilibj-java:2024.3.2' - implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2024.3.2' - implementation 'edu.wpi.first.apriltag:apriltag-java:2024.3.2' + def wpilibVersion = "2025.1.1-beta-1" + implementation "edu.wpi.first.wpilibj:wpilibj-java:$wpilibVersion" + implementation "edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:$wpilibVersion" + implementation "edu.wpi.first.apriltag:apriltag-java:$wpilibVersion" //def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) //annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:3.2.1" + def akitVersion = "4.0.0-alpha-1" + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitVersion" - implementation 'org.littletonrobotics.akit.junction:junction-core:3.2.1' + implementation "org.littletonrobotics.akit.junction:junction-core:$akitVersion" - implementation 'org.photonvision:photonlib-java:v2024.3.1' - implementation 'org.photonvision:photontargeting-java:v2024.3.1' + def photonVersion = "v2025.0.0-beta-5" + implementation "org.photonvision:photonlib-java:$photonVersion" + implementation "org.photonvision:photontargeting-java:$photonVersion" testRuntimeOnly 'org.junit.platform:junit-platform-launcher' diff --git a/wpilib_interface/build.gradle b/wpilib_interface/build.gradle index 35d3920..7ffeb88 100644 --- a/wpilib_interface/build.gradle +++ b/wpilib_interface/build.gradle @@ -39,22 +39,26 @@ task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { dependencies { //def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) //annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:3.2.1" + + def akitVersion = "4.0.0-alpha-1" + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitVersion" implementation project(":monitors") - implementation 'org.littletonrobotics.akit.junction:junction-core:3.2.1' - implementation 'edu.wpi.first.wpilibj:wpilibj-java:2024.3.2' - implementation 'edu.wpi.first.wpiutil:wpiutil-java:2024.3.2' - implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2024.3.2' - implementation 'edu.wpi.first.wpiunits:wpiunits-java:2024.3.2' - implementation 'edu.wpi.first.wpimath:wpimath-java:2024.3.2' + implementation "org.littletonrobotics.akit.junction:junction-core:$akitVersion" + + def wpilibVersion = "2025.1.1-beta-1" + implementation "edu.wpi.first.wpilibj:wpilibj-java:$wpilibVersion" + implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpilibVersion" + implementation "edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:$wpilibVersion" + implementation "edu.wpi.first.wpiunits:wpiunits-java:$wpilibVersion" + implementation "edu.wpi.first.wpimath:wpimath-java:$wpilibVersion" implementation project(":math") - testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' - testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + testImplementation "org.junit.jupiter:junit-jupiter:5.10.1" + testRuntimeOnly "org.junit.platform:junit-platform-launcher" - implementation 'com.googlecode.json-simple:json-simple:1.1.1' + implementation "com.googlecode.json-simple:json-simple:1.1.1" } test { diff --git a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java index 6e47643..6b7da8d 100644 --- a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java +++ b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java @@ -2,16 +2,16 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { - public static final String MAVEN_GROUP = "com.github.team401"; + public static final String MAVEN_GROUP = "io.github.team401.coppercore"; public static final String MAVEN_NAME = "wpilib_interface"; - public static final String VERSION = "0.0"; - public static final int GIT_REVISION = 33; - public static final String GIT_SHA = "1b2d554bb5908f70bb23c283af21cbe4da88f822"; - public static final String GIT_DATE = "2024-11-23 18:34:29 EST"; - public static final String GIT_BRANCH = "addJSONSaving"; - public static final String BUILD_DATE = "2024-11-23 18:37:13 EST"; - public static final long BUILD_UNIX_TIME = 1732405033508L; - public static final int DIRTY = 0; + public static final String VERSION = "2025.0.0-beta"; + public static final int GIT_REVISION = 43; + public static final String GIT_SHA = "60c0db4808cb5dcf1d9bb3c9a4ba41c86abf1d15"; + public static final String GIT_DATE = "2024-12-05 12:25:16 EST"; + public static final String GIT_BRANCH = "2025core"; + public static final String BUILD_DATE = "2024-12-06 09:44:25 EST"; + public static final long BUILD_UNIX_TIME = 1733496265888L; + public static final int DIRTY = 1; private BuildConstants() {} } From 1ad04a3ccd47e16744e6b5495f2669ebab094ff5 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Sun, 8 Dec 2024 10:53:13 -0500 Subject: [PATCH 3/3] Add UnitUtils class with clampMeasurement method (#67) --- .../wpilib_interface/UnitUtils.java | 37 +++++++++++++++++++ .../main/java/frc/robot/BuildConstants.java | 12 +++--- .../src/test/java/UnitUtilsTests.java | 32 ++++++++++++++++ 3 files changed, 75 insertions(+), 6 deletions(-) create mode 100644 wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java create mode 100644 wpilib_interface/src/test/java/UnitUtilsTests.java diff --git a/wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java b/wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java new file mode 100644 index 0000000..b927480 --- /dev/null +++ b/wpilib_interface/src/main/java/coppercore/wpilib_interface/UnitUtils.java @@ -0,0 +1,37 @@ +package coppercore.wpilib_interface; + +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; + +/** Utilities for dealing with the WPILib units library. */ +public final class UnitUtils { + /** + * Given a measurement, a minimum value, and a maximum value, return a clamped measurement + * within those bounds. + * + *

Note: If for some reason min is greater than max, measure < min takes precedence over + * measure < max, which takes precedence over min < measure < max. Therefore, when + * debugging, if this function is strangely always returning min it is highly likely there is an + * error in the generation of bounds. + * + * @param A measure, the type of the measures being compoared. This should be able to be + * inferred automatically. + * @param The Unit measured by M. This should be able to be inferred automatically. + * @param measure The measure being clamped. If it is greater than min and less than max, it + * will be the result. + * @param min The minimum value. If measure < min, min will be returned + * @param max The maximum value. If measure > max, max will be returned. + * @return If measure is within the bounds, measure. If measure is less than min, min. If + * measure is greater than max, max. + */ + public static final , U extends Unit> M clampMeasure( + M measure, M min, M max) { + if (measure.lt(min)) { + return min; + } else if (measure.gt(max)) { + return max; + } + + return measure; + } +} diff --git a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java index 6b7da8d..09c9ba9 100644 --- a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java +++ b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = "io.github.team401.coppercore"; public static final String MAVEN_NAME = "wpilib_interface"; public static final String VERSION = "2025.0.0-beta"; - public static final int GIT_REVISION = 43; - public static final String GIT_SHA = "60c0db4808cb5dcf1d9bb3c9a4ba41c86abf1d15"; - public static final String GIT_DATE = "2024-12-05 12:25:16 EST"; - public static final String GIT_BRANCH = "2025core"; - public static final String BUILD_DATE = "2024-12-06 09:44:25 EST"; - public static final long BUILD_UNIX_TIME = 1733496265888L; + public static final int GIT_REVISION = 42; + public static final String GIT_SHA = "ae92bd015cfea62f3e15e05ecaa129c58aa316b1"; + public static final String GIT_DATE = "2024-12-06 12:09:14 EST"; + public static final String GIT_BRANCH = "64-clamp-measures"; + public static final String BUILD_DATE = "2024-12-07 23:05:47 EST"; + public static final long BUILD_UNIX_TIME = 1733630747090L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/wpilib_interface/src/test/java/UnitUtilsTests.java b/wpilib_interface/src/test/java/UnitUtilsTests.java new file mode 100644 index 0000000..b7deb8f --- /dev/null +++ b/wpilib_interface/src/test/java/UnitUtilsTests.java @@ -0,0 +1,32 @@ +import coppercore.wpilib_interface.UnitUtils; +import edu.wpi.first.units.Units; +// import edu.wpi.first.units.Units.Meters; +import edu.wpi.first.units.Units.*; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + +public class UnitUtilsTests { + @Test + public void clampWithinBounds() { + Assertions.assertTrue( + UnitUtils.clampMeasure( + Units.Meters.of(2.0), Units.Meters.of(1.0), Units.Meters.of(3.0)) + .equals(Units.Meters.of(2.0))); + } + + @Test + public void clampBelowBounds() { + Assertions.assertTrue( + UnitUtils.clampMeasure( + Units.Meters.of(0.0), Units.Meters.of(1.0), Units.Meters.of(3.0)) + .equals(Units.Meters.of(1.0))); + } + + @Test + public void clampAboveBounds() { + Assertions.assertTrue( + UnitUtils.clampMeasure( + Units.Meters.of(4.0), Units.Meters.of(1.0), Units.Meters.of(3.0)) + .equals(Units.Meters.of(3.0))); + } +}