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..fdae47a9 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; @@ -56,8 +58,20 @@ public TolerantComparison isWithin(double tolerance) { 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..82dde139 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; @@ -56,8 +58,20 @@ public TolerantComparison isWithin(double tolerance) { 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 7c11ca3c..307c4e2d 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; @@ -53,11 +55,27 @@ public TolerantComparison isWithin(double tolerance) { return new TolerantComparison() { @Override public void of(Rotation2d expected) { + 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)); + } getRadians().isWithin(tolerance).of(expected.getRadians()); } }; } + static boolean equalWithinTolerance( + Rotation2d rotation1, Rotation2d rotation2, double tolerance) { + double distance = rotation1.getRadians() - rotation2.getRadians(); + return Math.abs(distance) < tolerance; + } + 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..17945892 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; @@ -53,13 +55,37 @@ public TolerantComparison isWithin(double tolerance) { 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)); + } + x().isWithin(tolerance).of(expected.getX()); + y().isWithin(tolerance).of(expected.getY()); + z().isWithin(tolerance).of(expected.getZ()); } }; } + 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..68685efc 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; @@ -54,12 +56,44 @@ public TolerantComparison isWithin(double tolerance) { 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)); + } } }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Translation2d expected) { + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + if (equalWithinTolerance(expected, nonNullActual(), tolerance)) { + failWithoutActual( + fact("expected not to be", expected), + fact("but was", actual), + fact("within 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..252712b7 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; @@ -54,13 +56,44 @@ public TolerantComparison isWithin(double tolerance) { 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)); + } } }; } + public TolerantComparison isNotWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Translation3d expected) { + if (expected == null) { + throw new NullPointerException("Expected value cannot be null."); + } + checkTolerance(tolerance); + if (equalWithinTolerance(expected, nonNullActual(), tolerance)) { + failWithoutActual( + fact("expected not to be", expected), + fact("but was", actual), + fact("within 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..1d13a911 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,10 +60,28 @@ 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, () -> Translation2dSubject.assertThat(closeTranslation).isWithin(0.01).of(TRANSLATION)); } + + @ParameterizedTest + @ArgumentsSource(Pose2dComponent.TranslationsArgumentsProvider.class) + public void isNotWithin_valueNotWithinTolerance_doesNotThrow(Pose2dComponent component) { + Translation2d closeTranslation = component.add(TRANSLATION, 0.016); + + Translation2dSubject.assertThat(closeTranslation).isNotWithin(0.01).of(TRANSLATION); + } + + @ParameterizedTest + @ArgumentsSource(Pose2dComponent.TranslationsArgumentsProvider.class) + public void isNotWithin_valueWithinTolerance_throws(Pose2dComponent component) { + Translation2d closeTranslation = component.add(TRANSLATION, 0.009); + + assertThrows( + AssertionError.class, + () -> Translation2dSubject.assertThat(closeTranslation).isNotWithin(0.01).of(TRANSLATION)); + } } 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..94ca9b79 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,10 +60,28 @@ 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, () -> Translation3dSubject.assertThat(closeTranslation).isWithin(0.01).of(TRANSLATION)); } + + @ParameterizedTest + @ArgumentsSource(Pose3dComponent.TranslationsArgumentsProvider.class) + public void isNotWithin_valueNotWithinTolerance_doesNotThrow(Pose3dComponent component) { + Translation3d closeTranslation = component.add(TRANSLATION, 0.016); + + Translation3dSubject.assertThat(closeTranslation).isNotWithin(0.01).of(TRANSLATION); + } + + @ParameterizedTest + @ArgumentsSource(Pose3dComponent.TranslationsArgumentsProvider.class) + public void isNotWithin_valueWithinTolerance_throws(Pose3dComponent component) { + Translation3d closeTranslation = component.add(TRANSLATION, 0.009); + + assertThrows( + AssertionError.class, + () -> Translation3dSubject.assertThat(closeTranslation).isNotWithin(0.01).of(TRANSLATION)); + } }