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

Handle IOException in Apriltag example (#807)

Browse Source 
* Deal with IOException

* Fix import
This commit is contained in:
Andrew Gasser
2023-02-14 13:30:30 -06:00
committed by GitHub
parent abe32a1aae
commit 9ac1050264
1 changed files with 21 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
View File

@@ -27,6 +27,7 @@ package frc.robot;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import frc.robot.Constants.VisionConstants;
import java.io.IOException;
import java.util.Optional;
@@ -36,25 +37,27 @@ import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
public class PhotonCameraWrapper {
public PhotonCamera photonCamera;
public PhotonPoseEstimator photonPoseEstimator;
private PhotonCamera photonCamera;
private PhotonPoseEstimator photonPoseEstimator;
public PhotonCameraWrapper() {
AprilTagFieldLayout atfl = null;
try {
atfl = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
} catch (IOException e) {
e.printStackTrace();
}
// Change the name of your camera here to whatever it is in the PhotonVision UI.
photonCamera = new PhotonCamera(VisionConstants.cameraName);
// Create pose estimator
photonPoseEstimator =
new PhotonPoseEstimator(
atfl, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam);
photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
try {
// Attempt to load the AprilTagFieldLayout that will tell us where the tags are on the field.
AprilTagFieldLayout fieldLayout = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField();
// Create pose estimator
photonPoseEstimator =
new PhotonPoseEstimator(
fieldLayout, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam);
photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
} catch (IOException e) {
// The AprilTagFieldLayout failed to load. We won't be able to estimate poses if we don't know
// where the tags are.
DriverStation.reportError("Failed to load AprilTagFieldLayout", e.getStackTrace());
photonPoseEstimator = null;
}
}
/**
@@ -63,6 +66,10 @@ public class PhotonCameraWrapper {
* the estimate
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
if (photonPoseEstimator == null) {
// The field layout failed to load, so we cannot estimate poses.
return Optional.empty();
}
photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
return photonPoseEstimator.update();
}