Skip to content

Commit e4cd82f

Browse files
PGgit08PGgit08
committed
added remaining unit tests for now and did some other todos
1 parent 9877607 commit e4cd82f

File tree

3 files changed

+150
-49
lines changed

3 files changed

+150
-49
lines changed

‎src/main/java/frc/robot/subsystems/Swerve.java

+2-2
Original file line numberDiff line numberDiff line change
@@ -392,7 +392,7 @@ public Rotation2d getHeading() {
392392

393393
/** Returns the robot's estimated rotation at the given timestamp. */
394394
public Rotation2d getHeadingAtTime(double timestamp) {
395-
return Rotation2d.kZero; // TODO
395+
return samplePoseAt(timestamp).orElse(getPose()).getRotation();
396396
}
397397

398398
/** Wrapper for getting current robot-relative chassis speeds. */
@@ -451,7 +451,7 @@ public void periodic() {
451451
DogLog.log("Swerve/Detected Tags", _detectedTags.toArray(Pose3d[]::new));
452452

453453
if (!_ignoreVisionEstimates) {
454-
_acceptedEstimates.sort(VisionPoseEstimate.comparator);
454+
_acceptedEstimates.sort(VisionPoseEstimate.sorter);
455455

456456
_acceptedEstimates.forEach(
457457
(e) -> {

‎src/main/java/frc/robot/utils/VisionPoseEstimator.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -125,7 +125,7 @@ public record VisionPoseEstimate(
125125
* smallest to highest), then the standard deviations at the same timestamp are sorted if
126126
* necessary.
127127
*/
128-
public static final Comparator<VisionPoseEstimate> comparator =
128+
public static final Comparator<VisionPoseEstimate> sorter =
129129
Comparator.comparing(
130130
VisionPoseEstimate::timestamp,
131131
(t1, t2) -> {

‎src/test/java/frc/robot/VisionPoseEstimatorTest.java

+147-46
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import edu.wpi.first.math.util.Units;
1515
import frc.lib.UnitTestingUtil;
1616
import frc.robot.utils.VisionPoseEstimator;
17+
import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimate;
1718
import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimatorConstants;
1819
import java.util.ArrayList;
1920
import java.util.List;
@@ -41,8 +42,22 @@ public static void setupField() {
4142
List<AprilTag> tags = new ArrayList<>();
4243

4344
// // add all tags to the field layout
44-
tags.add(new AprilTag(1, new Pose3d(1, 0, 1, new Rotation3d(0, 0, -Math.PI))));
45-
tags.add(new AprilTag(2, new Pose3d(2, 0.5, 0.5, new Rotation3d(0, -0.3, -Math.PI))));
45+
tags.add(
46+
new AprilTag(1, new Pose3d(1, 0, 1.2, new Rotation3d(0, 0, -Math.PI)))); // close tag #1
47+
tags.add(
48+
new AprilTag(
49+
2,
50+
new Pose3d(
51+
2, 0.5, 0.5, new Rotation3d(0, -0.3, -Math.PI)))); // close tag #2 (for multi-tag)
52+
tags.add(
53+
new AprilTag(
54+
3,
55+
new Pose3d(
56+
5, 0.5, 0.5, new Rotation3d(0, -1, -Math.PI)))); // far tag #1 (for distance test)
57+
tags.add(
58+
new AprilTag(
59+
4,
60+
new Pose3d(1.5, 0, 1, new Rotation3d(0, 0, -Math.PI)))); // close tag #3 (for ambiguity)
4661

4762
_fieldLayout = new AprilTagFieldLayout(tags, Units.feetToMeters(54), Units.feetToMeters(27));
4863
}
@@ -79,18 +94,19 @@ public void noResults() {
7994
_testCam.update(this::dummyGyroHeading);
8095

8196
// no targets, no new estimates
82-
assertEquals(_testCam.getNewEstimates().size(), 0);
97+
assertEquals(0, _testCam.getNewEstimates().size());
8398
}
8499

85100
@Test
86101
public void singleTagResult() {
87102
_visionSystemSim.addVisionTargets(
88103
new VisionTargetSim(_fieldLayout.getTagPose(1).get(), TargetModel.kAprilTag36h11, 1));
104+
89105
_visionSystemSim.update(Pose2d.kZero); // should see the single target
90106

91107
_testCam.update(this::dummyGyroHeading);
92108

93-
assertEquals(_testCam.getNewEstimates().size(), 1);
109+
assertEquals(1, _testCam.getNewEstimates().size());
94110
}
95111

96112
@Test
@@ -103,54 +119,55 @@ public void singleTagResults() {
103119

104120
_testCam.update(this::dummyGyroHeading);
105121

106-
assertEquals(_testCam.getNewEstimates().size(), 2);
122+
assertEquals(2, _testCam.getNewEstimates().size());
107123
}
108124

109125
@Test
110126
public void singleTagEstimate() {
111127
_visionSystemSim.addVisionTargets(
112128
new VisionTargetSim(_fieldLayout.getTagPose(1).get(), TargetModel.kAprilTag36h11, 1));
129+
113130
_visionSystemSim.update(Pose2d.kZero);
114131

115132
_testCam.update(this::dummyGyroHeading);
116133

117134
var estimates = _testCam.getNewEstimates(); // 1 estimate
118-
assertEquals(estimates.size(), 1);
135+
assertEquals(1, estimates.size());
119136

120137
var estimate = estimates.get(0);
121138

122139
assert estimate.isValid();
123140

124141
// pose validity
125-
assertEquals(estimate.pose().getX(), 0, 1e-4);
126-
assertEquals(estimate.pose().getY(), 0, 1e-4);
127-
assertEquals(estimate.pose().getZ(), 0, 1e-4);
128-
assertEquals(estimate.pose().getRotation().getX(), 0, 1e-4);
129-
assertEquals(estimate.pose().getRotation().getY(), 0, 1e-4);
130-
assertEquals(estimate.pose().getRotation().getZ(), 0, 1e-4);
142+
assertEquals(0, estimate.pose().getX(), 1e-4);
143+
assertEquals(0, estimate.pose().getY(), 1e-4);
144+
assertEquals(0, estimate.pose().getZ(), 1e-4);
145+
assertEquals(0, estimate.pose().getRotation().getX(), 1e-4);
146+
assertEquals(0, estimate.pose().getRotation().getY(), 1e-4);
147+
assertEquals(0, estimate.pose().getRotation().getZ(), 1e-4);
131148

132149
// should see only ID 1
133-
assertArrayEquals(estimate.detectedTags(), new int[] {1});
150+
assertArrayEquals(new int[] {1}, estimate.detectedTags());
134151

135152
// distance validity
136153
assertEquals(
137-
estimate.avgTagDistance(),
138154
_fieldLayout
139155
.getTagPose(1)
140156
.get()
141157
.getTranslation()
142158
.getDistance(Pose3d.kZero.getTranslation()),
159+
estimate.avgTagDistance(),
143160
1e-4);
144161

145162
// std devs validity
146-
assertNotEquals(estimate.stdDevs(), new int[] {0, 0, 0});
163+
assertNotEquals(new int[] {-1, -1, -1}, estimate.stdDevs());
164+
assertNotEquals(new int[] {0, 0, 0}, estimate.stdDevs());
147165
}
148166

149167
// multi-tag does not work due to this issue:
150168
// https://github.com/PhotonVision/photonvision/issues/1630
151-
// the field-relative corner coordinates are used in the multi-tag solvepnp:
169+
// and the field-relative corner coordinates are used in the multi-tag solvepnp:
152170
// https://github.com/PhotonVision/photonvision/blob/e8efef476b3b4681c8899a8720774d6dbd5ccf56/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java#L573
153-
154171
// @Test
155172
public void multiTagEstimate() {
156173
_visionSystemSim.addAprilTags(_fieldLayout);
@@ -159,7 +176,7 @@ public void multiTagEstimate() {
159176
_testCam.update(this::dummyGyroHeading);
160177

161178
var estimates = _testCam.getNewEstimates(); // 1 estimate
162-
assertEquals(estimates.size(), 1);
179+
assertEquals(1, estimates.size());
163180

164181
var estimate = estimates.get(0);
165182

@@ -168,19 +185,20 @@ public void multiTagEstimate() {
168185
assert estimate.isValid();
169186

170187
// pose validity
171-
assertEquals(estimate.pose().getX(), 0, 1e-4);
172-
assertEquals(estimate.pose().getY(), 0, 1e-4);
173-
assertEquals(estimate.pose().getZ(), 0, 1e-4);
174-
assertEquals(estimate.pose().getRotation().getX(), 0, 1e-4);
175-
assertEquals(estimate.pose().getRotation().getY(), 0, 1e-4);
176-
assertEquals(estimate.pose().getRotation().getZ(), 0, 1e-4);
188+
assertEquals(0, estimate.pose().getX(), 1e-4);
189+
assertEquals(0, estimate.pose().getY(), 1e-4);
190+
assertEquals(0, estimate.pose().getZ(), 1e-4);
191+
assertEquals(0, estimate.pose().getRotation().getX(), 1e-4);
192+
assertEquals(0, estimate.pose().getRotation().getY(), 1e-4);
193+
assertEquals(0, estimate.pose().getRotation().getZ(), 1e-4);
177194

178195
// should see only ID 1 and 2
179-
assertArrayEquals(estimate.detectedTags(), new int[] {1, 2});
196+
assertArrayEquals(new int[] {1, 2}, estimate.detectedTags());
197+
198+
assertEquals(-1, estimate.ambiguity()); // -1 ambiguity, it's not present during multi-tag
180199

181200
// distance validity
182201
assertEquals(
183-
estimate.avgTagDistance(),
184202
(_fieldLayout
185203
.getTagPose(1)
186204
.get()
@@ -192,10 +210,12 @@ public void multiTagEstimate() {
192210
.getTranslation()
193211
.getDistance(Pose3d.kZero.getTranslation()))
194212
/ 2,
213+
estimate.avgTagDistance(),
195214
1e-4);
196215

197216
// std devs validity
198-
assertNotEquals(estimate.stdDevs(), new int[] {0, 0, 0});
217+
assertNotEquals(new int[] {-1, -1, -1}, estimate.stdDevs());
218+
assertNotEquals(new int[] {0, 0, 0}, estimate.stdDevs());
199219
}
200220

201221
@Test
@@ -209,51 +229,132 @@ public void boundsFilter() {
209229
_testCam.update(this::dummyGyroHeading);
210230

211231
var estimates = _testCam.getNewEstimates(); // 1 estimate
212-
assertEquals(estimates.size(), 1);
232+
assertEquals(1, estimates.size());
213233

214234
var estimate = estimates.get(0);
215235

216236
assertFalse(estimate.isValid());
217237

218238
// pose validity
219-
assertEquals(estimate.pose().getX(), 0, 1e-4);
220-
assertEquals(estimate.pose().getY(), 0, 1e-4);
221-
assertNotEquals(estimate.pose().getZ(), 0, 1e-4);
222-
assertEquals(estimate.pose().getRotation().getX(), 0, 1e-4);
223-
assertEquals(estimate.pose().getRotation().getY(), 0, 1e-4);
224-
assertEquals(estimate.pose().getRotation().getZ(), 0, 1e-4);
239+
assertEquals(0, estimate.pose().getX(), 1e-4);
240+
assertEquals(0, estimate.pose().getY(), 1e-4);
241+
assertNotEquals(0, estimate.pose().getZ(), 1e-4);
242+
assertEquals(0, estimate.pose().getRotation().getX(), 1e-4);
243+
assertEquals(0, estimate.pose().getRotation().getY(), 1e-4);
244+
assertEquals(0, estimate.pose().getRotation().getZ(), 1e-4);
225245

226246
// should see only ID 1
227-
assertArrayEquals(estimate.detectedTags(), new int[] {1});
247+
assertArrayEquals(new int[] {1}, estimate.detectedTags());
228248

229249
// distance validity
230250
assertNotEquals(
231-
estimate.avgTagDistance(),
232251
_fieldLayout
233252
.getTagPose(1)
234253
.get()
235254
.getTranslation()
236255
.getDistance(Pose3d.kZero.getTranslation()),
256+
estimate.avgTagDistance(),
237257
1e-4);
238258

239259
// std devs validity
240-
assertArrayEquals(estimate.stdDevs(), new double[] {-1, -1, -1});
260+
assertArrayEquals(new double[] {-1, -1, -1}, estimate.stdDevs());
241261
}
242262

243-
// ⬇ TODO ⬇
263+
// beta ambiguity issue:
264+
// https://github.com/PhotonVision/photonvision/issues/1623
265+
// so this isn't gonna work
266+
// @Test
267+
public void ambiguityFilter() {
268+
// TODO
269+
}
244270

245271
@Test
246-
public void ambiguityFilter() {}
272+
public void singleTagDistanceFilter() {
273+
_visionSystemSim.addVisionTargets(
274+
new VisionTargetSim(_fieldLayout.getTagPose(3).get(), TargetModel.kAprilTag36h11, 3));
247275

248-
@Test
249-
public void singleTagDistanceFilter() {}
276+
_visionSystemSim.update(Pose2d.kZero);
250277

251-
@Test
252-
public void multiTagDistanceFilter() {}
278+
_testCam.update(this::dummyGyroHeading);
253279

254-
@Test
255-
public void estimateSort() {}
280+
var estimates = _testCam.getNewEstimates(); // 1 estimate
281+
assertEquals(1, estimates.size());
282+
283+
var estimate = estimates.get(0);
284+
285+
assertFalse(estimate.isValid());
286+
287+
// pose validity
288+
assertEquals(0, estimate.pose().getX(), 1e-4);
289+
assertEquals(0, estimate.pose().getY(), 1e-4);
290+
assertEquals(0, estimate.pose().getZ(), 1e-4);
291+
assertEquals(0, estimate.pose().getRotation().getX(), 1e-4);
292+
assertEquals(0, estimate.pose().getRotation().getY(), 1e-4);
293+
assertEquals(0, estimate.pose().getRotation().getZ(), 1e-4);
294+
295+
// should see only ID 3
296+
assertArrayEquals(new int[] {3}, estimate.detectedTags());
297+
298+
// distance validity
299+
assertEquals(
300+
_fieldLayout
301+
.getTagPose(3)
302+
.get()
303+
.getTranslation()
304+
.getDistance(Pose3d.kZero.getTranslation()),
305+
estimate.avgTagDistance(),
306+
1e-4);
307+
308+
// std devs validity
309+
assertArrayEquals(new double[] {-1, -1, -1}, estimate.stdDevs());
310+
}
311+
312+
// multi-tag doesn't work, see other comment about multi-tag above
313+
// @Test
314+
public void multiTagDistanceFilter() {
315+
// TODO
316+
}
256317

257318
@Test
258-
public void disambiguation() {}
319+
public void estimateSort() {
320+
List<VisionPoseEstimate> newEstimates = new ArrayList<>();
321+
322+
for (int i = 3; i > 0; i--) {
323+
newEstimates.add(
324+
new VisionPoseEstimate(
325+
Pose3d.kZero, i, 0.03, new int[] {1}, 1.2, new double[] {0.3, 0.1, 0.2}, true));
326+
}
327+
328+
newEstimates.add(
329+
new VisionPoseEstimate(
330+
Pose3d.kZero,
331+
2, // same timestamp case
332+
0.03,
333+
new int[] {1},
334+
1.2,
335+
new double[] {
336+
4, 3, 5
337+
}, // these are worse std devs, so it should come before the better estimate @ timestamp
338+
// = 2s
339+
true));
340+
341+
newEstimates.sort(VisionPoseEstimate.sorter);
342+
343+
// should be sorted from least timestamp to greatest
344+
assertEquals(1, newEstimates.get(0).timestamp());
345+
assertEquals(2, newEstimates.get(1).timestamp());
346+
assertEquals(2, newEstimates.get(2).timestamp());
347+
assertEquals(3, newEstimates.get(3).timestamp());
348+
349+
assertArrayEquals(new double[] {4, 3, 5}, newEstimates.get(1).stdDevs());
350+
assertArrayEquals(new double[] {0.3, 0.1, 0.2}, newEstimates.get(2).stdDevs());
351+
}
352+
353+
// beta ambiguity issue:
354+
// https://github.com/PhotonVision/photonvision/issues/1623
355+
// so this isn't gonna work
356+
// @Test
357+
public void disambiguation() {
358+
// TODO
359+
}
259360
}

0 commit comments

Comments
 (0)