Skip to content

Commit e25fc44

Browse files
kcooneykcooney
committed
Add initial tests for processAllUnreadResults()
1 parent 01a4f8e commit e25fc44

1 file changed

Lines changed: 160 additions & 12 deletions

File tree

‎vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java‎

Lines changed: 160 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -16,39 +16,66 @@
1616
package com.team2813.lib2813.vision;
1717

1818
import static com.google.common.truth.Truth.assertThat;
19+
import static com.team2813.lib2813.testing.truth.Pose3dSubject.assertThat;
20+
import static edu.wpi.first.units.Units.Meters;
1921

22+
import com.team2813.lib2813.testing.junit.jupiter.InitWPILib;
2023
import com.team2813.lib2813.testing.junit.jupiter.ProvideUniqueNetworkTableInstance;
2124
import edu.wpi.first.apriltag.AprilTag;
2225
import edu.wpi.first.apriltag.AprilTagFieldLayout;
26+
import edu.wpi.first.math.geometry.Pose2d;
2327
import edu.wpi.first.math.geometry.Pose3d;
2428
import edu.wpi.first.math.geometry.Quaternion;
29+
import edu.wpi.first.math.geometry.Rotation2d;
2530
import edu.wpi.first.math.geometry.Rotation3d;
2631
import edu.wpi.first.math.geometry.Transform3d;
32+
import edu.wpi.first.math.geometry.Translation2d;
2733
import edu.wpi.first.math.geometry.Translation3d;
2834
import edu.wpi.first.networktables.NetworkTableInstance;
35+
import edu.wpi.first.units.measure.Distance;
36+
import java.util.ArrayList;
2937
import java.util.List;
38+
import java.util.function.Consumer;
39+
import java.util.stream.Stream;
3040
import org.junit.jupiter.params.ParameterizedTest;
41+
import org.junit.jupiter.params.provider.Arguments;
3142
import org.junit.jupiter.params.provider.EnumSource;
43+
import org.junit.jupiter.params.provider.MethodSource;
44+
import org.photonvision.EstimatedRobotPose;
3245
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
46+
import org.photonvision.simulation.SimCameraProperties;
47+
import org.photonvision.simulation.VisionSystemSim;
3348

3449
/** Tests for {@link MultiPhotonPoseEstimator}. */
3550
@ProvideUniqueNetworkTableInstance
51+
@InitWPILib
3652
class MultiPhotonPoseEstimatorTest {
3753
private static final double FIELD_LENGTH = 17.548;
3854
private static final double FIELD_WIDTH = 8.052;
39-
private static final int REEFSCAPE_APRIL_TAG_ID = 7;
40-
private static final Pose3d REEFSCAPE_APRIL_TAG_POSE =
41-
new Pose3d(
42-
new Translation3d(13.890498, 4.0259, 0.308102),
43-
new Rotation3d(new Quaternion(1.0, 0.0, 0.0, 0.0)));
55+
private static final AprilTag REEFSCAPE_APRIL_TAG_LOWER_LEFT =
56+
new AprilTag(
57+
12,
58+
new Pose3d(
59+
new Translation3d(0.851154, 0.65532, 1.4859),
60+
new Rotation3d(new Quaternion(0.8910065241883679, 0.0, 0.0, 0.45399049973954675))));
61+
private static final AprilTag REEFSCAPE_APRIL_TAG_RED_REEF_CENTER =
62+
new AprilTag(
63+
7,
64+
new Pose3d(
65+
new Translation3d(13.890498, 4.0259, 0.308102),
66+
new Rotation3d(new Quaternion(1.0, 0.0, 0.0, 0.0))));
67+
private static final AprilTag REEFSCAPE_APRIL_TAG_BLUE_REEF_CENTER =
68+
new AprilTag(
69+
18,
70+
new Pose3d(
71+
new Translation3d(3.6576, 4.0259, 0.308102),
72+
new Rotation3d(new Quaternion(6.123233995736766e-17, 0.0, 0.0, 1.0))));
73+
// Place the camera in the center of the robot, ~17.1cm up, facing forward and up.
4474
private static final Transform3d FRONT_CAMERA_TRANSFORM =
45-
new Transform3d(
46-
0.1688157406,
47-
0.2939800826,
48-
0.1708140348,
49-
new Rotation3d(0, -0.1745329252, -0.5235987756));
75+
new Transform3d(0, 0, 0.1708140348, new Rotation3d(0, -0.1745329252, 0));
5076

51-
private static final Camera FRONT_CAMERA = new Camera("front", FRONT_CAMERA_TRANSFORM);
77+
private static final Camera FRONT_CAMERA =
78+
new Camera("front", FRONT_CAMERA_TRANSFORM, SimCameraProperties::PERFECT_90DEG);
5279

5380
@ParameterizedTest
5481
@EnumSource(value = PoseStrategy.class)
@@ -61,9 +88,130 @@ void getPrimaryStrategy(PoseStrategy poseStrategy, NetworkTableInstance ntInstan
6188
}
6289
}
6390

