Skip to content

Commit 024489d

Browse files
kcooneykcooney
committed
Add tests for processAllUnreadResults()
1 parent 01a4f8e commit 024489d

2 files changed

Lines changed: 237 additions & 21 deletions

File tree

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

Lines changed: 169 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -16,54 +16,202 @@
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;
21-
import edu.wpi.first.apriltag.AprilTag;
2224
import edu.wpi.first.apriltag.AprilTagFieldLayout;
25+
import edu.wpi.first.math.geometry.Pose2d;
2326
import edu.wpi.first.math.geometry.Pose3d;
24-
import edu.wpi.first.math.geometry.Quaternion;
27+
import edu.wpi.first.math.geometry.Rotation2d;
2528
import edu.wpi.first.math.geometry.Rotation3d;
2629
import edu.wpi.first.math.geometry.Transform3d;
27-
import edu.wpi.first.math.geometry.Translation3d;
30+
import edu.wpi.first.math.geometry.Translation2d;
2831
import edu.wpi.first.networktables.NetworkTableInstance;
32+
import edu.wpi.first.units.measure.Distance;
33+
import java.util.ArrayList;
2934
import java.util.List;
35+
import java.util.function.Consumer;
36+
import java.util.stream.Stream;
3037
import org.junit.jupiter.params.ParameterizedTest;
38+
import org.junit.jupiter.params.provider.Arguments;
3139
import org.junit.jupiter.params.provider.EnumSource;
40+
import org.junit.jupiter.params.provider.MethodSource;
41+
import org.photonvision.EstimatedRobotPose;
3242
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
43+
import org.photonvision.simulation.SimCameraProperties;
44+
import org.photonvision.simulation.VisionSystemSim;
45+
import org.photonvision.targeting.PhotonTrackedTarget;
3346

3447
/** Tests for {@link MultiPhotonPoseEstimator}. */
3548
@ProvideUniqueNetworkTableInstance
49+
@InitWPILib
3650
class MultiPhotonPoseEstimatorTest {
37-
private static final double FIELD_LENGTH = 17.548;
38-
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)));
51+
// Place the camera in the center of the robot, ~17.1cm up, facing forward and up.
4452
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));
53+
new Transform3d(0, 0, 0.1708140348, new Rotation3d(0, -0.1745329252, 0));
5054

51-
private static final Camera FRONT_CAMERA = new Camera("front", FRONT_CAMERA_TRANSFORM);
55+
private static final Camera FRONT_CAMERA =
56+
new Camera("front", FRONT_CAMERA_TRANSFORM, SimCameraProperties::PERFECT_90DEG);
5257

5358
@ParameterizedTest
5459
@EnumSource(value = PoseStrategy.class)
5560
void getPrimaryStrategy(PoseStrategy poseStrategy, NetworkTableInstance ntInstance) {
5661
try (var estimator =
57-
MultiPhotonPoseEstimator.builder(ntInstance, createFieldLayout(), poseStrategy)
62+
MultiPhotonPoseEstimator.builder(
63+
ntInstance, ReefscapeAprilTag.createFieldLayout(), poseStrategy)
5864
.addCamera(FRONT_CAMERA)
5965
.build()) {
6066
assertThat(estimator.getPrimaryStrategy()).isEqualTo(poseStrategy);
6167
}
6268
}
6369

