Skip to content

Commit 00e9cd5

Browse files
kcooneykcooney
authored
Rewrite Pose2dSubject to compare with Pose2d instances (#40)
1 parent 0421ab5 commit 00e9cd5

File tree

8 files changed

+383
-165
lines changed

8 files changed

+383
-165
lines changed

‎limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java

+14-15
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,13 @@
22

33
import static com.google.common.truth.Truth.assertThat;
44
import static com.google.common.truth.Truth.assertWithMessage;
5+
56
import static com.team2813.lib2813.limelight.truth.Pose2dSubject.assertThat;
67
import static com.team2813.lib2813.limelight.truth.Pose3dSubject.assertThat;
8+
import static com.team2813.lib2813.limelight.truth.Translation3dSubject.assertThat;
79
import static org.junit.Assert.assertEquals;
810

11+
import edu.wpi.first.math.geometry.Pose2d;
912
import edu.wpi.first.math.geometry.Pose3d;
1013
import edu.wpi.first.math.geometry.Rotation2d;
1114
import edu.wpi.first.math.geometry.Rotation3d;
@@ -117,21 +120,19 @@ public final void presentTest1() throws Exception {
117120
assertThat(locationalData.getBotPoseEstimate()).isPresent();
118121
var poseEstimate = locationalData.getBotPoseEstimate().get();
119122
assertThat(poseEstimate.timestampSeconds()).isGreaterThan(0.0);
120-
expectedPose = new Pose3d(7.35, 0.71, 0.0, new Rotation3d(0.0, 0.0, rotation.getZ()));
121-
assertThat(poseEstimate.pose()).isWithin(0.005).of(expectedPose);
123+
assertThat(poseEstimate.pose()).isWithin(0.005).of(expectedPose.toPose2d());
122124

123125
assertThat(locationalData.getBotPoseEstimateBlue()).isPresent();
124126
var blueEstimate = locationalData.getBotPoseEstimateBlue().get();
125127
assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0);
126-
expectedPose = new Pose3d(15.62, 4.52, 0.0, new Rotation3d(0.0, 0.0, rotation.getZ()));
127-
assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPose);
128+
var expectedPoseEstimate = new Pose2d(15.62, 4.52, rotation.toRotation2d());
129+
assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate);
128130

129131
assertThat(locationalData.getBotPoseEstimateRed()).isPresent();
130132
var redEstimate = locationalData.getBotPoseEstimateRed().get();
131133
assertThat(redEstimate.timestampSeconds()).isWithin(0.005).of(blueEstimate.timestampSeconds());
132-
rotation = new Rotation3d(Math.toRadians(6.82), Math.toRadians(-25.66), Math.toRadians(6.86));
133-
expectedPose = new Pose3d(0.92, 3.10, 0, new Rotation3d(0.0, 0.0, rotation.getZ()));
134-
assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPose);
134+
expectedPoseEstimate = new Pose2d(0.92, 3.10, new Rotation2d(Math.toRadians(6.86)));
135+
assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate);
135136
}
136137

137138
@Test
@@ -161,15 +162,13 @@ public final void presentTest2() throws Exception {
161162
assertThat(locationalData.getBotPoseEstimateBlue()).isPresent();
162163
var blueEstimate = locationalData.getBotPoseEstimateBlue().get();
163164
assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0);
164-
var expectedYaw = new Rotation2d(-2.87);
165-
expectedPose = new Pose3d(15.74, 4.81, 0, new Rotation3d(expectedYaw));
166-
assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPose);
165+
var expectedPoseEstimate = new Pose2d(15.74, 4.81, rotation.toRotation2d());
166+
assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate);
167167

168168
assertThat(locationalData.getBotPoseEstimateRed()).isPresent();
169169
var redEstimate = locationalData.getBotPoseEstimateRed().get();
170-
expectedYaw = new Rotation2d(0.268);
171-
expectedPose = new Pose3d(0.80, 3.20, 0, new Rotation3d(expectedYaw));
172-
assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPose);
170+
expectedPoseEstimate = new Pose2d(0.80, 3.20, new Rotation2d(Math.toRadians(15.36)));
171+
assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate);
173172
assertThat(blueEstimate.timestampSeconds()).isWithin(0.005).of(redEstimate.timestampSeconds());
174173
}
175174