91+
private static Stream<Arguments> posesInField() {
92+
return Stream.of(
93+
Arguments.of(
94+
"facingRedReefCenter",
95+
facingAprilTag(REEFSCAPE_APRIL_TAG_RED_REEF_CENTER, Meters.of(1))),
96+
Arguments.of(
97+
"facingBlueReefCenter",
98+
facingAprilTag(REEFSCAPE_APRIL_TAG_BLUE_REEF_CENTER, Meters.of(1))));
99+
}
100+
101+
@ParameterizedTest(name = "{0}")
102+
@MethodSource("posesInField")
103+
void processAllUnreadResults_estimatedPoseInField(
104+
String testName, Pose3d robotPose, NetworkTableInstance ntInstance) {
105+
AprilTagFieldLayout fieldLayout = createFieldLayout();
106+
VisionSystemSim visionSystemSim = new VisionSystemSim("test");
107+
visionSystemSim.addAprilTags(fieldLayout);
108+
109+
try (var estimator =
110+
MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY)
111+
.addCamera(FRONT_CAMERA)
112+
.build()) {
113+
estimator.addCamerasToSimulator(
114+
visionSystemSim,
115+
(camera, simCamera) -> {
116+
simCamera.enableRawStream(false);
117+
simCamera.enableProcessedStream(false);
118+
});
119+
visionSystemSim.update(robotPose);
120+
121+
var estimateCollector = new EstimateCollector();
122+
var rejectedPoseCollector = new RejectedPoseCollector();
123+
124+
// Call the method under test
125+
estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector);
126+
127+
assertThat(estimateCollector.estimates).hasSize(1);
128+
assertThat(rejectedPoseCollector.rejectedPoses).isEmpty();
129+
assertThat(estimateCollector.estimates.get(0).estimatedPose).isWithin(0.01).of(robotPose);
130+
}
131+
}
132+
133+
private static Stream<Arguments> posesOutOfField() {
134+
return Stream.of(
135+
Arguments.of(
136+
"outsideRed", facingAprilTag(REEFSCAPE_APRIL_TAG_RED_REEF_CENTER, Meters.of(4))),
137+
Arguments.of(
138+
"outsideBlue", facingAprilTag(REEFSCAPE_APRIL_TAG_BLUE_REEF_CENTER, Meters.of(4))));
139+
}
140+
141+
@ParameterizedTest(name = "{0}")
142+
@MethodSource("posesOutOfField")
143+
void processAllUnreadResults_estimatedPoseOutsideField(
144+
String testName, Pose3d robotPose, NetworkTableInstance ntInstance) {
145+
AprilTagFieldLayout fieldLayout = createFieldLayout();
146+
VisionSystemSim visionSystemSim = new VisionSystemSim("test");
147+
visionSystemSim.addAprilTags(fieldLayout);
148+
149+
try (var estimator =
150+
MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY)
151+
.addCamera(FRONT_CAMERA)
152+
.build()) {
153+
estimator.addCamerasToSimulator(
154+
visionSystemSim,
155+
(camera, simCamera) -> {
156+
simCamera.enableRawStream(false);
157+
simCamera.enableProcessedStream(false);
158+
});
159+
visionSystemSim.update(robotPose);
160+
161+
var estimateCollector = new EstimateCollector();
162+
var rejectedPoseCollector = new RejectedPoseCollector();
163+
164+
// Call the method under test
165+
estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector);
166+
167+
assertThat(estimateCollector.estimates).isEmpty();
168+
assertThat(rejectedPoseCollector.rejectedPoses).hasSize(1);
169+
assertThat(rejectedPoseCollector.rejectedPoses.get(0).estimatedPose)
170+
.isWithin(0.1)
171+
.of(robotPose);
172+
}
173+
}
174+
175+
/** Creates a field layout similar to REEFSCAPE with the origin set to zero. */
64176
private static AprilTagFieldLayout createFieldLayout() {
65177
List<AprilTag> aprilTags =
66-
List.of(new AprilTag(REEFSCAPE_APRIL_TAG_ID, REEFSCAPE_APRIL_TAG_POSE));
178+
List.of(
179+
REEFSCAPE_APRIL_TAG_LOWER_LEFT,
180+
REEFSCAPE_APRIL_TAG_RED_REEF_CENTER,
181+
REEFSCAPE_APRIL_TAG_BLUE_REEF_CENTER);
67182
return new AprilTagFieldLayout(aprilTags, FIELD_LENGTH, FIELD_WIDTH);
68183
}
184+
185+
/** Creates a position that is the given distance away from the given AprilTag. */
186+
private static Pose3d facingAprilTag(AprilTag aprilTag, Distance distanceFromTag) {
187+
double distance = distanceFromTag.in(Meters);
188+
Pose2d closestTagPose = aprilTag.pose.toPose2d();
189+
Rotation2d tagRotation = closestTagPose.getRotation();
190+
Translation2d translation =
191+
new Translation2d(distance * tagRotation.getCos(), distance * tagRotation.getSin());
192+
Pose2d robotPose2d =
193+
new Pose2d(
194+
closestTagPose.getTranslation().plus(translation),
195+
tagRotation.rotateBy(Rotation2d.k180deg));
196+
return new Pose3d(robotPose2d);
197+
}
198+
199+
private static class EstimateCollector implements PoseEstimateConsumer<Camera> {
200+
final List<EstimatedRobotPose> estimates = new ArrayList<>();
201+
202+
@Override
203+
public void addEstimatedRobotPose(EstimatedRobotPose estimatedPose, Camera camera) {
204+
assertThat(camera.name()).isEqualTo(FRONT_CAMERA.name());
205+
estimates.add(estimatedPose);
206+
}
207+
}
208+
209+
private static class RejectedPoseCollector implements Consumer<EstimatedRobotPose> {
210+
final List<EstimatedRobotPose> rejectedPoses = new ArrayList<>();
211+
212+
@Override
213+
public void accept(EstimatedRobotPose estimatedRobotPose) {
214+
rejectedPoses.add(estimatedRobotPose);
215+
}
216+
}
69217
}

0 commit comments

Comments
 (0)