From b49a67937ce4be6856f29a9d88488c25618d8da3 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Mon, 23 Feb 2026 17:26:23 -0800 Subject: [PATCH 01/19] Add isNotWithin for all subjects, and add MeasureSubject --- .../lib2813/testing/truth/MeasureSubject.java | 109 ++++++++++++++++++ .../lib2813/testing/truth/Pose2dSubject.java | 10 ++ .../lib2813/testing/truth/Pose3dSubject.java | 10 ++ .../testing/truth/Rotation2dSubject.java | 9 ++ .../testing/truth/Rotation3dSubject.java | 11 ++ .../testing/truth/Translation2dSubject.java | 10 ++ .../testing/truth/Translation3dSubject.java | 11 ++ 7 files changed, 170 insertions(+) create mode 100644 testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java new file mode 100644 index 00000000..814ba386 --- /dev/null +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java @@ -0,0 +1,109 @@ +/* +Copyright 2026 Prospect Robotics SWENext Club + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +package com.team2813.lib2813.testing.truth; + +import static com.google.common.base.Preconditions.checkArgument; +import static com.google.common.truth.Fact.fact; +import static com.google.common.truth.Fact.simpleFact; +import static com.google.common.truth.Truth.assertAbout; + +import com.google.common.primitives.Doubles; +import com.google.common.truth.FailureMetadata; +import com.google.common.truth.Subject; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Unit; +import javax.annotation.Nullable; + +public class MeasureSubject extends Subject { + public static MeasureSubject assertThat(@Nullable Measure measure) { + return assertAbout(MeasureSubject.measures()).that(measure); + } + + public static Subject.Factory, Measure> measures() { + return MeasureSubject::new; + } + + private final Measure actual; + + private MeasureSubject(FailureMetadata failureMetadata, @Nullable Measure subject) { + super(failureMetadata, subject); + this.actual = subject; + } + + public TolerantComparison> isWithin(Measure tolerance) { + return new TolerantComparison>() { + @Override + public void of(Measure expected) { + Measure actual = nonNullActual(); + checkTolerance(tolerance); + if (!equalWithinTolerance(actual, expected, tolerance)) { + failWithoutActual( + fact("expected", expected.toLongString()), + fact("but was", actual.toLongString()), + fact("outside tolerance", tolerance.toLongString())); + } + } + }; + } + + public TolerantComparison> isNotWithin(Measure tolerance) { + return new TolerantComparison>() { + @Override + public void of(Measure expected) { + Measure actual = nonNullActual(); + checkTolerance(tolerance); + if (!notEqualWithinTolerance(actual, expected, tolerance)) { + failWithoutActual( + fact("expected not to be", expected.toLongString()), + fact("but was", actual.toLongString()), + fact("within tolerance", tolerance.toLongString())); + } + } + }; + } + + private static boolean equalWithinTolerance( + Measure left, Measure right, Measure tolerance) { + return Math.abs(left.baseUnitMagnitude() - right.baseUnitMagnitude()) + <= Math.abs(tolerance.baseUnitMagnitude()); + } + + private static boolean notEqualWithinTolerance( + Measure left, Measure right, Measure tolerance) { + double leftD = left.baseUnitMagnitude(); + double rightD = right.baseUnitMagnitude(); + if (Doubles.isFinite(leftD) && Doubles.isFinite(rightD)) { + return Math.abs(leftD - rightD) > Math.abs(tolerance.baseUnitMagnitude()); + } else { + return false; + } + } + + private void checkTolerance(Measure tolerance) { + double mag = tolerance.baseUnitMagnitude(); + checkArgument(!Double.isNaN(mag), "tolerance cannot be NaN"); + checkArgument(mag >= 0, "tolerance (%s) cannot be negative", tolerance); + checkArgument( + mag != Double.POSITIVE_INFINITY, "tolerance cannot be POSITIVE_INFINITY", tolerance); + } + + private Measure nonNullActual() { + if (actual == null) { + failWithActual(simpleFact("expected a non-null Measure")); + } + return actual; + } +} diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java index a5e56dab..56d9b34d 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java @@ -62,6 +62,16 @@ public void of(Pose2d expected) { }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Pose2d expected) { + translation().isNotWithin(tolerance).of(expected.getTranslation()); + rotation().isWithin(tolerance).of(expected.getRotation()); + } + }; + } + public Translation2dSubject translation() { return check("getTranslation()") .about(Translation2dSubject.translation2ds()) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java index fd9ada6e..0b13aead 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java @@ -62,6 +62,16 @@ public void of(Pose3d expected) { }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Pose3d expected) { + translation().isNotWithin(tolerance).of(expected.getTranslation()); + rotation().isNotWithin(tolerance).of(expected.getRotation()); + } + }; + } + // Chained subjects methods below this point public Translation3dSubject translation() { diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java index 7c11ca3c..ce81bb6c 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java @@ -58,6 +58,15 @@ public void of(Rotation2d expected) { }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Rotation2d expected) { + getRadians().isNotWithin(tolerance).of(expected.getRadians()); + } + }; + } + public void isZero() { if (!Rotation2d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java index 9e6e487d..106c77c5 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java @@ -60,6 +60,17 @@ public void of(Rotation3d expected) { }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Rotation3d expected) { + x().isNotWithin(tolerance).of(expected.getX()); // roll, in radians + y().isNotWithin(tolerance).of(expected.getY()); // pitch, in radians + z().isNotWithin(tolerance).of(expected.getZ()); // yaw, in radians + } + }; + } + public void isZero() { if (!Rotation3d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java index 8a96d4a2..8009170e 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java @@ -60,6 +60,16 @@ public void of(Translation2d expected) { }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Translation2d expected) { + x().isWithin(tolerance).of(expected.getX()); + y().isWithin(tolerance).of(expected.getY()); + } + }; + } + public void isZero() { if (!Translation2d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java index 955ff8e6..59c179ed 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java @@ -61,6 +61,17 @@ public void of(Translation3d expected) { }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Translation3d expected) { + x().isNotWithin(tolerance).of(expected.getX()); + y().isNotWithin(tolerance).of(expected.getY()); + z().isNotWithin(tolerance).of(expected.getZ()); + } + }; + } + public void isZero() { if (!Translation3d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); From c7f4c226be2ce4a0e1345ea8dde0f53973c12a9f Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Mon, 23 Feb 2026 19:06:44 -0800 Subject: [PATCH 02/19] Make the error prints look nicer --- .../lib2813/testing/truth/MeasureSubject.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java index 814ba386..9e25c568 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java @@ -51,9 +51,9 @@ public void of(Measure expected) { checkTolerance(tolerance); if (!equalWithinTolerance(actual, expected, tolerance)) { failWithoutActual( - fact("expected", expected.toLongString()), - fact("but was", actual.toLongString()), - fact("outside tolerance", tolerance.toLongString())); + fact("expected", formatUnit(expected)), + fact("but was", formatUnit(actual)), + fact("outside tolerance", formatUnit(tolerance))); } } }; @@ -67,9 +67,9 @@ public void of(Measure expected) { checkTolerance(tolerance); if (!notEqualWithinTolerance(actual, expected, tolerance)) { failWithoutActual( - fact("expected not to be", expected.toLongString()), - fact("but was", actual.toLongString()), - fact("within tolerance", tolerance.toLongString())); + fact("expected not to be", formatUnit(expected)), + fact("but was", formatUnit(actual)), + fact("within tolerance", formatUnit(tolerance))); } } }; @@ -92,6 +92,10 @@ private static boolean notEqualWithinTolerance( } } + private static String formatUnit(Measure measure) { + return String.format("%g %s", measure.magnitude(), measure.unit().name()); + } + private void checkTolerance(Measure tolerance) { double mag = tolerance.baseUnitMagnitude(); checkArgument(!Double.isNaN(mag), "tolerance cannot be NaN"); From 60517f2ec212bcf56618b6469bf87feae853e4f7 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Mon, 23 Feb 2026 19:33:29 -0800 Subject: [PATCH 03/19] Add some documentation on MeasureSubject --- .../team2813/lib2813/testing/truth/MeasureSubject.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java index 9e25c568..9a5d3ea6 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java @@ -27,6 +27,15 @@ import edu.wpi.first.units.Unit; import javax.annotation.Nullable; +/** + * Truth subject for making assertions about {@link Measure} values. + * + *

See Writing your own custom subject to learn about + * creating custom Truth subjects. + * + * @param The WPILib Unit type of the {@link Measure} + * @since 2.1.0 + */ public class MeasureSubject extends Subject { public static MeasureSubject assertThat(@Nullable Measure measure) { return assertAbout(MeasureSubject.measures()).that(measure); From 25e55810f26aba05963eeb128e1206807cabbe4e Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 28 Mar 2026 11:05:41 -0700 Subject: [PATCH 04/19] Add tests for Pose2dSubject.isNotWithin() --- .../lib2813/testing/truth/Pose2dSubject.java | 2 +- .../testing/truth/Translation2dSubject.java | 4 ++-- .../testing/truth/Pose2dSubjectTest.java | 17 +++++++++++++++++ 3 files changed, 20 insertions(+), 3 deletions(-) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java index 56d9b34d..2565d4ba 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java @@ -67,7 +67,7 @@ public TolerantComparison isNotWithin(double tolerance) { @Override public void of(Pose2d expected) { translation().isNotWithin(tolerance).of(expected.getTranslation()); - rotation().isWithin(tolerance).of(expected.getRotation()); + rotation().isNotWithin(tolerance).of(expected.getRotation()); } }; } diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java index 8009170e..b443887f 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java @@ -64,8 +64,8 @@ public TolerantComparison isNotWithin(double tolerance) { return new TolerantComparison() { @Override public void of(Translation2d expected) { - x().isWithin(tolerance).of(expected.getX()); - y().isWithin(tolerance).of(expected.getY()); + x().isNotWithin(tolerance).of(expected.getX()); + y().isNotWithin(tolerance).of(expected.getY()); } }; } diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java index 211b72a7..d503454c 100644 --- a/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java @@ -34,6 +34,15 @@ public void isWithin_valueWithinTolerance_doesNotThrow(Pose2dComponent component Pose2dSubject.assertThat(closePose).isWithin(0.01).of(POSE); } + @ParameterizedTest + @EnumSource(Pose2dComponent.class) + public void isNotWithin_valueWithinTolerance_throws(Pose2dComponent component) { + Pose2d closePose = component.add(POSE, 0.009); + + assertThrows( + AssertionError.class, () -> Pose2dSubject.assertThat(closePose).isNotWithin(0.01).of(POSE)); + } + @ParameterizedTest @EnumSource(Pose2dComponent.class) public void isWithin_valueNotWithinTolerance_throws(Pose2dComponent component) { @@ -42,4 +51,12 @@ public void isWithin_valueNotWithinTolerance_throws(Pose2dComponent component) { assertThrows( AssertionError.class, () -> Pose2dSubject.assertThat(closePose).isWithin(0.01).of(POSE)); } + + @ParameterizedTest + @EnumSource(Pose2dComponent.class) + public void isNotWithin_valueNotWithinTolerance_doesNotThrow(Pose2dComponent component) { + Pose2d closePose = component.add(POSE, 0.011); + + Pose2dSubject.assertThat(closePose).isNotWithin(0.01).of(POSE); + } } From 787d19c945429ccc445f7dbdb2149092f0a59708 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 28 Mar 2026 11:07:25 -0700 Subject: [PATCH 05/19] Add tests for Rotation2dSubject.isNotWithin() --- .../testing/truth/Rotation2dSubjectTest.java | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java index 1c93357c..98c89a44 100644 --- a/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/Rotation2dSubjectTest.java @@ -39,4 +39,20 @@ public void isWithin_valueNotWithinTolerance_throws() { AssertionError.class, () -> Rotation2dSubject.assertThat(closeRotation).isWithin(0.01).of(ROTATION)); } + + @Test + public void isNotWithin_valueWithinTolerance_throws() { + Rotation2d closeRotation = Pose2dComponent.R.add(ROTATION, 0.009); + + assertThrows( + AssertionError.class, + () -> Rotation2dSubject.assertThat(closeRotation).isNotWithin(0.01).of(ROTATION)); + } + + @Test + public void isNotWithin_valueNotWithinTolerance_doesNotThrow() { + Rotation2d closeRotation = Pose2dComponent.R.add(ROTATION, 0.011); + + Rotation2dSubject.assertThat(closeRotation).isNotWithin(0.01).of(ROTATION); + } } From d5c5d85fe3f44b9810e55285b580cec0488c36de Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 28 Mar 2026 11:18:38 -0700 Subject: [PATCH 06/19] Remove (for now?) broken isNotWith() methods --- .../lib2813/testing/truth/Pose2dSubject.java | 10 ---------- .../lib2813/testing/truth/Pose3dSubject.java | 10 ---------- .../testing/truth/Rotation3dSubject.java | 11 ----------- .../testing/truth/Translation2dSubject.java | 10 ---------- .../testing/truth/Translation3dSubject.java | 11 ----------- .../testing/truth/Pose2dSubjectTest.java | 17 ----------------- 6 files changed, 69 deletions(-) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java index 2565d4ba..a5e56dab 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java @@ -62,16 +62,6 @@ public void of(Pose2d expected) { }; } - public TolerantComparison isNotWithin(double tolerance) { - return new TolerantComparison() { - @Override - public void of(Pose2d expected) { - translation().isNotWithin(tolerance).of(expected.getTranslation()); - rotation().isNotWithin(tolerance).of(expected.getRotation()); - } - }; - } - public Translation2dSubject translation() { return check("getTranslation()") .about(Translation2dSubject.translation2ds()) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java index 0b13aead..fd9ada6e 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java @@ -62,16 +62,6 @@ public void of(Pose3d expected) { }; } - public TolerantComparison isNotWithin(double tolerance) { - return new TolerantComparison() { - @Override - public void of(Pose3d expected) { - translation().isNotWithin(tolerance).of(expected.getTranslation()); - rotation().isNotWithin(tolerance).of(expected.getRotation()); - } - }; - } - // Chained subjects methods below this point public Translation3dSubject translation() { diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java index 106c77c5..9e6e487d 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java @@ -60,17 +60,6 @@ public void of(Rotation3d expected) { }; } - public TolerantComparison isNotWithin(double tolerance) { - return new TolerantComparison() { - @Override - public void of(Rotation3d expected) { - x().isNotWithin(tolerance).of(expected.getX()); // roll, in radians - y().isNotWithin(tolerance).of(expected.getY()); // pitch, in radians - z().isNotWithin(tolerance).of(expected.getZ()); // yaw, in radians - } - }; - } - public void isZero() { if (!Rotation3d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java index b443887f..8a96d4a2 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java @@ -60,16 +60,6 @@ public void of(Translation2d expected) { }; } - public TolerantComparison isNotWithin(double tolerance) { - return new TolerantComparison() { - @Override - public void of(Translation2d expected) { - x().isNotWithin(tolerance).of(expected.getX()); - y().isNotWithin(tolerance).of(expected.getY()); - } - }; - } - public void isZero() { if (!Translation2d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java index 59c179ed..955ff8e6 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java @@ -61,17 +61,6 @@ public void of(Translation3d expected) { }; } - public TolerantComparison isNotWithin(double tolerance) { - return new TolerantComparison() { - @Override - public void of(Translation3d expected) { - x().isNotWithin(tolerance).of(expected.getX()); - y().isNotWithin(tolerance).of(expected.getY()); - z().isNotWithin(tolerance).of(expected.getZ()); - } - }; - } - public void isZero() { if (!Translation3d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java index d503454c..211b72a7 100644 --- a/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/Pose2dSubjectTest.java @@ -34,15 +34,6 @@ public void isWithin_valueWithinTolerance_doesNotThrow(Pose2dComponent component Pose2dSubject.assertThat(closePose).isWithin(0.01).of(POSE); } - @ParameterizedTest - @EnumSource(Pose2dComponent.class) - public void isNotWithin_valueWithinTolerance_throws(Pose2dComponent component) { - Pose2d closePose = component.add(POSE, 0.009); - - assertThrows( - AssertionError.class, () -> Pose2dSubject.assertThat(closePose).isNotWithin(0.01).of(POSE)); - } - @ParameterizedTest @EnumSource(Pose2dComponent.class) public void isWithin_valueNotWithinTolerance_throws(Pose2dComponent component) { @@ -51,12 +42,4 @@ public void isWithin_valueNotWithinTolerance_throws(Pose2dComponent component) { assertThrows( AssertionError.class, () -> Pose2dSubject.assertThat(closePose).isWithin(0.01).of(POSE)); } - - @ParameterizedTest - @EnumSource(Pose2dComponent.class) - public void isNotWithin_valueNotWithinTolerance_doesNotThrow(Pose2dComponent component) { - Pose2d closePose = component.add(POSE, 0.011); - - Pose2dSubject.assertThat(closePose).isNotWithin(0.01).of(POSE); - } } From 590d3d297d2d5bd979119dfe388e45cc08ac0179 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 28 Mar 2026 12:17:11 -0700 Subject: [PATCH 07/19] Add tests for MeasureSubject --- .../testing/truth/MeasureSubjectTest.java | 293 ++++++++++++++++++ 1 file changed, 293 insertions(+) create mode 100644 testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java new file mode 100644 index 00000000..d4a38755 --- /dev/null +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java @@ -0,0 +1,293 @@ +/* +Copyright 2026 Prospect Robotics SWENext Club + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +package com.team2813.lib2813.testing.truth; + +import static com.google.common.truth.ExpectFailure.assertThat; +import static com.google.common.truth.Truth.assertThat; +import static edu.wpi.first.units.Units.Volts; +import static org.junit.jupiter.api.Assertions.*; + +import com.google.common.truth.ExpectFailure; +import edu.wpi.first.units.measure.Voltage; +import org.junit.jupiter.api.Test; + +/** Tests for {@link MeasureSubject}. */ +class MeasureSubjectTest { + + @Test + public void isWithin_toleranceIsNegative_throwsIllegalArgumentException() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.001); + Voltage tolerance = Volts.of(-0.01); + + IllegalArgumentException e = + assertThrows( + IllegalArgumentException.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("negative"); + } + + @Test + public void isWithin_toleranceIsNan_throwsIllegalArgumentException() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.001); + Voltage tolerance = Volts.of(Double.NaN); + + IllegalArgumentException e = + assertThrows( + IllegalArgumentException.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("NaN"); + } + + @Test + public void isWithin_toleranceIsInfinity_throwsIllegalArgumentException() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.001); + Voltage tolerance = Volts.of(Double.POSITIVE_INFINITY); + + IllegalArgumentException e = + assertThrows( + IllegalArgumentException.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("POSITIVE_INFINITY"); + } + + @Test + public void isWithin_nullActual_throws() { + Voltage expected = Volts.of(12); + Voltage actual = null; + Voltage tolerance = Volts.of(0.01); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("non-null"); + } + + @Test + public void isWithin_valueWithinTolerance_doesNotThrow() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.001); + Voltage tolerance = Volts.of(0.01); + + MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected); + } + + @Test + public void isWithin_valueNotWithinTolerance_throws() { + Voltage expected = Volts.of(12.1); + Voltage actual = Volts.of(12.2); + Voltage tolerance = Volts.of(0.001); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + assertThat(e).factKeys().containsExactly("expected", "but was", "outside tolerance"); + assertThat(e).factValue("expected").matches("12\\.1.*Volt"); + assertThat(e).factValue("but was").matches("12\\.2.*Volt"); + assertThat(e).factValue("outside tolerance").matches("0\\.001.*Volt"); + } + + @Test + public void isWithin_actualPositiveInfinity_throws() { + Voltage expected = Volts.of(12.1); + Voltage actual = Volts.of(Double.POSITIVE_INFINITY); + Voltage tolerance = Volts.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e) + .factKeys() + .containsExactly("expected", "but was", "outside tolerance"); + ExpectFailure.assertThat(e).factValue("expected").matches("12\\.1.*Volt"); + ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Volt"); + } + + @Test + public void isWithin_expectedPositiveInfinity_throws() { + Voltage expected = Volts.of(Double.POSITIVE_INFINITY); + Voltage actual = Volts.of(12.1); + Voltage tolerance = Volts.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e) + .factKeys() + .containsExactly("expected", "but was", "outside tolerance"); + ExpectFailure.assertThat(e).factValue("expected").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("but was").matches("12\\.1.*Volt"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Volt"); + } + + @Test + public void isWithin_bothPositiveInfinity_throws() { + Voltage expected = Volts.of(Double.POSITIVE_INFINITY); + Voltage actual = Volts.of(Double.POSITIVE_INFINITY); + Voltage tolerance = Volts.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e) + .factKeys() + .containsExactly("expected", "but was", "outside tolerance"); + ExpectFailure.assertThat(e).factValue("expected").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Volt"); + } + + @Test + public void isNotWithin_toleranceIsNegative_throwsIllegalArgumentException() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.001); + Voltage tolerance = Volts.of(-0.01); + + IllegalArgumentException e = + assertThrows( + IllegalArgumentException.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("negative"); + } + + @Test + public void isNotWithin_toleranceIsNan_throwsIllegalArgumentException() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.001); + Voltage tolerance = Volts.of(Double.NaN); + + IllegalArgumentException e = + assertThrows( + IllegalArgumentException.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("NaN"); + } + + @Test + public void isNotWithin_toleranceIsInfinity_throwsIllegalArgumentException() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.001); + Voltage tolerance = Volts.of(Double.POSITIVE_INFINITY); + + IllegalArgumentException e = + assertThrows( + IllegalArgumentException.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("POSITIVE_INFINITY"); + } + + @Test + public void isNotWithin_nullActual_throws() { + Voltage expected = Volts.of(12); + Voltage actual = null; + Voltage tolerance = Volts.of(0.01); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + assertThat(e).hasMessageThat().contains("non-null"); + } + + @Test + public void isNotWithin_valueNotWithinTolerance_doesNotThrow() { + Voltage expected = Volts.of(12); + Voltage actual = Volts.of(12.1); + Voltage tolerance = Volts.of(0.001); + + MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected); + } + + @Test + public void isNotWithin_valueWithinTolerance_throws() { + Voltage expected = Volts.of(12.1); + Voltage actual = Volts.of(12.01); + Voltage tolerance = Volts.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e) + .factKeys() + .containsExactly("expected not to be", "but was", "within tolerance"); + ExpectFailure.assertThat(e).factValue("expected not to be").matches("12\\.1.*Volt"); + ExpectFailure.assertThat(e).factValue("but was").matches("12\\.01.*Volt"); + ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt"); + } + + @Test + public void isNotWithin_actualPositiveInfinity_throws() { + Voltage expected = Volts.of(12.1); + Voltage actual = Volts.of(Double.POSITIVE_INFINITY); + Voltage tolerance = Volts.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e) + .factKeys() + .containsExactly("expected not to be", "but was", "within tolerance"); + ExpectFailure.assertThat(e).factValue("expected not to be").matches("12\\.1.*Volt"); + ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt"); + } + + @Test + public void isNotWithin_expectedPositiveInfinity_throws() { + Voltage expected = Volts.of(Double.POSITIVE_INFINITY); + Voltage actual = Volts.of(12.1); + Voltage tolerance = Volts.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e) + .factKeys() + .containsExactly("expected not to be", "but was", "within tolerance"); + ExpectFailure.assertThat(e).factValue("expected not to be").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("but was").matches("12\\.1.*Volt"); + ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt"); + } + + @Test + public void isNotWithin_bothPositiveInfinity_throws() { + Voltage expected = Volts.of(Double.POSITIVE_INFINITY); + Voltage actual = Volts.of(Double.POSITIVE_INFINITY); + Voltage tolerance = Volts.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, + () -> MeasureSubject.assertThat(actual).isNotWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e) + .factKeys() + .containsExactly("expected not to be", "but was", "within tolerance"); + ExpectFailure.assertThat(e).factValue("expected not to be").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt"); + ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt"); + } +} From f642779f729c71e8c396d266f3e4b453d504956f Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Tue, 9 Jun 2026 22:34:06 -0700 Subject: [PATCH 08/19] remove uneccesary absolute value calculation, and check for negative 0 in the tolerence Should just use the helper method once #156 gets merged into main fully --- .idea/AndroidProjectSystem.xml | 6 ++++++ .idea/compiler.xml | 14 +++++++++++++- .idea/misc.xml | 3 +-- .idea/modules.xml | 8 ++++++++ .idea/modules/core/lib2813.core.main.iml | 8 ++++++++ .../lib2813/testing/truth/MeasureSubject.java | 10 ++++++++-- 6 files changed, 44 insertions(+), 5 deletions(-) create mode 100644 .idea/AndroidProjectSystem.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/modules/core/lib2813.core.main.iml diff --git a/.idea/AndroidProjectSystem.xml b/.idea/AndroidProjectSystem.xml new file mode 100644 index 00000000..4a53bee8 --- /dev/null +++ b/.idea/AndroidProjectSystem.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/.idea/compiler.xml b/.idea/compiler.xml index cdd14b22..ec590373 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -7,13 +7,16 @@ - + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index 87489874..ffab8a5d 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,5 +1,4 @@ - - + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 00000000..9e57f5be --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/modules/core/lib2813.core.main.iml b/.idea/modules/core/lib2813.core.main.iml new file mode 100644 index 00000000..e1ffb953 --- /dev/null +++ b/.idea/modules/core/lib2813.core.main.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java index 9a5d3ea6..93e1be28 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java @@ -87,7 +87,7 @@ public void of(Measure expected) { private static boolean equalWithinTolerance( Measure left, Measure right, Measure tolerance) { return Math.abs(left.baseUnitMagnitude() - right.baseUnitMagnitude()) - <= Math.abs(tolerance.baseUnitMagnitude()); + <= tolerance.baseUnitMagnitude(); } private static boolean notEqualWithinTolerance( @@ -95,7 +95,7 @@ private static boolean notEqualWithinTolerance( double leftD = left.baseUnitMagnitude(); double rightD = right.baseUnitMagnitude(); if (Doubles.isFinite(leftD) && Doubles.isFinite(rightD)) { - return Math.abs(leftD - rightD) > Math.abs(tolerance.baseUnitMagnitude()); + return Math.abs(leftD - rightD) > tolerance.baseUnitMagnitude(); } else { return false; } @@ -106,9 +106,15 @@ private static String formatUnit(Measure measure) { } private void checkTolerance(Measure tolerance) { + // TOOO: Replace with call to SubjectHelper.checkTolerance when Prospect-Robotics/lib2813#156 + // gets fully merged into main double mag = tolerance.baseUnitMagnitude(); checkArgument(!Double.isNaN(mag), "tolerance cannot be NaN"); checkArgument(mag >= 0, "tolerance (%s) cannot be negative", tolerance); + checkArgument( + Double.doubleToLongBits(mag) != Double.doubleToLongBits(-0.0), + "tolerance (%s) cannot be negative", + tolerance); checkArgument( mag != Double.POSITIVE_INFINITY, "tolerance cannot be POSITIVE_INFINITY", tolerance); } From 1679846def2168696b9210b67c7f438cf5fc2fa2 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 10 Jun 2026 12:19:44 -0700 Subject: [PATCH 09/19] Add more tests and fix a bug Originally, the tolerance was just converted to the base unit, and that is used as the tolerance. This is usually accurate, but it isn't for units that have an offset, like temperature. So, the tolerance is now the given tolerance (in the base unit) minus 0 units of the tolerance's unit in the base unit. This makes it work as expected for units with an offset (like Fahrenheit and Celsius). --- .../lib2813/testing/truth/MeasureSubject.java | 9 ++- .../testing/truth/MeasureSubjectTest.java | 78 ++++++++++++++++++- 2 files changed, 83 insertions(+), 4 deletions(-) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java index 93e1be28..880b36b4 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java @@ -86,8 +86,10 @@ public void of(Measure expected) { private static boolean equalWithinTolerance( Measure left, Measure right, Measure tolerance) { - return Math.abs(left.baseUnitMagnitude() - right.baseUnitMagnitude()) - <= tolerance.baseUnitMagnitude(); + // If there is an offset, 0 of tolerance's unit is not 0 in the base unit. Explicitly subtract + // them so that the tolerance acts as expected (a delta from 0 of the unit) + double baseTolerance = tolerance.baseUnitMagnitude() - tolerance.unit().toBaseUnits(0); + return Math.abs(left.baseUnitMagnitude() - right.baseUnitMagnitude()) <= baseTolerance; } private static boolean notEqualWithinTolerance( @@ -95,7 +97,8 @@ private static boolean notEqualWithinTolerance( double leftD = left.baseUnitMagnitude(); double rightD = right.baseUnitMagnitude(); if (Doubles.isFinite(leftD) && Doubles.isFinite(rightD)) { - return Math.abs(leftD - rightD) > tolerance.baseUnitMagnitude(); + double baseTolerance = tolerance.baseUnitMagnitude() - tolerance.unit().toBaseUnits(0); + return Math.abs(leftD - rightD) > baseTolerance; } else { return false; } diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java index d4a38755..157ada03 100644 --- a/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java @@ -17,15 +17,22 @@ import static com.google.common.truth.ExpectFailure.assertThat; import static com.google.common.truth.Truth.assertThat; -import static edu.wpi.first.units.Units.Volts; +import static com.team2813.lib2813.testing.truth.MeasureSubject.assertThat; +import static edu.wpi.first.units.Units.*; import static org.junit.jupiter.api.Assertions.*; import com.google.common.truth.ExpectFailure; +import edu.wpi.first.units.TemperatureUnit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.junit.jupiter.api.Test; /** Tests for {@link MeasureSubject}. */ class MeasureSubjectTest { + /** The Rankine unit of temperature. It's just Fahrenheit with 0°Ra being absolute zero. */ + private static final TemperatureUnit Rankine = + derive(Units.Fahrenheit).offset(-459.67).named("Rankine").symbol("°Ra").make(); @Test public void isWithin_toleranceIsNegative_throwsIllegalArgumentException() { @@ -290,4 +297,73 @@ public void isNotWithin_bothPositiveInfinity_throws() { ExpectFailure.assertThat(e).factValue("but was").matches("Infinity Volt"); ExpectFailure.assertThat(e).factValue("within tolerance").matches("0\\.1.*Volt"); } + + @Test + public void isWithin_rankineTolerance_withinTolerance_doesNotThrow() { + Temperature expected = Fahrenheit.of(60); + Temperature actual = Fahrenheit.of(60.05); + Temperature tolerance = Rankine.of(0.1); + + assertThat(actual).isWithin(tolerance).of(expected); + } + + @Test + public void isWithin_rankineTolerance_notWithinTolerance_throws() { + Temperature expected = Fahrenheit.of(60.1); + Temperature actual = Fahrenheit.of(60.5); + Temperature tolerance = Rankine.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, () -> assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e).factValue("expected").matches("60\\.1.*Fahrenheit"); + ExpectFailure.assertThat(e).factValue("but was").matches("60\\.5.*Fahrenheit"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Rankine"); + } + + @Test + public void isWithin_rankineExpected_withinTolerance_doesNotThrow() { + Temperature expected = Rankine.of(519.67); + Temperature actual = Fahrenheit.of(60.05); + Temperature tolerance = Fahrenheit.of(0.1); + + assertThat(actual).isWithin(tolerance).of(expected); + } + + @Test + public void isWithin_rankineExpected_notWithinTolerance_throws() { + Temperature expected = Rankine.of(519.77); + Temperature actual = Fahrenheit.of(60.5); + Temperature tolerance = Fahrenheit.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, () -> assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e).factValue("expected").matches("519\\.77.*Rankine"); + ExpectFailure.assertThat(e).factValue("but was").matches("60\\.5.*Fahrenheit"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Fahrenheit"); + } + + @Test + public void isWithin_rankineActual_withinTolerance_doesNotThrow() { + Temperature expected = Fahrenheit.of(60); + Temperature actual = Rankine.of(519.72); + Temperature tolerance = Fahrenheit.of(0.1); + + assertThat(actual).isWithin(tolerance).of(expected); + } + + @Test + public void isWithin_rankineActual_notWithinTolerance_throws() { + Temperature expected = Fahrenheit.of(60.1); + Temperature actual = Rankine.of(520.17); + Temperature tolerance = Fahrenheit.of(0.1); + + AssertionError e = + assertThrows( + AssertionError.class, () -> assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e).factValue("expected").matches("60\\.1.*Fahrenheit"); + ExpectFailure.assertThat(e).factValue("but was").matches("520\\.17.*Rankine"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Fahrenheit"); + } } From 24a7a32b5b2ccc9bb7565a4d3b41a3912c25ff99 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 10 Jun 2026 17:17:25 -0700 Subject: [PATCH 10/19] fixup! remove uneccesary absolute value calculation, and check for negative 0 in the tolerence --- .idea/compiler.xml | 14 +------------- .idea/misc.xml | 3 ++- 2 files changed, 3 insertions(+), 14 deletions(-) diff --git a/.idea/compiler.xml b/.idea/compiler.xml index ec590373..cdd14b22 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -7,16 +7,13 @@ - + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index ffab8a5d..87489874 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,4 +1,5 @@ + - + \ No newline at end of file From 9bc6fddd48a341051fbffaf0dac70d3277079052 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 10 Jun 2026 17:19:26 -0700 Subject: [PATCH 11/19] fixup! remove uneccesary absolute value calculation, and check for negative 0 in the tolerence --- .idea/AndroidProjectSystem.xml | 6 ------ .idea/modules.xml | 8 -------- .idea/modules/core/lib2813.core.main.iml | 8 -------- 3 files changed, 22 deletions(-) delete mode 100644 .idea/AndroidProjectSystem.xml delete mode 100644 .idea/modules.xml delete mode 100644 .idea/modules/core/lib2813.core.main.iml diff --git a/.idea/AndroidProjectSystem.xml b/.idea/AndroidProjectSystem.xml deleted file mode 100644 index 4a53bee8..00000000 --- a/.idea/AndroidProjectSystem.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index 9e57f5be..00000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/modules/core/lib2813.core.main.iml b/.idea/modules/core/lib2813.core.main.iml deleted file mode 100644 index e1ffb953..00000000 --- a/.idea/modules/core/lib2813.core.main.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file From 02064cab199e35dfccd34470e98cd5f24b03a2f6 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 10 Jun 2026 17:29:09 -0700 Subject: [PATCH 12/19] Add tests for centimeters and meters Now there are tests for both units with offsets, and those without offsets --- .../testing/truth/MeasureSubjectTest.java | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java index 157ada03..5b4050d3 100644 --- a/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/MeasureSubjectTest.java @@ -24,6 +24,7 @@ import com.google.common.truth.ExpectFailure; import edu.wpi.first.units.TemperatureUnit; import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.junit.jupiter.api.Test; @@ -366,4 +367,73 @@ public void isWithin_rankineActual_notWithinTolerance_throws() { ExpectFailure.assertThat(e).factValue("but was").matches("520\\.17.*Rankine"); ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.1.*Fahrenheit"); } + + @Test + public void isWithin_centimeterTolerance_withinTolerance_doesNotThrow() { + Distance expected = Meters.of(28.13); + Distance actual = Meters.of(28.14); + Distance tolerance = Centimeters.of(2); + + assertThat(actual).isWithin(tolerance).of(expected); + } + + @Test + public void isWithin_centimeterTolerance_notWithinTolerance_throws() { + Distance expected = Meters.of(28.13); + Distance actual = Meters.of(28.16); + Distance tolerance = Centimeters.of(2); + + AssertionError e = + assertThrows( + AssertionError.class, () -> assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e).factValue("expected").matches("28\\.13.*Meter"); + ExpectFailure.assertThat(e).factValue("but was").matches("28\\.16.*Meter"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("2.*Centimeter"); + } + + @Test + public void isWithin_centimeterActual_withinTolerance_doesNotThrow() { + Distance expected = Meters.of(28.13); + Distance actual = Centimeters.of(2814); + Distance tolerance = Meters.of(0.02); + + assertThat(actual).isWithin(tolerance).of(expected); + } + + @Test + public void isWithin_centimeterActual_notWithinTolerance_throws() { + Distance expected = Meters.of(28.13); + Distance actual = Centimeters.of(2816); + Distance tolerance = Meters.of(0.02); + + AssertionError e = + assertThrows( + AssertionError.class, () -> assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e).factValue("expected").matches("28\\.13.*Meter"); + ExpectFailure.assertThat(e).factValue("but was").matches("2816.*Centimeter"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.02.*Meter"); + } + + @Test + public void isWithin_centimeterExpected_withinTolerance_doesNotThrow() { + Distance expected = Centimeters.of(2813); + Distance actual = Meters.of(28.14); + Distance tolerance = Meters.of(0.02); + + assertThat(actual).isWithin(tolerance).of(expected); + } + + @Test + public void isWithin_centimeterExpected_notWithinTolerance_throws() { + Distance expected = Centimeters.of(2813); + Distance actual = Meters.of(28.16); + Distance tolerance = Meters.of(0.02); + + AssertionError e = + assertThrows( + AssertionError.class, () -> assertThat(actual).isWithin(tolerance).of(expected)); + ExpectFailure.assertThat(e).factValue("expected").matches("2813.*Centimeter"); + ExpectFailure.assertThat(e).factValue("but was").matches("28\\.16.*Meter"); + ExpectFailure.assertThat(e).factValue("outside tolerance").matches("0\\.02.*Meter"); + } } From 78c4a549eac7abf21a3db13a9d7f15579de0605e Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Wed, 10 Jun 2026 17:14:00 -0700 Subject: [PATCH 13/19] Add a task that creates a tar.gz of the javadoc (#155) This will make it easier to copy new Javadoc to the website for future releases. --- MAINTAINERS.md | 5 +++- build.gradle | 30 ++++++++++++++++++- .../main/groovy/publishing-conventions.gradle | 2 +- settings.gradle | 1 + site/javadoc/latest/index.j2.html | 28 +++++++++++++++++ site/javadoc/stylesheet.css | 6 ++++ 6 files changed, 69 insertions(+), 3 deletions(-) create mode 100644 site/javadoc/latest/index.j2.html create mode 100644 site/javadoc/stylesheet.css diff --git a/MAINTAINERS.md b/MAINTAINERS.md index 084b30da..38523012 100644 --- a/MAINTAINERS.md +++ b/MAINTAINERS.md @@ -16,4 +16,7 @@ Before publishing to Maven Central, consider publishing to Maven Local. 9. Decrypt `lib2813-maven-publishing.tar.gz.gpg` 10. Copy the last five lines of the decrypted file to your personal `gradle.properties` file 11. Run `./gradlew publishToMavenCentral` -12. Celebrate! +12. Generate Javadoc by running `./gradlew archiveJavadoc` +13. Copy `build/docs/javadoc/javadoc.tar.gz` to the web site and extract it +14. Update the 'latest' symlink to point to the latest javadoc +15. Celebrate! diff --git a/build.gradle b/build.gradle index ce034fb9..1af37e5d 100644 --- a/build.gradle +++ b/build.gradle @@ -25,4 +25,32 @@ allprojects { } } } -} \ No newline at end of file +} + +tasks.register('archiveJavadoc', Tar) { + archiveFileName = 'javadoc.tar.gz' + compression = Compression.GZIP + destinationDirectory = layout.buildDirectory.dir("docs/javadoc") + + from(tasks.named('generateJavadocIndex')) + + subprojects.forEach { sub -> + sub.tasks.withType(Javadoc).configureEach { javadocTask -> + from(javadocTask.destinationDir) { + into sub.name + } + } + } +} + +tasks.register('generateJavadocIndex', Copy) { + from 'site/javadoc/latest' + include 'index.j2.html' + into layout.buildDirectory.dir("docs/javadoc") + + // Rename the file during the copy process + rename { 'index.html' } + + // This performs the variable substitution + expand([version: gradle.lib_version]) +} diff --git a/buildSrc/src/main/groovy/publishing-conventions.gradle b/buildSrc/src/main/groovy/publishing-conventions.gradle index 3d9d167f..3adba0ec 100644 --- a/buildSrc/src/main/groovy/publishing-conventions.gradle +++ b/buildSrc/src/main/groovy/publishing-conventions.gradle @@ -3,7 +3,7 @@ plugins { } group = 'com.team2813.lib2813' -version = "2.0.0" +version = gradle.lib_version mavenPublishing { diff --git a/settings.gradle b/settings.gradle index 226471d5..9470d2f0 100644 --- a/settings.gradle +++ b/settings.gradle @@ -8,5 +8,6 @@ */ rootProject.name = 'lib2813' +gradle.ext.lib_version = "2.0.0" include 'core', 'limelight', 'vision', 'testing' include 'vendor:ctre', 'vendor:rev' diff --git a/site/javadoc/latest/index.j2.html b/site/javadoc/latest/index.j2.html new file mode 100644 index 00000000..45b26b2b --- /dev/null +++ b/site/javadoc/latest/index.j2.html @@ -0,0 +1,28 @@ + + + + + + lib2813 2.0.0 Javadoc + + + + +

