Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use ReadQueue for PhotonCamera timestamps (#1316)

Browse Source 
This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -133,10 +133,9 @@ class PhotonPoseEstimatorTest {
cameraOne.result.setRecieveTimestampMicros((long) (11 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, new Transform3d());
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
@@ -224,10 +223,9 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
cameraOne,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
@@ -314,11 +312,10 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
@@ -405,12 +402,11 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
@@ -484,7 +480,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (7 * 1e6));
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
@@ -529,25 +525,24 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
@@ -557,7 +552,7 @@ class PhotonPoseEstimatorTest {
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
}
@@ -640,10 +635,9 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
@@ -659,6 +653,11 @@ class PhotonPoseEstimatorTest {
PhotonPipelineResult result;
@Override
public List<PhotonPipelineResult> getAllUnreadResults() {
return List.of(result);
}
@Override
public PhotonPipelineResult getLatestResult() {
return result;

10
photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
View File

@@ -61,7 +61,6 @@ public class ApriltagWorkbenchTest {
new PhotonPoseEstimator(
tagLayout,
PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
cam,
robotToCamera);
var field = new Field2d();
@@ -70,10 +69,11 @@ public class ApriltagWorkbenchTest {
while (!Thread.interrupted()) {
Thread.sleep(500);
var ret = pe.update();
System.out.println(ret);
field.setRobotPose(ret.get().estimatedPose.toPose2d());
for (var change : cam.getAllUnreadResults()) {
var ret = pe.update(change);
System.out.println(ret);
field.setRobotPose(ret.get().estimatedPose.toPose2d());
}
}
}
}