Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41: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

78
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -90,7 +90,6 @@ public class PhotonPoseEstimator {
private TargetModel tagModel = TargetModel.kAprilTag36h11;
private PoseStrategy primaryStrategy;
private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
private final PhotonCamera camera;
private Transform3d robotToCamera;
private Pose3d lastPose;
@@ -107,31 +106,21 @@ public class PhotonPoseEstimator {
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCamera
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
*/
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags,
PoseStrategy strategy,
PhotonCamera camera,
Transform3d robotToCamera) {
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.primaryStrategy = strategy;
this.camera = camera;
this.robotToCamera = robotToCamera;
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
InstanceCount++;
}
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this(fieldTags, strategy, null, robotToCamera);
}
/** Invalidates the pose cache. */
private void invalidatePoseCache() {
poseCacheTimestampSeconds = -1;
@@ -288,31 +277,8 @@ public class PhotonPoseEstimator {
}
/**
* Poll data from the configured cameras and update the estimated position of the robot. Returns
* empty if:
*
* <ul>
* <li>New data has not been received since the last call to {@code update()}.
* <li>No targets were found from the camera
* <li>There is no camera set
* </ul>
*
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update() {
if (camera == null) {
DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false);
return Optional.empty();
}
PhotonPipelineResult cameraResult = camera.getLatestResult();
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
}
/**
* Updates the estimated position of the robot. Returns empty if:
* Updates the estimated position of the robot, assuming no camera calibration is required for the
* selected strategy. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
@@ -320,13 +286,15 @@ public class PhotonPoseEstimator {
* <li>No targets were found in the pipeline results.
* </ul>
*
* Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not
* provided
*
* @param cameraResult The latest pipeline result from the camera
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty());
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
return update(cameraResult, Optional.empty(), Optional.empty());
}
/**
@@ -338,10 +306,10 @@ public class PhotonPoseEstimator {
* <li>No targets were found in the pipeline results.
* </ul>
*
* @param cameraMatrix Camera calibration data that can be used in the case of no assigned
* PhotonCamera.
* @param distCoeffs Camera calibration data that can be used in the case of no assigned
* PhotonCamera
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
@@ -378,7 +346,7 @@ public class PhotonPoseEstimator {
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
Optional<EstimatedRobotPose> estimatedPose = Optional.empty();
switch (strat) {
case LOWEST_AMBIGUITY:
estimatedPose = lowestAmbiguityStrategy(cameraResult);
@@ -397,10 +365,20 @@ public class PhotonPoseEstimator {
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP_ON_RIO:
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
if (cameraMatrix.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
} else if (distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
} else {
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
}
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
estimatedPose = multiTagOnCoprocStrategy(cameraResult);
break;
default:
DriverStation.reportError(
@@ -415,10 +393,7 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt) {
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
var best =
@@ -433,7 +408,8 @@ public class PhotonPoseEstimator {
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
} else {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
// We can nver fall back on another multitag strategy
return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
}
}