Skip to content

Commit

Permalink
Merge branch 'main' into 47-add-json-naming
Browse files Browse the repository at this point in the history
  • Loading branch information
avidraccoon authored Dec 9, 2024
2 parents 24e923b + 1ad04a3 commit 49e5af4
Show file tree
Hide file tree
Showing 10 changed files with 157 additions and 57 deletions.
2 changes: 1 addition & 1 deletion gradle.properties
Original file line number Diff line number Diff line change
@@ -1 +1 @@
version=2024.11.29-beta
version=2025.0.0-beta
17 changes: 10 additions & 7 deletions vision/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -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'

Expand Down
23 changes: 21 additions & 2 deletions vision/src/main/java/coppercore/vision/Camera.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -39,7 +38,27 @@ public CameraMeasurement getLatestMeasurement() {
}

public Matrix<N3, N1> getLatestVariance() {
Matrix<N3, N1> 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;
}
}
49 changes: 22 additions & 27 deletions vision/src/main/java/coppercore/vision/CameraIOPhoton.java
Original file line number Diff line number Diff line change
@@ -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;
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -72,15 +71,12 @@ public void updateInputs(CameraIOInputs inputs) {
latestTimestampSeconds = result.getTimestampSeconds();
Optional<EstimatedRobotPose> 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;
Expand All @@ -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<EstimatedRobotPose> getEstimatedPose () {
// Optional<EstimatedRobotPose> 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) {
Expand Down
11 changes: 11 additions & 0 deletions vision/src/main/java/coppercore/vision/CoreVisionConstants.java
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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<N3, N1> singleTagStdDev = VecBuilder.fill(1, 1, 1);
public static final Matrix<N3, N1> multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5);
public static final Matrix<N3, N1> 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;
}
24 changes: 14 additions & 10 deletions wpilib_interface/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
1 change: 0 additions & 1 deletion wpilib_interface/src/.gitignore
Original file line number Diff line number Diff line change
@@ -1 +0,0 @@
frc/
Original file line number Diff line number Diff line change
@@ -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.
*
* <p>Note: If for some reason min is greater than max, measure &lt; min takes precedence over
* measure &lt; max, which takes precedence over min &lt; measure &lt; 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 <M> A measure, the type of the measures being compoared. This should be able to be
* inferred automatically.
* @param <U> 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 &lt; min, min will be returned
* @param max The maximum value. If measure &gt; 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 <M extends Measure<U>, 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;
}
}
18 changes: 9 additions & 9 deletions wpilib_interface/src/main/java/frc/robot/BuildConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -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 = 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() {}
}
32 changes: 32 additions & 0 deletions wpilib_interface/src/test/java/UnitUtilsTests.java
Original file line number Diff line number Diff line change
@@ -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)));
}
}

0 comments on commit 49e5af4

Please sign in to comment.