64-
private static AprilTagFieldLayout createFieldLayout() {
65-
List<AprilTag> aprilTags =
66-
List.of(new AprilTag(REEFSCAPE_APRIL_TAG_ID, REEFSCAPE_APRIL_TAG_POSE));
67-
return new AprilTagFieldLayout(aprilTags, FIELD_LENGTH, FIELD_WIDTH);
70+
private record PoseTestData(Pose3d robotPose, ReefscapeAprilTag aprilTag) {}
71+
72+
private static Stream<Arguments> posesInField() {
73+
return Stream.of(
74+
facingAprilTag(ReefscapeAprilTag.RED_REEF_CENTER, Meters.of(1)),
75+
facingAprilTag(ReefscapeAprilTag.BLUE_REEF_CENTER, Meters.of(1)),
76+
facingAprilTag(ReefscapeAprilTag.RED_PROCESSOR, Meters.of(0.5)),
77+
facingAprilTag(ReefscapeAprilTag.BLUE_PROCESSOR, Meters.of(0.5)));
78+
}
79+
80+
@ParameterizedTest(name = "{0}")
81+
@MethodSource("posesInField")
82+
void processAllUnreadResults_estimatedPoseInField(
83+
String testName, PoseTestData testData, NetworkTableInstance ntInstance) {
84+
AprilTagFieldLayout fieldLayout = ReefscapeAprilTag.createFieldLayout(testData.aprilTag);
85+
VisionSystemSim visionSystemSim = new VisionSystemSim("test");
86+
visionSystemSim.addAprilTags(fieldLayout);
87+
88+
double z = testData.aprilTag.toAprilTag().pose.getZ();
89+
Camera camera =
90+
new Camera(
91+
"front",
92+
new Transform3d(0, 0, z, FRONT_CAMERA_TRANSFORM.getRotation()),
93+
SimCameraProperties::PERFECT_90DEG);
94+
95+
try (var estimator =
96+
MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY)
97+
.addCamera(camera)
98+
.build()) {
99+
estimator.addCamerasToSimulator(
100+
visionSystemSim,
101+
(c, simCamera) -> {
102+
simCamera.enableRawStream(false);
103+
simCamera.enableProcessedStream(false);
104+
});
105+
visionSystemSim.update(testData.robotPose);
106+
107+
var estimateCollector = new EstimateCollector();
108+
var rejectedPoseCollector = new RejectedPoseCollector();
109+
110+
// Call the method under test
111+
estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector);
112+
113+
assertThat(estimateCollector.estimates).hasSize(1);
114+
assertThat(rejectedPoseCollector.rejectedPoses).isEmpty();
115+
assertThat(estimateCollector.estimates.get(0).estimatedPose)
116+
.isWithin(0.01)
117+
.of(testData.robotPose);
118+
}
119+
}
120+
121+
private static Stream<Arguments> posesOutOfField() {
122+
return Stream.of(
123+
facingAprilTag(ReefscapeAprilTag.RED_REEF_CENTER, Meters.of(4)),
124+
facingAprilTag(ReefscapeAprilTag.BLUE_REEF_CENTER, Meters.of(4)),
125+
facingAprilTag(ReefscapeAprilTag.RED_PROCESSOR, Meters.of(8.1)),
126+
facingAprilTag(ReefscapeAprilTag.BLUE_PROCESSOR, Meters.of(8.1)));
127+
}
128+
129+
@ParameterizedTest(name = "{0}")
130+
@MethodSource("posesOutOfField")
131+
void processAllUnreadResults_estimatedPoseOutsideField(
132+
String testName, PoseTestData testData, NetworkTableInstance ntInstance) {
133+
AprilTagFieldLayout fieldLayout = ReefscapeAprilTag.createFieldLayout(testData.aprilTag);
134+
VisionSystemSim visionSystemSim = new VisionSystemSim("test");
135+
visionSystemSim.addAprilTags(fieldLayout);
136+
137+
double z = testData.aprilTag.toAprilTag().pose.getZ();
138+
Camera camera =
139+
new Camera(
140+
"front",
141+
new Transform3d(0, 0, z, FRONT_CAMERA_TRANSFORM.getRotation()),
142+
SimCameraProperties::PERFECT_90DEG);
143+
144+
try (var estimator =
145+
MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY)
146+
.addCamera(camera)
147+
.build()) {
148+
estimator.addCamerasToSimulator(
149+
visionSystemSim,
150+
(c, simCamera) -> {
151+
simCamera.enableRawStream(false);
152+
simCamera.enableProcessedStream(false);
153+
});
154+
visionSystemSim.update(testData.robotPose);
155+
156+
var estimateCollector = new EstimateCollector();
157+
var rejectedPoseCollector = new RejectedPoseCollector();
158+
159+
// Call the method under test
160+
estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector);
161+
162+
assertThat(estimateCollector.estimates).isEmpty();
163+
assertThat(rejectedPoseCollector.rejectedPoses).hasSize(1);
164+
assertThat(targetsUsed(rejectedPoseCollector.rejectedPoses.get(0)))
165+
.containsExactly(testData.aprilTag.id());
166+
}
167+
}
168+
169+
private static List<Integer> targetsUsed(EstimatedRobotPose estimatedPose) {
170+
return estimatedPose.targetsUsed.stream().map(PhotonTrackedTarget::getFiducialId).toList();
171+
}
172+
173+
/** Creates test data with a position that is the given distance away from the given AprilTag. */
174+
private static Arguments facingAprilTag(ReefscapeAprilTag tag, Distance distanceFromTag) {
175+
Pose2d closestTagPose = tag.toAprilTag().pose.toPose2d();
176+
Rotation2d tagRotation = closestTagPose.getRotation();
177+
double distance = distanceFromTag.in(Meters);
178+
Translation2d translation =
179+
new Translation2d(distance * tagRotation.getCos(), distance * tagRotation.getSin());
180+
181+
Pose2d robotPose =
182+
new Pose2d(
183+
closestTagPose.getTranslation().plus(translation),
184+
tagRotation.rotateBy(Rotation2d.k180deg));
185+
186+
var testName = String.format("%.1fmFrom%s", distance, toCamelCase(tag.name()));
187+
return Arguments.of(testName, new PoseTestData(new Pose3d(robotPose), tag));
188+
}
189+
190+
private static String toCamelCase(String s) {
191+
StringBuilder camelCaseString = new StringBuilder();
192+
for (String part : s.split("_")) {
193+
camelCaseString.append(part.substring(0, 1).toUpperCase());
194+
camelCaseString.append(part.substring(1).toLowerCase());
195+
}
196+
return camelCaseString.toString();
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+
}
68216
}
69217
}

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

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
/*
2+
Copyright 2026 Prospect Robotics SWENext Club
3+
4+
Licensed under the Apache License, Version 2.0 (the "License");
5+
you may not use this file except in compliance with the License.
6+
You may obtain a copy of the License at
7+
8+
http://www.apache.org/licenses/LICENSE-2.0
9+
10+
Unless required by applicable law or agreed to in writing, software
11+
distributed under the License is distributed on an "AS IS" BASIS,
12+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
See the License for the specific language governing permissions and
14+
limitations under the License.
15+
*/
16+
package com.team2813.lib2813.vision;
17+
18+
import edu.wpi.first.apriltag.AprilTag;
19+
import edu.wpi.first.apriltag.AprilTagFieldLayout;
20+
import edu.wpi.first.apriltag.AprilTagFields;
21+
import edu.wpi.first.math.geometry.Pose3d;
22+
import java.util.List;
23+
import java.util.Optional;
24+
25+
/** Contains AprilTags for the Reefscape Welded field. */
26+
enum ReefscapeAprilTag {
27+
RED_REEF_CENTER(7),
28+
BLUE_REEF_CENTER(18),
29+
RED_PROCESSOR(3),
30+
BLUE_PROCESSOR(16);
31+
32+
/** Creates a (mutable) AprilTag for this enum value. */
33+
AprilTag toAprilTag() {
34+
Optional<Pose3d> tagPose = memoizedFieldLayout().getTagPose(tagId);
35+
return new AprilTag(tagId, tagPose.orElseThrow());
36+
}
37+
38+
public int id() {
39+
return tagId;
40+
}
41+
42+
/** Creates a field layout for Reefscape Welded with the origin set to zero. */
43+
static AprilTagFieldLayout createFieldLayout() {
44+
return AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
45+
}
46+
47+
/** Creates a field layout with the dimensions of Reefscape Welded and the given tag. */
48+
static AprilTagFieldLayout createFieldLayout(ReefscapeAprilTag tag) {
49+
var fieldLayout = memoizedFieldLayout();
50+
return new AprilTagFieldLayout(
51+
List.of(tag.toAprilTag()), fieldLayout.getFieldLength(), fieldLayout.getFieldWidth());
52+
}
53+
54+
private static AprilTagFieldLayout possiblyNullFieldLayout;
55+
56+
private static AprilTagFieldLayout memoizedFieldLayout() {
57+
if (possiblyNullFieldLayout == null) {
58+
possiblyNullFieldLayout = createFieldLayout();
59+
}
60+
return possiblyNullFieldLayout;
61+
}
62+
63+
ReefscapeAprilTag(int tagId) {
64+
this.tagId = tagId;
65+
}
66+
67+
private final int tagId;
68+
}

0 commit comments

Comments
 (0)