From b49a67937ce4be6856f29a9d88488c25618d8da3 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Mon, 23 Feb 2026 17:26:23 -0800 Subject: [PATCH 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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"); + } +}