+

lib2813 {{ version }} Javadoc

+
+
+ +
+
+

Copyright 2023-2026 Prospect Robotics SWENext Club

+
+ + diff --git a/site/javadoc/stylesheet.css b/site/javadoc/stylesheet.css new file mode 100644 index 00000000..d21912ae --- /dev/null +++ b/site/javadoc/stylesheet.css @@ -0,0 +1,6 @@ +body > header { + --accent-bg: #4D7A97; +} +:root { + --accent: #4D7A97; +} From 547a265dde5c5e2881679e13b807b69178c05fe4 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Wed, 10 Jun 2026 17:17:54 -0700 Subject: [PATCH 14/19] MultiPhotonPoseEstimator improvements (#140) **New Builder Methods** - Add `withPoseFilter()` - Poses that are not within the field are filtered out - Add `withMultiTagFallbackStrategy()` **Logging Support** - Add an overload for `processAllUnreadResults()` that allows the caller to be notified of rejected poses --- .../vision/MultiPhotonPoseEstimator.java | 145 +++++++++++-- .../vision/MultiPhotonPoseEstimatorTest.java | 190 ++++++++++++++++-- .../lib2813/vision/ReefscapeAprilTag.java | 68 +++++++ vision/vendordeps/photonlib.json | 14 +- 4 files changed, 377 insertions(+), 40 deletions(-) create mode 100644 vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 18f263a8..e7dab816 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -34,6 +34,10 @@ import java.util.Map; import java.util.Objects; import java.util.Optional; +import java.util.function.BiConsumer; +import java.util.function.Consumer; +import java.util.function.Predicate; +import java.util.stream.Collectors; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -63,7 +67,10 @@ */ public class MultiPhotonPoseEstimator implements AutoCloseable { private final List> cameraWrappers; + private final Predicate isPoseValid; + private final PhotonPoseEstimator.PoseStrategy fallbackStrategy; private PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + private boolean needHeadingData; /** A builder for {@code MultiPhotonPoseEstimator}. */ public static final class Builder { @@ -71,6 +78,9 @@ public static final class Builder { private final AprilTagFieldLayout aprilTagFieldLayout; private final NetworkTableInstance ntInstance; private final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + private PhotonPoseEstimator.PoseStrategy fallbackStrategy = + PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY; + private Predicate isPoseValid = pose -> true; Builder( NetworkTableInstance ntInstance, @@ -98,6 +108,39 @@ public Builder addCamera(C camera) { return this; } + /** + * Sets the filter to use deciding which estimates to consider. + * + * @param isPoseValid A predicate that returns {@code true} if the pose should be considered + * valid. + * @return Builder instance. + * @since 2.1.0 + */ + public Builder withPoseFilter(Predicate isPoseValid) { + this.isPoseValid = isPoseValid; + return this; + } + + /** + * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. + * Must NOT be {@link PhotonPoseEstimator.PoseStrategy#MULTI_TAG_PNP_ON_COPROCESSOR} or {@link + * PhotonPoseEstimator.PoseStrategy#MULTI_TAG_PNP_ON_RIO}. + * + *

If this is not called, {@link PhotonPoseEstimator.PoseStrategy#LOWEST_AMBIGUITY} will be + * used. + * + * @param strategy the strategy to set + * @return Builder instance. + * @since 2.1.0 + */ + public Builder withMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy strategy) { + if (isMultiTagStrategy(strategy)) { + throw new IllegalArgumentException("Fallback strategy cannot be " + strategy); + } + fallbackStrategy = strategy; + return this; + } + /** Builds a configured MultiPhotonPoseEstimator. */ public MultiPhotonPoseEstimator build() { return new MultiPhotonPoseEstimator<>(this); @@ -150,6 +193,21 @@ public static Builder builder( * @param simVisionSystem The simulated visual system. */ public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { + addCamerasToSimulator(simVisionSystem, (camera, simCamera) -> {}); + } + + /** + * Adds all cameras to a simulated vision system, updating camera properties + * + *

Note that the robot code is responsible for calling {@link VisionSystemSim#update(Pose2d)} + * or {@link VisionSystemSim#update(Pose3d)} in {@code simulationPeriodic()}. + * + * @param simVisionSystem The simulated visual system. + * @param simCameraUpdater Callback that is called for each new simulated camera. + * @since 2.1.0 + */ + public void addCamerasToSimulator( + VisionSystemSim simVisionSystem, BiConsumer simCameraUpdater) { // Validate all inputs and create SimCameraProperties for each camera. Map cameraNameToSimProperties = cameraWrappers.stream() @@ -161,6 +219,7 @@ public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { wrapper -> { SimCameraProperties cameraProps = cameraNameToSimProperties.get(wrapper.camera.name()); PhotonCameraSim simCamera = new PhotonCameraSim(wrapper.photonCamera, cameraProps); + simCameraUpdater.accept(wrapper.camera, simCamera); simVisionSystem.addCamera(simCamera, wrapper.estimator.getRobotToCameraTransform()); }); } @@ -233,10 +292,13 @@ public void close() { /** Creates an instance using values from a {@code Builder}. */ private MultiPhotonPoseEstimator(Builder builder) { poseEstimatorStrategy = builder.poseEstimatorStrategy; + fallbackStrategy = builder.fallbackStrategy; + isPoseValid = poseIsInField(builder.aprilTagFieldLayout).and(builder.isPoseValid); cameraWrappers = builder.cameras.values().stream() .map(camera -> createCameraWrapper(builder, camera)) .collect(toCollection(ArrayList::new)); + needHeadingData = calculateNeedHeadingData(); } /** @@ -251,6 +313,7 @@ private static PhotonCameraWrapper createCameraWrapper( PhotonPoseEstimator estimator = new PhotonPoseEstimator( builder.aprilTagFieldLayout, builder.poseEstimatorStrategy, camera.robotToCamera()); + estimator.setMultiTagFallbackStrategy(builder.fallbackStrategy); // Create NetworkTables publishers for 1) the position of the camera relative to the robot and // 2) the estimated position provided by the camera. @@ -283,6 +346,7 @@ public void setPrimaryStrategy(PhotonPoseEstimator.PoseStrategy poseStrategy) { if (!poseStrategy.equals(poseEstimatorStrategy)) { cameraWrappers.forEach(wrapper -> wrapper.estimator.setPrimaryStrategy(poseStrategy)); poseEstimatorStrategy = poseStrategy; + needHeadingData = calculateNeedHeadingData(); } } @@ -292,10 +356,7 @@ public void setPrimaryStrategy(PhotonPoseEstimator.PoseStrategy poseStrategy) { * @return {@code true} if the pose strategy is documented to require addHeadingData(). */ public boolean poseStrategyRequiresHeadingData() { - return switch (poseEstimatorStrategy) { - case PNP_DISTANCE_TRIG_SOLVE, CONSTRAINED_SOLVEPNP -> true; - default -> false; - }; + return needHeadingData; } /** @@ -361,23 +422,45 @@ public void resetHeadingData(double timestampSeconds, Rotation3d heading) { } /** - * Sends all unread robot-pose estimations from all cameras to the provided consumer. + * Sends all validated unread robot-pose estimations from all cameras to the provided consumer. * - *

This method is supposed to be called from a routine updating drive-train pose with pose - * estimates from the photon vision cameras. + *

This method should be called from a routine updating drive-train pose with pose estimates + * from the photon vision cameras. * - * @param poseEstimateConsumer Functional interface for consuming computed pose estimates. + * @param poseEstimateConsumer Consumer for validated pose estimates. */ public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer) { + processAllUnreadResults(poseEstimateConsumer, pose -> {}); + } + + /** + * Sends all unread robot-pose estimations from all cameras to the provided consumers. + * + *

This method should be called from a routine updating drive-train pose with pose estimates + * from the photon vision cameras. + * + * @param poseEstimateConsumer Consumer for validated pose estimates. + * @param rejectedPoseConsumer Consumer for rejected pose estimates. + * @since 2.1.0 + */ + public void processAllUnreadResults( + PoseEstimateConsumer poseEstimateConsumer, + Consumer rejectedPoseConsumer) { for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { - List poses = + Map> poses = cameraWrapper.photonCamera.getAllUnreadResults().stream() .map(cameraWrapper.estimator::update) // PhotonPipelineResult -> EstimatedRobotPose .flatMap(Optional::stream) // Convert Stream> -> Stream

- .toList(); + .collect(Collectors.partitioningBy(isPoseValid)); + + List validatedPoses = poses.get(Boolean.TRUE); + for (EstimatedRobotPose pose : validatedPoses) { + poseEstimateConsumer.addEstimatedRobotPose(pose, cameraWrapper.camera); + } + cameraWrapper.robotPosePublisher.publish(validatedPoses); - poses.forEach(pose -> poseEstimateConsumer.addEstimatedRobotPose(pose, cameraWrapper.camera)); - cameraWrapper.robotPosePublisher.publish(poses); + List rejectedPoses = poses.get(Boolean.FALSE); + rejectedPoses.forEach(rejectedPoseConsumer); } } @@ -386,4 +469,42 @@ public void close() { cameraWrappers.forEach(PhotonCameraWrapper::close); cameraWrappers.clear(); } + + private static boolean poseStrategyRequiresHeadingData( + PhotonPoseEstimator.PoseStrategy strategy) { + return switch (strategy) { + case PNP_DISTANCE_TRIG_SOLVE, CONSTRAINED_SOLVEPNP -> true; + default -> false; + }; + } + + private static boolean isMultiTagStrategy(PhotonPoseEstimator.PoseStrategy strategy) { + return switch (strategy) { + case MULTI_TAG_PNP_ON_COPROCESSOR, MULTI_TAG_PNP_ON_RIO -> true; + default -> false; + }; + } + + private boolean calculateNeedHeadingData() { + if (poseStrategyRequiresHeadingData(poseEstimatorStrategy)) { + return true; + } + return isMultiTagStrategy(poseEstimatorStrategy) + && poseStrategyRequiresHeadingData(fallbackStrategy); + } + + /** Creates a predicate that determines if a pose is inside the field. */ + private static Predicate poseIsInField( + AprilTagFieldLayout aprilTagFieldLayout) { + return pose -> { + Pose3d estimate = pose.estimatedPose; + double x = estimate.getX(); + double y = estimate.getY(); + + return x >= 0.0 + && x <= aprilTagFieldLayout.getFieldLength() + && y >= 0.0 + && y <= aprilTagFieldLayout.getFieldWidth(); + }; + } } diff --git a/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java index 447ad23b..5399a9c9 100644 --- a/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java +++ b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java @@ -16,54 +16,202 @@ package com.team2813.lib2813.vision; import static com.google.common.truth.Truth.assertThat; +import static com.team2813.lib2813.testing.truth.Pose3dSubject.assertThat; +import static edu.wpi.first.units.Units.Meters; +import com.team2813.lib2813.testing.junit.jupiter.InitWPILib; import com.team2813.lib2813.testing.junit.jupiter.ProvideUniqueNetworkTableInstance; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.measure.Distance; +import java.util.ArrayList; import java.util.List; +import java.util.function.Consumer; +import java.util.stream.Stream; import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; import org.junit.jupiter.params.provider.EnumSource; +import org.junit.jupiter.params.provider.MethodSource; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; /** Tests for {@link MultiPhotonPoseEstimator}. */ @ProvideUniqueNetworkTableInstance +@InitWPILib class MultiPhotonPoseEstimatorTest { - private static final double FIELD_LENGTH = 17.548; - private static final double FIELD_WIDTH = 8.052; - private static final int REEFSCAPE_APRIL_TAG_ID = 7; - private static final Pose3d REEFSCAPE_APRIL_TAG_POSE = - new Pose3d( - new Translation3d(13.890498, 4.0259, 0.308102), - new Rotation3d(new Quaternion(1.0, 0.0, 0.0, 0.0))); + // Place the camera in the center of the robot, ~17.1cm up, facing forward and up. private static final Transform3d FRONT_CAMERA_TRANSFORM = - new Transform3d( - 0.1688157406, - 0.2939800826, - 0.1708140348, - new Rotation3d(0, -0.1745329252, -0.5235987756)); + new Transform3d(0, 0, 0.1708140348, new Rotation3d(0, -0.1745329252, 0)); - private static final Camera FRONT_CAMERA = new Camera("front", FRONT_CAMERA_TRANSFORM); + private static final Camera FRONT_CAMERA = + new Camera("front", FRONT_CAMERA_TRANSFORM, SimCameraProperties::PERFECT_90DEG); @ParameterizedTest @EnumSource(value = PoseStrategy.class) void getPrimaryStrategy(PoseStrategy poseStrategy, NetworkTableInstance ntInstance) { try (var estimator = - MultiPhotonPoseEstimator.builder(ntInstance, createFieldLayout(), poseStrategy) + MultiPhotonPoseEstimator.builder( + ntInstance, ReefscapeAprilTag.createFieldLayout(), poseStrategy) .addCamera(FRONT_CAMERA) .build()) { assertThat(estimator.getPrimaryStrategy()).isEqualTo(poseStrategy); } } - private static AprilTagFieldLayout createFieldLayout() { - List aprilTags = - List.of(new AprilTag(REEFSCAPE_APRIL_TAG_ID, REEFSCAPE_APRIL_TAG_POSE)); - return new AprilTagFieldLayout(aprilTags, FIELD_LENGTH, FIELD_WIDTH); + private record PoseTestData(Pose3d robotPose, ReefscapeAprilTag aprilTag) {} + + private static Stream posesInField() { + return Stream.of( + facingAprilTag(ReefscapeAprilTag.RED_REEF_CENTER, Meters.of(1)), + facingAprilTag(ReefscapeAprilTag.BLUE_REEF_CENTER, Meters.of(1)), + facingAprilTag(ReefscapeAprilTag.RED_PROCESSOR, Meters.of(0.5)), + facingAprilTag(ReefscapeAprilTag.BLUE_PROCESSOR, Meters.of(0.5))); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("posesInField") + void processAllUnreadResults_estimatedPoseInField( + String testName, PoseTestData testData, NetworkTableInstance ntInstance) { + AprilTagFieldLayout fieldLayout = ReefscapeAprilTag.createFieldLayout(testData.aprilTag); + VisionSystemSim visionSystemSim = new VisionSystemSim("test"); + visionSystemSim.addAprilTags(fieldLayout); + + double z = testData.aprilTag.toAprilTag().pose.getZ(); + Camera camera = + new Camera( + "front", + new Transform3d(0, 0, z, FRONT_CAMERA_TRANSFORM.getRotation()), + SimCameraProperties::PERFECT_90DEG); + + try (var estimator = + MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY) + .addCamera(camera) + .build()) { + estimator.addCamerasToSimulator( + visionSystemSim, + (c, simCamera) -> { + simCamera.enableRawStream(false); + simCamera.enableProcessedStream(false); + }); + visionSystemSim.update(testData.robotPose); + + var estimateCollector = new EstimateCollector(); + var rejectedPoseCollector = new RejectedPoseCollector(); + + // Call the method under test + estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector); + + assertThat(estimateCollector.estimates).hasSize(1); + assertThat(rejectedPoseCollector.rejectedPoses).isEmpty(); + assertThat(estimateCollector.estimates.get(0).estimatedPose) + .isWithin(0.01) + .of(testData.robotPose); + } + } + + private static Stream posesOutOfField() { + return Stream.of( + facingAprilTag(ReefscapeAprilTag.RED_REEF_CENTER, Meters.of(4)), + facingAprilTag(ReefscapeAprilTag.BLUE_REEF_CENTER, Meters.of(4)), + facingAprilTag(ReefscapeAprilTag.RED_PROCESSOR, Meters.of(8.1)), + facingAprilTag(ReefscapeAprilTag.BLUE_PROCESSOR, Meters.of(8.1))); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("posesOutOfField") + void processAllUnreadResults_estimatedPoseOutsideField( + String testName, PoseTestData testData, NetworkTableInstance ntInstance) { + AprilTagFieldLayout fieldLayout = ReefscapeAprilTag.createFieldLayout(testData.aprilTag); + VisionSystemSim visionSystemSim = new VisionSystemSim("test"); + visionSystemSim.addAprilTags(fieldLayout); + + double z = testData.aprilTag.toAprilTag().pose.getZ(); + Camera camera = + new Camera( + "front", + new Transform3d(0, 0, z, FRONT_CAMERA_TRANSFORM.getRotation()), + SimCameraProperties::PERFECT_90DEG); + + try (var estimator = + MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY) + .addCamera(camera) + .build()) { + estimator.addCamerasToSimulator( + visionSystemSim, + (c, simCamera) -> { + simCamera.enableRawStream(false); + simCamera.enableProcessedStream(false); + }); + visionSystemSim.update(testData.robotPose); + + var estimateCollector = new EstimateCollector(); + var rejectedPoseCollector = new RejectedPoseCollector(); + + // Call the method under test + estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector); + + assertThat(estimateCollector.estimates).isEmpty(); + assertThat(rejectedPoseCollector.rejectedPoses).hasSize(1); + assertThat(targetsUsed(rejectedPoseCollector.rejectedPoses.get(0))) + .containsExactly(testData.aprilTag.id()); + } + } + + private static List targetsUsed(EstimatedRobotPose estimatedPose) { + return estimatedPose.targetsUsed.stream().map(PhotonTrackedTarget::getFiducialId).toList(); + } + + /** Creates test data with a position that is the given distance away from the given AprilTag. */ + private static Arguments facingAprilTag(ReefscapeAprilTag tag, Distance distanceFromTag) { + Pose2d closestTagPose = tag.toAprilTag().pose.toPose2d(); + Rotation2d tagRotation = closestTagPose.getRotation(); + double distance = distanceFromTag.in(Meters); + Translation2d translation = + new Translation2d(distance * tagRotation.getCos(), distance * tagRotation.getSin()); + + Pose2d robotPose = + new Pose2d( + closestTagPose.getTranslation().plus(translation), + tagRotation.rotateBy(Rotation2d.k180deg)); + + var testName = String.format("%.1fmFrom%s", distance, toCamelCase(tag.name())); + return Arguments.of(testName, new PoseTestData(new Pose3d(robotPose), tag)); + } + + private static String toCamelCase(String s) { + StringBuilder camelCaseString = new StringBuilder(); + for (String part : s.split("_")) { + camelCaseString.append(part.substring(0, 1).toUpperCase()); + camelCaseString.append(part.substring(1).toLowerCase()); + } + return camelCaseString.toString(); + } + + private static class EstimateCollector implements PoseEstimateConsumer { + final List estimates = new ArrayList<>(); + + @Override + public void addEstimatedRobotPose(EstimatedRobotPose estimatedPose, Camera camera) { + assertThat(camera.name()).isEqualTo(FRONT_CAMERA.name()); + estimates.add(estimatedPose); + } + } + + private static class RejectedPoseCollector implements Consumer { + final List rejectedPoses = new ArrayList<>(); + + @Override + public void accept(EstimatedRobotPose estimatedRobotPose) { + rejectedPoses.add(estimatedRobotPose); + } } } diff --git a/vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java b/vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java new file mode 100644 index 00000000..6c9169b0 --- /dev/null +++ b/vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java @@ -0,0 +1,68 @@ +/* +Copyright 2026 Prospect Robotics SWENext Club + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +package com.team2813.lib2813.vision; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import java.util.List; +import java.util.Optional; + +/** Contains AprilTags for the Reefscape Welded field. */ +enum ReefscapeAprilTag { + RED_REEF_CENTER(7), + BLUE_REEF_CENTER(18), + RED_PROCESSOR(3), + BLUE_PROCESSOR(16); + + /** Creates a (mutable) AprilTag for this enum value. */ + AprilTag toAprilTag() { + Optional tagPose = memoizedFieldLayout().getTagPose(tagId); + return new AprilTag(tagId, tagPose.orElseThrow()); + } + + public int id() { + return tagId; + } + + /** Creates a field layout for Reefscape Welded with the origin set to zero. */ + static AprilTagFieldLayout createFieldLayout() { + return AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + } + + /** Creates a field layout with the dimensions of Reefscape Welded and the given tag. */ + static AprilTagFieldLayout createFieldLayout(ReefscapeAprilTag tag) { + var fieldLayout = memoizedFieldLayout(); + return new AprilTagFieldLayout( + List.of(tag.toAprilTag()), fieldLayout.getFieldLength(), fieldLayout.getFieldWidth()); + } + + private static AprilTagFieldLayout possiblyNullFieldLayout; + + private static AprilTagFieldLayout memoizedFieldLayout() { + if (possiblyNullFieldLayout == null) { + possiblyNullFieldLayout = createFieldLayout(); + } + return possiblyNullFieldLayout; + } + + ReefscapeAprilTag(int tagId) { + this.tagId = tagId; + } + + private final int tagId; +} diff --git a/vision/vendordeps/photonlib.json b/vision/vendordeps/photonlib.json index a1bc5a57..dbdc1849 100644 --- a/vision/vendordeps/photonlib.json +++ b/vision/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1", + "version": "v2026.2.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.2.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1", + "version": "v2026.2.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.2.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1" + "version": "v2026.2.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1" + "version": "v2026.2.2" } ] -} \ No newline at end of file +} From 226336fb2ce4a691a700c005af0a5a8023cea87d Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Wed, 10 Jun 2026 17:19:12 -0700 Subject: [PATCH 15/19] Address issues found by IntelliJ IDEA inspections (#158) - Use `instanceof` pattern matching - Make classes static that can be static - Fix Javadoc errors --- .../com/team2813/lib2813/util/InvalidCanIdException.java | 7 ++++--- .../team2813/lib2813/util/BuildConstantsPublisherTest.java | 7 ++++--- .../com/team2813/lib2813/limelight/LimelightHelpers.java | 5 +---- .../team2813/lib2813/vendor/ctre/motor/TalonFXWrapper.java | 2 +- 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/core/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java b/core/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java index 0d06a6ac..86aecaf4 100644 --- a/core/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java +++ b/core/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java @@ -41,9 +41,10 @@ public int getCanId() { @Override public boolean equals(Object o) { - if (!(o instanceof InvalidCanIdException)) return false; - InvalidCanIdException other = (InvalidCanIdException) o; - return getMessage().equals(other.getMessage()); + if (o instanceof InvalidCanIdException other) { + return getMessage().equals(other.getMessage()); + } + return false; } @Override diff --git a/core/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java b/core/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java index cba34401..a2db1dad 100644 --- a/core/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java +++ b/core/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java @@ -41,7 +41,7 @@ public class BuildConstantsPublisherTest { // Fake constants copied and adapted from this article // https://docs.wpilib.org/en/stable/docs/software/advanced-gradlerio/deploy-git-data.html - public final class FakeBuildConstants { + public static final class FakeBuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2813Robot"; public static final String VERSION = "unspecified"; @@ -60,9 +60,10 @@ private FakeBuildConstants() {} * A Truth {@link Subject} for asserting properties of strings that should parse as {@link * LocalDateTime}. * - *

Composed with the help of Gemini: https://g.co/gemini/share/d8db68a8fbaf + *

Composed with the help of Gemini: thread */ - private class DateTimeStringSubject extends Subject { + private static class DateTimeStringSubject extends Subject { private final String actual; private DateTimeStringSubject(FailureMetadata metadata, String actual) { diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java index 7696ce78..413e78ae 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java @@ -1645,10 +1645,7 @@ public static double[] getPythonScriptData(String limelightName) { /** Asynchronously take snapshot. */ public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync( - () -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); + return CompletableFuture.supplyAsync(() -> SYNCH_TAKESNAPSHOT(tableName, snapshotName)); } private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { diff --git a/vendor/ctre/src/main/java/com/team2813/lib2813/vendor/ctre/motor/TalonFXWrapper.java b/vendor/ctre/src/main/java/com/team2813/lib2813/vendor/ctre/motor/TalonFXWrapper.java index 62f2cc37..879cf187 100644 --- a/vendor/ctre/src/main/java/com/team2813/lib2813/vendor/ctre/motor/TalonFXWrapper.java +++ b/vendor/ctre/src/main/java/com/team2813/lib2813/vendor/ctre/motor/TalonFXWrapper.java @@ -190,7 +190,7 @@ public TalonFX motor() { *

  • Brake: The motor stops applying an input, and actively opposes its inertia. * * - * @param mode + * @param mode the state of the motor controller bridge when output is neutral or disabled */ public void setNeutralMode(NeutralModeValue mode) { motor.setNeutralMode(mode); From fb638e23d09c5afdc4174fff56dcedaf4be2e261 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Wed, 10 Jun 2026 17:40:14 -0700 Subject: [PATCH 16/19] Update Translation2dSubject and Translation3dSubject to use distance (#156) --- .../lib2813/limelight/LimelightTestCase.java | 8 ++-- .../lib2813/testing/truth/Pose2dSubject.java | 20 +++++++-- .../lib2813/testing/truth/Pose3dSubject.java | 20 +++++++-- .../testing/truth/Rotation2dSubject.java | 21 +++++++++- .../testing/truth/Rotation3dSubject.java | 31 ++++++++++++-- .../lib2813/testing/truth/SubjectHelper.java | 41 +++++++++++++++++++ .../testing/truth/Translation2dSubject.java | 22 ++++++++-- .../testing/truth/Translation3dSubject.java | 23 +++++++++-- .../truth/Translation2dSubjectTest.java | 26 +++++++++++- .../truth/Translation3dSubjectTest.java | 26 +++++++++++- 10 files changed, 213 insertions(+), 25 deletions(-) create mode 100644 testing/src/main/java/com/team2813/lib2813/testing/truth/SubjectHelper.java diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java index 3a0c5c57..a9513c58 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java @@ -141,7 +141,7 @@ public final void presentTest1() throws Exception { var blueEstimate = locationalData.getBotPoseEstimateBlue().get(); assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0); var expectedPoseEstimate = new Pose2d(15.62, 4.52, rotation.toRotation2d()); - assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); + assertThat(blueEstimate.pose()).isWithin(0.01).of(expectedPoseEstimate); assertThat(locationalData.getBotPoseEstimateRed()).isPresent(); var redEstimate = locationalData.getBotPoseEstimateRed().get(); @@ -173,7 +173,7 @@ public final void presentTest2() throws Exception { Rotation3d rotation = new Rotation3d(Math.toRadians(-5.18), Math.toRadians(-24.32), Math.toRadians(-164.64)); Pose3d expectedPose = new Pose3d(7.469, 0.81, 1.01, rotation); - assertThat(actualPose).isWithin(0.005).of(expectedPose); + assertThat(actualPose).isWithin(0.01).of(expectedPose); assertThat(locationalData.getBotPoseEstimateBlue()).isPresent(); var blueEstimate = locationalData.getBotPoseEstimateBlue().get(); @@ -201,7 +201,7 @@ public final void getBotposeBlue() throws Exception { Pose3d actualPose = botposeBlue.get(); Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(-123.49)); Pose3d expectedPose = new Pose3d(4.72, 5.20, 0, expectedRotation); - assertThat(actualPose).isWithin(0.005).of(expectedPose); + assertThat(actualPose).isWithin(0.01).of(expectedPose); } @Test @@ -218,7 +218,7 @@ public final void getBotposeRed() throws Exception { Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(56.51)); Pose3d expectedPose = new Pose3d(11.83, 3.01, 0, expectedRotation); - assertThat(actualPose).isWithin(0.005).of(expectedPose); + assertThat(actualPose).isWithin(0.01).of(expectedPose); } @Test diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java index a5e56dab..952a6296 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java @@ -15,8 +15,10 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Fact.fact; import static com.google.common.truth.Fact.simpleFact; import static com.google.common.truth.Truth.assertAbout; +import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance; import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; @@ -53,11 +55,23 @@ private Pose2dSubject(FailureMetadata failureMetadata, @Nullable Pose2d subject) // User-defined test assertion SPI below this point public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { + return new TolerantComparison<>() { @Override public void of(Pose2d expected) { - translation().isWithin(tolerance).of(expected.getTranslation()); - rotation().isWithin(tolerance).of(expected.getRotation()); + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + Pose2d actual = nonNullActualPose(); + if (!Translation2dSubject.equalWithinTolerance( + expected.getTranslation(), actual.getTranslation(), tolerance) + || !Rotation2dSubject.equalWithinTolerance( + expected.getRotation(), actual.getRotation(), tolerance)) { + failWithoutActual( + fact("expected", expected), + fact("but was", actual), + fact("outside tolerance", tolerance)); + } } }; } diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java index fd9ada6e..2c292829 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java @@ -15,8 +15,10 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Fact.fact; import static com.google.common.truth.Fact.simpleFact; import static com.google.common.truth.Truth.assertAbout; +import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance; import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; @@ -53,11 +55,23 @@ private Pose3dSubject(FailureMetadata failureMetadata, @Nullable Pose3d subject) // User-defined test assertion SPI below this point public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { + return new TolerantComparison<>() { @Override public void of(Pose3d expected) { - translation().isWithin(tolerance).of(expected.getTranslation()); - rotation().isWithin(tolerance).of(expected.getRotation()); + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + Pose3d actual = nonNullActualPose(); + if (!Translation3dSubject.equalWithinTolerance( + expected.getTranslation(), actual.getTranslation(), tolerance) + || !Rotation3dSubject.equalWithinTolerance( + expected.getRotation(), actual.getRotation(), tolerance)) { + failWithoutActual( + fact("expected", expected), + fact("but was", actual), + fact("outside tolerance", tolerance)); + } } }; } diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java index ce81bb6c..f791ad22 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java @@ -15,8 +15,10 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Fact.fact; import static com.google.common.truth.Fact.simpleFact; import static com.google.common.truth.Truth.assertAbout; +import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance; import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; @@ -50,14 +52,29 @@ private Rotation2dSubject(FailureMetadata failureMetadata, @Nullable Rotation2d // User-defined test assertion SPI below this point public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { + return new TolerantComparison<>() { @Override public void of(Rotation2d expected) { - getRadians().isWithin(tolerance).of(expected.getRadians()); + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) { + failWithoutActual( + fact("expected", expected), + fact("but was", actual), + fact("outside tolerance", tolerance)); + } } }; } + static boolean equalWithinTolerance( + Rotation2d rotation1, Rotation2d rotation2, double tolerance) { + double distance = rotation1.getRadians() - rotation2.getRadians(); + return Math.abs(distance) < tolerance; + } + public TolerantComparison isNotWithin(double tolerance) { return new TolerantComparison() { @Override diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java index 9e6e487d..694ffeb0 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java @@ -15,8 +15,10 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Fact.fact; import static com.google.common.truth.Fact.simpleFact; import static com.google.common.truth.Truth.assertAbout; +import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance; import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; @@ -50,16 +52,37 @@ private Rotation3dSubject(FailureMetadata failureMetadata, @Nullable Rotation3d // User-defined test assertion SPI below this point public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { + return new TolerantComparison<>() { @Override public void of(Rotation3d expected) { - x().isWithin(tolerance).of(expected.getX()); // roll, in radians - y().isWithin(tolerance).of(expected.getY()); // pitch, in radians - z().isWithin(tolerance).of(expected.getZ()); // yaw, in radians + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) { + failWithoutActual( + fact("expected", expected), + fact("but was", actual), + fact("outside tolerance", tolerance)); + } } }; } + static boolean equalWithinTolerance( + Rotation3d rotation1, Rotation3d rotation2, double tolerance) { + double distance = rotation1.getX() - rotation2.getX(); // roll, in radians + if (Math.abs(distance) >= tolerance) { + return false; + } + distance = rotation1.getY() - rotation2.getY(); // pitch, in radians + if (Math.abs(distance) >= tolerance) { + return false; + } + distance = rotation1.getZ() - rotation2.getZ(); // yaw, in radians + return Math.abs(distance) < tolerance; + } + public void isZero() { if (!Rotation3d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/SubjectHelper.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/SubjectHelper.java new file mode 100644 index 00000000..95ec450a --- /dev/null +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/SubjectHelper.java @@ -0,0 +1,41 @@ +/* +Copyright 2026 Prospect Robotics SWENext Club + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +package com.team2813.lib2813.testing.truth; + +import static com.google.common.base.Preconditions.checkArgument; +import static java.lang.Double.doubleToLongBits; + +class SubjectHelper { + private static final long NEG_ZERO_BITS = doubleToLongBits(-0.0); + + /** + * Ensures that the given tolerance is a non-negative finite value, i.e. not {@code Double.NaN}, + * {@code Double.POSITIVE_INFINITY}, or negative, including {@code -0.0}. + */ + static void checkTolerance(double tolerance) { + checkArgument(!Double.isNaN(tolerance), "tolerance cannot be NaN"); + checkArgument(tolerance >= 0.0, "tolerance (%s) cannot be negative", tolerance); + checkArgument( + doubleToLongBits(tolerance) != NEG_ZERO_BITS, + "tolerance (%s) cannot be negative", + tolerance); + checkArgument(tolerance != Double.POSITIVE_INFINITY, "tolerance cannot be POSITIVE_INFINITY"); + } + + private SubjectHelper() { + throw new AssertionError("Not instantiable"); + } +} diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java index 8a96d4a2..846f97d8 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java @@ -15,8 +15,10 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Fact.fact; import static com.google.common.truth.Fact.simpleFact; import static com.google.common.truth.Truth.assertAbout; +import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance; import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; @@ -51,15 +53,29 @@ private Translation2dSubject(FailureMetadata failureMetadata, @Nullable Translat // User-defined test assertion SPI below this point public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { + return new TolerantComparison<>() { @Override public void of(Translation2d expected) { - x().isWithin(tolerance).of(expected.getX()); - y().isWithin(tolerance).of(expected.getY()); + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) { + failWithoutActual( + fact("expected", expected), + fact("but was", actual), + fact("outside tolerance", tolerance)); + } } }; } + static boolean equalWithinTolerance( + Translation2d translation1, Translation2d translation2, double tolerance) { + double distance = translation1.getDistance(translation2); + return Math.abs(distance) < tolerance; + } + public void isZero() { if (!Translation2d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java index 955ff8e6..4e21fadf 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java @@ -15,8 +15,10 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Fact.fact; import static com.google.common.truth.Fact.simpleFact; import static com.google.common.truth.Truth.assertAbout; +import static com.team2813.lib2813.testing.truth.SubjectHelper.checkTolerance; import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; @@ -51,16 +53,29 @@ private Translation3dSubject(FailureMetadata failureMetadata, @Nullable Translat // User-defined test assertion SPI below this point public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { + return new TolerantComparison<>() { @Override public void of(Translation3d expected) { - x().isWithin(tolerance).of(expected.getX()); - y().isWithin(tolerance).of(expected.getY()); - z().isWithin(tolerance).of(expected.getZ()); + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) { + failWithoutActual( + fact("expected", expected), + fact("but was", actual), + fact("outside tolerance", tolerance)); + } } }; } + static boolean equalWithinTolerance( + Translation3d translation1, Translation3d translation2, double tolerance) { + double distance = translation1.getDistance(translation2); + return Math.abs(distance) < tolerance; + } + public void isZero() { if (!Translation3d.kZero.equals(actual)) { failWithActual(simpleFact("expected to be zero")); diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation2dSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation2dSubjectTest.java index dc2b7ede..6b47d309 100644 --- a/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation2dSubjectTest.java +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation2dSubjectTest.java @@ -15,9 +15,11 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Truth.assertThat; import static org.junit.jupiter.api.Assertions.*; import edu.wpi.first.math.geometry.Translation2d; +import org.junit.jupiter.api.Test; import org.junit.jupiter.params.ParameterizedTest; import org.junit.jupiter.params.provider.ArgumentsSource; @@ -25,6 +27,28 @@ class Translation2dSubjectTest { private static final Translation2d TRANSLATION = new Translation2d(7.353, 0.706); + @Test + public void isWithin_nullActual_throws() { + Translation2d actual = null; + + AssertionError e = + assertThrows( + AssertionError.class, + () -> Translation2dSubject.assertThat(actual).isWithin(0.01).of(TRANSLATION)); + assertThat(e).hasMessageThat().contains(": null"); + } + + @Test + public void isWithin_nullExpected_throwsNullPointerException() { + Translation2d expected = null; + + NullPointerException e = + assertThrows( + NullPointerException.class, + () -> Translation2dSubject.assertThat(TRANSLATION).isWithin(0.01).of(expected)); + assertThat(e).hasMessageThat().contains("cannot be null"); + } + @ParameterizedTest @ArgumentsSource(Pose2dComponent.TranslationsArgumentsProvider.class) public void isWithin_valueWithinTolerance_doesNotThrow(Pose2dComponent component) { @@ -36,7 +60,7 @@ public void isWithin_valueWithinTolerance_doesNotThrow(Pose2dComponent component @ParameterizedTest @ArgumentsSource(Pose2dComponent.TranslationsArgumentsProvider.class) public void isWithin_valueNotWithinTolerance_throws(Pose2dComponent component) { - Translation2d closeTranslation = component.add(TRANSLATION, 0.011); + Translation2d closeTranslation = component.add(TRANSLATION, 0.016); assertThrows( AssertionError.class, diff --git a/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation3dSubjectTest.java b/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation3dSubjectTest.java index a11d2ad0..e41e5f23 100644 --- a/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation3dSubjectTest.java +++ b/testing/src/test/java/com/team2813/lib2813/testing/truth/Translation3dSubjectTest.java @@ -15,9 +15,11 @@ */ package com.team2813.lib2813.testing.truth; +import static com.google.common.truth.Truth.assertThat; import static org.junit.jupiter.api.Assertions.assertThrows; import edu.wpi.first.math.geometry.Translation3d; +import org.junit.jupiter.api.Test; import org.junit.jupiter.params.ParameterizedTest; import org.junit.jupiter.params.provider.ArgumentsSource; @@ -25,6 +27,28 @@ class Translation3dSubjectTest { private static final Translation3d TRANSLATION = new Translation3d(7.353, 0.706, 42.00); + @Test + public void isWithin_nullActual_throws() { + Translation3d actual = null; + + AssertionError e = + assertThrows( + AssertionError.class, + () -> Translation3dSubject.assertThat(actual).isWithin(0.01).of(TRANSLATION)); + assertThat(e).hasMessageThat().contains(": null"); + } + + @Test + public void isWithin_nullExpected_throwsNullPointerException() { + Translation3d expected = null; + + NullPointerException e = + assertThrows( + NullPointerException.class, + () -> Translation3dSubject.assertThat(TRANSLATION).isWithin(0.01).of(expected)); + assertThat(e).hasMessageThat().contains("cannot be null"); + } + @ParameterizedTest @ArgumentsSource(Pose3dComponent.TranslationsArgumentsProvider.class) public void isWithin_valueWithinTolerance_doesNotThrow(Pose3dComponent component) { @@ -36,7 +60,7 @@ public void isWithin_valueWithinTolerance_doesNotThrow(Pose3dComponent component @ParameterizedTest @ArgumentsSource(Pose3dComponent.TranslationsArgumentsProvider.class) public void isWithin_valueNotWithinTolerance_throws(Pose3dComponent component) { - Translation3d closeTranslation = component.add(TRANSLATION, 0.011); + Translation3d closeTranslation = component.add(TRANSLATION, 0.04); assertThrows( AssertionError.class, From 1c5dde3d10178c2c2fcb1bfa2d60d9d788abdbfa Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 10 Jun 2026 17:51:38 -0700 Subject: [PATCH 17/19] Use SubjectHelper#checkTolerance(double) for tolerance checking --- .../lib2813/testing/truth/MeasureSubject.java | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java index 880b36b4..ece7dbef 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/MeasureSubject.java @@ -15,7 +15,6 @@ */ package com.team2813.lib2813.testing.truth; -import static com.google.common.base.Preconditions.checkArgument; import static com.google.common.truth.Fact.fact; import static com.google.common.truth.Fact.simpleFact; import static com.google.common.truth.Truth.assertAbout; @@ -109,17 +108,8 @@ private static String formatUnit(Measure measure) { } private void checkTolerance(Measure tolerance) { - // TOOO: Replace with call to SubjectHelper.checkTolerance when Prospect-Robotics/lib2813#156 - // gets fully merged into main - double mag = tolerance.baseUnitMagnitude(); - checkArgument(!Double.isNaN(mag), "tolerance cannot be NaN"); - checkArgument(mag >= 0, "tolerance (%s) cannot be negative", tolerance); - checkArgument( - Double.doubleToLongBits(mag) != Double.doubleToLongBits(-0.0), - "tolerance (%s) cannot be negative", - tolerance); - checkArgument( - mag != Double.POSITIVE_INFINITY, "tolerance cannot be POSITIVE_INFINITY", tolerance); + double baseTolerance = tolerance.baseUnitMagnitude() - tolerance.unit().toBaseUnits(0); + SubjectHelper.checkTolerance(baseTolerance); } private Measure nonNullActual() { From 151a1e1d2426834d51599de798d615ae63e33067 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 10 Jun 2026 18:03:09 -0700 Subject: [PATCH 18/19] remove duplicate equalWithinTolerance from merge --- .../team2813/lib2813/testing/truth/Rotation2dSubject.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java index 192cc8ae..5874a0b5 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java @@ -69,12 +69,6 @@ public void of(Rotation2d expected) { }; } - static boolean equalWithinTolerance( - Rotation2d rotation1, Rotation2d rotation2, double tolerance) { - double distance = rotation1.getRadians() - rotation2.getRadians(); - return Math.abs(distance) < tolerance; - } - public TolerantComparison isNotWithin(double tolerance) { return new TolerantComparison<>() { @Override From 07a1763803b1bf68dadfa44ac7f57b0094d246d9 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 10 Jun 2026 18:06:05 -0700 Subject: [PATCH 19/19] fix tests --- .../com/team2813/lib2813/testing/truth/Rotation2dSubject.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java index 5874a0b5..74b7c607 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java @@ -77,7 +77,7 @@ public void of(Rotation2d expected) { throw new NullPointerException("Expected value cannot be null."); } checkTolerance(tolerance); - if (!equalWithinTolerance(expected, nonNullActual(), tolerance)) { + if (equalWithinTolerance(expected, nonNullActual(), tolerance)) { failWithoutActual( fact("expected", expected), fact("but was", actual), @@ -90,7 +90,7 @@ public void of(Rotation2d expected) { static boolean equalWithinTolerance( Rotation2d rotation1, Rotation2d rotation2, double tolerance) { double distance = rotation1.getRadians() - rotation2.getRadians(); - return Math.abs(distance) < tolerance; + return Math.abs(distance) <= tolerance; } public void isZero() {