@@ -228,7 +227,7 @@ public final void getVisibleAprilTagPoses() throws Exception {
228227
assertThat(tagMap).containsKey(20);
229228
Set<Integer> tags = tagMap.keySet();
230229
Pose3d pose = tagMap.get(20);
231-
assertThat(pose).translation()
230+
assertThat(pose.getTranslation())
232231
.isWithin(0.005)
233232
.of(new Translation3d(-3.87, 0.72, 0.31));
234233
assertThat(tagMap).hasSize(1);
@@ -254,7 +253,7 @@ public final void visibleTagLocation() throws Exception {
254253
List<Pose3d> aprilTags = limelight.getLocatedAprilTags(visibleTags);
255254
assertThat(aprilTags).hasSize(1);
256255
Pose3d pose = aprilTags.get(0);
257-
assertThat(pose).translation().isWithin(0.005)
256+
assertThat(pose.getTranslation()).isWithin(0.005)
258257
.of(new Translation3d(-3.87, 0.72, 0.31));
259258
}
260259

‎limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose2dSubject.java

+33-8
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,10 @@
33
import com.google.common.truth.FailureMetadata;
44
import com.google.common.truth.Subject;
55
import edu.wpi.first.math.geometry.Pose2d;
6-
import edu.wpi.first.math.geometry.Pose3d;
76

87
import javax.annotation.Nullable;
98

9+
import static com.google.common.truth.Fact.simpleFact;
1010
import static com.google.common.truth.Truth.assertAbout;
1111

1212
/**
@@ -15,10 +15,10 @@
1515
* <p>See <a href="https://truth.dev/extension">Writing your own custom subject</a>
1616
* to learn about creating custom Truth subjects.
1717
*/
18-
public final class Pose2dSubject extends Pose3dSubject {
18+
public final class Pose2dSubject extends Subject {
1919

2020
// User-defined entry point
21-
public static Pose3dSubject assertThat(@Nullable Pose2d pose) {
21+
public static Pose2dSubject assertThat(@Nullable Pose2d pose) {
2222
return assertAbout(pose2ds()).that(pose);
2323
}
2424

@@ -27,14 +27,39 @@ public static Subject.Factory<Pose2dSubject, Pose2d> pose2ds() {
2727
return Pose2dSubject::new;
2828
}
2929

30+
private final Pose2d actual;
31+
3032
private Pose2dSubject(FailureMetadata failureMetadata, @Nullable Pose2d subject) {
31-
super(failureMetadata, toPose3d(subject));
33+
super(failureMetadata, subject);
34+
this.actual = subject;
3235
}
3336

34-
private static @Nullable Pose3d toPose3d(@Nullable Pose2d pose) {
35-
if (pose == null) {
36-
return null;
37+
// User-defined test assertion SPI below this point
38+
39+
public TolerantComparison<Pose2d> isWithin(double tolerance) {
40+
return new TolerantComparison<Pose2d>() {
41+
@Override
42+
public void of(Pose2d expected) {
43+
translation().isWithin(tolerance).of(expected.getTranslation());
44+
rotation().isWithin(tolerance).of(expected.getRotation());
45+
}
46+
};
47+
}
48+
49+
public Translation2dSubject translation() {
50+
return check("getTranslation()").about(Translation2dSubject.translation2ds()).that(nonNullActualPose().getTranslation());
51+
}
52+
53+
public Rotation2dSubject rotation() {
54+
return check("getRotation()").about(Rotation2dSubject.rotation2ds()).that(nonNullActualPose().getRotation());
55+
}
56+
57+
// Helper methods below this point
58+
59+
private Pose2d nonNullActualPose() {
60+
if (actual == null) {
61+
failWithActual(simpleFact("expected a non-null Pose2d"));
3762
}
38-
return new Pose3d(pose);
63+
return actual;
3964
}
4065
}

‎limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose3dSubject.java

+2-142
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,8 @@
11
package com.team2813.lib2813.limelight.truth;
22

3-
import com.google.common.truth.DoubleSubject;
43
import com.google.common.truth.FailureMetadata;
54
import com.google.common.truth.Subject;
65
import edu.wpi.first.math.geometry.Pose3d;
7-
import edu.wpi.first.math.geometry.Rotation3d;
8-
import edu.wpi.first.math.geometry.Translation3d;
96

107
import javax.annotation.Nullable;
118

@@ -18,7 +15,7 @@
1815
* <p>See <a href="https://truth.dev/extension">Writing your own custom subject</a>
1916
* to learn about creating custom Truth subjects.
2017
*/
21-
public class Pose3dSubject extends Subject {
18+
public final class Pose3dSubject extends Subject {
2219

2320
// User-defined entry point
2421
public static Pose3dSubject assertThat(@Nullable Pose3d pose) {
@@ -32,7 +29,7 @@ public static Subject.Factory<Pose3dSubject, Pose3d> pose3ds() {
3229

3330
private final Pose3d actual;
3431

35-
protected Pose3dSubject(FailureMetadata failureMetadata, @Nullable Pose3d subject) {
32+
private Pose3dSubject(FailureMetadata failureMetadata, @Nullable Pose3d subject) {
3633
super(failureMetadata, subject);
3734
this.actual = subject;
3835
}
@@ -68,141 +65,4 @@ private Pose3d nonNullActualPose() {
6865
return actual;
6966
}
7067

71-
/** Truth Subject for making assertions about {@link Translation3d} values. */
72-
public static class Translation3dSubject extends Subject {
73-
74-
// User-defined entry point
75-
public static Translation3dSubject assertThat(@Nullable Translation3d translation) {
76-
return assertAbout(translation3ds()).that(translation);
77-
}
78-
79-
// Static method for getting the subject factory (for use with assertAbout())
80-
public static Factory<Translation3dSubject, Translation3d> translation3ds() {
81-
return Translation3dSubject::new;
82-
}
83-
84-
private final Translation3d actual;
85-
86-
private Translation3dSubject(FailureMetadata failureMetadata, @Nullable Translation3d subject) {
87-
super(failureMetadata, subject);
88-
this.actual = subject;
89-
}
90-
91-
// User-defined test assertion SPI below this point
92-
93-
public TolerantComparison<Translation3d> isWithin(double tolerance) {
94-
return new TolerantComparison<Translation3d>() {
95-
@Override
96-
public void of(Translation3d expected) {
97-
x().isWithin(tolerance).of(expected.getX());
98-
y().isWithin(tolerance).of(expected.getY());
99-
z().isWithin(tolerance).of(expected.getZ());
100-
}
101-
};
102-
}
103-
104-
public void isZero() {
105-
if (!Translation3d.kZero.equals(actual)) {
106-
failWithActual(simpleFact("expected to be zero"));
107-
}
108-
}
109-
110-
// Chained subjects methods below this point
111-
112-
public DoubleSubject x() {
113-
return check("getX()").that(nonNullActual().getX());
114-
}
115-
116-
public DoubleSubject y() {
117-
return check("getY()").that(nonNullActual().getY());
118-
}
119-
120-
public DoubleSubject z() {
121-
return check("getZ()").that(nonNullActual().getZ());
122-
}
123-
124-
// Helper methods below this point
125-
126-
private Translation3d nonNullActual() {
127-
if (actual == null) {
128-
failWithActual(simpleFact("expected a non-null Translation3d"));
129-
}
130-
return actual;
131-
}
132-
}
133-
134-
/** Truth Subject for making assertions about {@link Rotation3d} values. */
135-
static class Rotation3dSubject extends Subject {
136-
// User-defined entry point
137-
public static Rotation3dSubject assertThat(@Nullable Rotation3d rotation) {
138-
return assertAbout(rotation3ds()).that(rotation);
139-
}
140-
141-
// Static method for getting the subject factory (for use with assertAbout())
142-
public static Factory<Rotation3dSubject, Rotation3d> rotation3ds() {
143-
return Rotation3dSubject::new;
144-
}
145-
146-
private final Rotation3d actual;
147-
148-
private Rotation3dSubject(FailureMetadata failureMetadata, @Nullable Rotation3d subject) {
149-
super(failureMetadata, subject);
150-
this.actual = subject;
151-
}
152-
153-
// User-defined test assertion SPI below this point
154-
155-
public TolerantComparison<Rotation3d> isWithin(double tolerance) {
156-
return new TolerantComparison<Rotation3d>() {
157-
@Override
158-
public void of(Rotation3d expected) {
159-
x().isWithin(tolerance).of(expected.getX());
160-
y().isWithin(tolerance).of(expected.getY());
161-
z().isWithin(tolerance).of(expected.getZ());
162-
}
163-
};
164-
}
165-
166-
public void isZero() {
167-
if (!Rotation3d.kZero.equals(actual)) {
168-
failWithActual(simpleFact("expected to be zero"));
169-
}
170-
}
171-
172-
// Chained subjects methods below this point
173-
174-
public DoubleSubject x() {
175-
return check("getX()").that(nonNullActual().getX());
176-
}
177-
178-
public DoubleSubject y() {
179-
return check("getY()").that(nonNullActual().getY());
180-
}
181-
182-
public DoubleSubject z() {
183-
return check("getZ()").that(nonNullActual().getZ());
184-
}
185-
186-
// Helper methods below this point
187-
188-
private Rotation3d nonNullActual() {
189-
if (actual == null) {
190-
failWithActual(simpleFact("expected a non-null Rotation3d"));
191-
}
192-
return actual;
193-
}
194-
}
195-
196-
/**
197-
* A partially specified check about an approximate relationship to a {@code double} subject using
198-
* a tolerance.
199-
*/
200-
public interface TolerantComparison<T> {
201-
202-
/**
203-
* Fails if the subject was expected to be within the tolerance of the given value but was not.
204-
* The subject and tolerance are specified earlier in the fluent call chain.
205-
*/
206-
void of(T expected);
207-
}
20868
}

‎limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation2dSubject.java

+63
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
package com.team2813.lib2813.limelight.truth;
2+
3+
import com.google.common.truth.DoubleSubject;
4+
import com.google.common.truth.FailureMetadata;
5+
import com.google.common.truth.Subject;
6+
import edu.wpi.first.math.geometry.Rotation2d;
7+
8+
import javax.annotation.Nullable;
9+
10+
import static com.google.common.truth.Fact.simpleFact;
11+
import static com.google.common.truth.Truth.assertAbout;
12+
13+
/** Truth Subject for making assertions about {@link Rotation2d} values. */
14+
public final class Rotation2dSubject extends Subject {
15+
// User-defined entry point
16+
public static Rotation2dSubject assertThat(@Nullable Rotation2d rotation) {
17+
return assertAbout(rotation2ds()).that(rotation);
18+
}
19+
20+
// Static method for getting the subject factory (for use with assertAbout())
21+
public static Factory<Rotation2dSubject, Rotation2d> rotation2ds() {
22+
return Rotation2dSubject::new;
23+
}
24+
25+
private final Rotation2d actual;
26+
27+
private Rotation2dSubject(FailureMetadata failureMetadata, @Nullable Rotation2d subject) {
28+
super(failureMetadata, subject);
29+
this.actual = subject;
30+
}
31+
32+
// User-defined test assertion SPI below this point
33+
34+
public TolerantComparison<Rotation2d> isWithin(double tolerance) {
35+
return new TolerantComparison<Rotation2d>() {
36+
@Override
37+
public void of(Rotation2d expected) {
38+
getRadians().isWithin(tolerance).of(expected.getRadians());
39+
}
40+
};
41+
}
42+
43+
public void isZero() {
44+
if (!Rotation2d.kZero.equals(actual)) {
45+
failWithActual(simpleFact("expected to be zero"));
46+
}
47+
}
48+
49+
// Chained subjects methods below this point
50+
51+
public DoubleSubject getRadians() {
52+
return check("getRadians()").that(nonNullActual().getRadians());
53+
}
54+
55+
// Helper methods below this point
56+
57+
private Rotation2d nonNullActual() {
58+
if (actual == null) {
59+
failWithActual(simpleFact("expected a non-null Rotation2d"));
60+
}
61+
return actual;
62+
}
63+
}

0 commit comments

Comments
 (0)