diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java index a1a9f83..08bd7aa 100644 --- a/vision/src/main/java/coppercore/vision/Camera.java +++ b/vision/src/main/java/coppercore/vision/Camera.java @@ -1,29 +1,20 @@ package coppercore.vision; -import coppercore.vision.CoreVisionConstants.CameraParams; import coppercore.vision.VisionLocalizer.CameraMeasurement; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import org.littletonrobotics.junction.Logger; public class Camera { - - public enum CameraTrustZone { - LEFT, - RIGHT, - MIDDLE, - } - public final String name; - public final CameraTrustZone zone; private final CameraIO io; private final CameraIOInputsAutoLogged inputs; public Camera(CameraParams params, CameraIO io) { name = params.name(); - zone = params.zone(); this.io = io; inputs = new CameraIOInputsAutoLogged(); @@ -48,42 +39,7 @@ public CameraMeasurement getLatestMeasurement() { } public Matrix getLatestVariance() { - // If the robot is not in teleop, trust cameras based on their location relative to the tags - return CoreVisionConstants.lowCameraUncertainty; // TODO: Actually calculate variances! - // if (!DriverStation.isTeleop()) { - // switch (zone) { - // case LEFT: - // if (AllianceUtil.isRightOfSpeaker(inputs.latestFieldToRobot.getY(), 2)) { - // return VisionConstants.lowCameraUncertainty; - // } - // break; - // case RIGHT: - // if (AllianceUtil.isLeftOfSpeaker(inputs.latestFieldToRobot.getY(), 2)) { - // return VisionConstants.lowCameraUncertainty; - // } - // break; - // case MIDDLE: - // break; - // } - // } - - // // If the robot is very close, trust highly - // if (inputs.averageTagDistanceM < VisionConstants.skewCutoffDistance) { - // return DriverStation.isTeleop() - // ? VisionConstants.teleopCameraUncertainty - // : VisionConstants.lowCameraUncertainty; - // // If the robot is somewhat close, check if the cameras are at extreme angles, and - // trust - // // accordingly - // } else if (inputs.averageTagDistanceM < VisionConstants.lowUncertaintyCutoffDistance) { - // return Math.abs(inputs.averageTagYaw.getDegrees()) < - // VisionConstants.skewCutoffRotation - // ? VisionConstants.lowCameraUncertainty - // : VisionConstants.highCameraUncertainty; - // // If the robot is past the final distance cutoff, distrust - // } else { - // return VisionConstants.highCameraUncertainty; - // } + return VecBuilder.fill(0.0, 0.0, 0.0); } } diff --git a/vision/src/main/java/coppercore/vision/CameraContainerReal.java b/vision/src/main/java/coppercore/vision/CameraContainerReal.java index b898130..fcf6bc5 100644 --- a/vision/src/main/java/coppercore/vision/CameraContainerReal.java +++ b/vision/src/main/java/coppercore/vision/CameraContainerReal.java @@ -1,6 +1,6 @@ package coppercore.vision; -import coppercore.vision.CoreVisionConstants.CameraParams; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import java.util.ArrayList; import java.util.List; @@ -8,9 +8,9 @@ public class CameraContainerReal implements CameraContainer { private List cameras = new ArrayList<>(); - public CameraContainerReal(List params) { + public CameraContainerReal(List params, AprilTagFieldLayout layout) { for (CameraParams param : params) { - cameras.add(new Camera(param, CameraIOPhoton.fromRealCameraParams(param))); + cameras.add(new Camera(param, CameraIOPhoton.fromRealCameraParams(param, layout))); } } diff --git a/vision/src/main/java/coppercore/vision/CameraContainerReplay.java b/vision/src/main/java/coppercore/vision/CameraContainerReplay.java index 6980656..1d207df 100644 --- a/vision/src/main/java/coppercore/vision/CameraContainerReplay.java +++ b/vision/src/main/java/coppercore/vision/CameraContainerReplay.java @@ -1,6 +1,5 @@ package coppercore.vision; -import coppercore.vision.CoreVisionConstants.CameraParams; import java.util.ArrayList; import java.util.List; diff --git a/vision/src/main/java/coppercore/vision/CameraContainerSim.java b/vision/src/main/java/coppercore/vision/CameraContainerSim.java index 438b51b..dc215fc 100644 --- a/vision/src/main/java/coppercore/vision/CameraContainerSim.java +++ b/vision/src/main/java/coppercore/vision/CameraContainerSim.java @@ -1,6 +1,6 @@ package coppercore.vision; -import coppercore.vision.CoreVisionConstants.CameraParams; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Twist2d; @@ -36,12 +36,13 @@ public class CameraContainerSim implements CameraContainer { public CameraContainerSim( List params, + AprilTagFieldLayout layout, Pose2d initialPose, SwerveDriveKinematics kinematics, Supplier getModuleStates) { this.getModuleStates = getModuleStates; - visionSim.addAprilTags(CoreVisionConstants.fieldLayout); + visionSim.addAprilTags(layout); this.kinematics = kinematics; @@ -53,7 +54,9 @@ public CameraContainerSim( for (CameraParams param : params) { cameras.add( - new Camera(param, CameraIOPhoton.fromSimCameraParams(param, visionSim, true))); + new Camera( + param, + CameraIOPhoton.fromSimCameraParams(param, layout, visionSim, true))); } } diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java index 7f2fcf7..7978692 100644 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java @@ -1,6 +1,6 @@ package coppercore.vision; -import coppercore.vision.CoreVisionConstants.CameraParams; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; @@ -22,26 +22,26 @@ public class CameraIOPhoton implements CameraIO { private double latestTimestampSeconds = 0.0; - public CameraIOPhoton(String name, Transform3d robotToCamera) { - this(new PhotonCamera(name), robotToCamera); + public CameraIOPhoton(String name, AprilTagFieldLayout layout, Transform3d robotToCamera) { + this(new PhotonCamera(name), layout, robotToCamera); } - public CameraIOPhoton(PhotonCamera camera, Transform3d robotToCamera) { + public CameraIOPhoton( + PhotonCamera camera, AprilTagFieldLayout layout, Transform3d robotToCamera) { this.camera = camera; poseEstimator = new PhotonPoseEstimator( - CoreVisionConstants.fieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCamera); + layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera); } - public static CameraIOPhoton fromRealCameraParams(CameraParams params) { - return new CameraIOPhoton(params.name(), params.robotToCamera()); + public static CameraIOPhoton fromRealCameraParams( + CameraParams params, AprilTagFieldLayout layout) { + return new CameraIOPhoton(params.name(), layout, params.robotToCamera()); } public static CameraIOPhoton fromSimCameraParams( - CameraParams params, VisionSystemSim sim, boolean stream) { + CameraParams params, AprilTagFieldLayout layout, VisionSystemSim sim, boolean stream) { PhotonCamera camera = new PhotonCamera(params.name()); SimCameraProperties props = new SimCameraProperties(); @@ -55,7 +55,7 @@ public static CameraIOPhoton fromSimCameraParams( cameraSim.enableRawStream(stream); cameraSim.enableProcessedStream(stream); - return new CameraIOPhoton(camera, params.robotToCamera()); + return new CameraIOPhoton(camera, layout, params.robotToCamera()); } @Override @@ -91,6 +91,7 @@ public void updateInputs(CameraIOInputs inputs) { } private static boolean filterPhotonPose(EstimatedRobotPose photonPose) { + // TODO: Actual pose rejection if (photonPose.targetsUsed.size() < 2) { return false; } @@ -101,6 +102,11 @@ private static boolean filterPhotonPose(EstimatedRobotPose photonPose) { return false; } + if (calculateAverageTagDistance(photonPose) + > CoreVisionConstants.maxAcceptedDistanceMeters) { + return false; + } + return true; } diff --git a/vision/src/main/java/coppercore/vision/CameraParams.java b/vision/src/main/java/coppercore/vision/CameraParams.java new file mode 100644 index 0000000..a92b1f5 --- /dev/null +++ b/vision/src/main/java/coppercore/vision/CameraParams.java @@ -0,0 +1,12 @@ +package coppercore.vision; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; + +public record CameraParams( + String name, + int xResolution, + int yResolution, + int fps, + Rotation2d fov, + Transform3d robotToCamera) {} diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java index 2a66a44..432f992 100644 --- a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java +++ b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java @@ -1,61 +1,12 @@ package coppercore.vision; -import coppercore.vision.Camera.CameraTrustZone; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.IOException; -import java.util.Collections; - +/** + * Vision constants for coppercore's internal vision constants. Not to be confused with + * VisionConstants, which is likely your robot project's local vision constants that are configured + * per-project. + */ public final class CoreVisionConstants { - public static final String tagLayoutName = "Pairs-Only"; - public static final AprilTagFieldLayout fieldLayout = initLayout(tagLayoutName); - - public static final double lowUncertaintyCutoffDistance = 6.5; - - public static final double skewCutoffDistance = 5.8; - public static final double skewCutoffRotation = Units.degreesToRadians(50); - - public static final Matrix teleopCameraUncertainty = VecBuilder.fill(0.35, 0.35, 3.5); - - public static final Matrix lowCameraUncertainty = VecBuilder.fill(0.6, 1.0, 4); - - public static final Matrix highCameraUncertainty = VecBuilder.fill(12.0, 16.0, 40); - - public static final Matrix driveUncertainty = VecBuilder.fill(0.1, 0.1, 0.1); - - public static record CameraParams( - String name, - int xResolution, - int yResolution, - int fps, - Rotation2d fov, - Transform3d robotToCamera, - CameraTrustZone zone) {} - - private static AprilTagFieldLayout initLayout(String name) { - AprilTagFieldLayout layout; - // AprilTagFieldLayout's constructor throws an IOException, so we must catch it - // in order to initialize our layout as a static constant - try { - layout = - new AprilTagFieldLayout( - Filesystem.getDeployDirectory().getAbsolutePath() - + "/taglayout/" - + name - + ".json"); - } catch (IOException ioe) { - DriverStation.reportWarning( - "Failed to load AprilTag Layout: " + ioe.getLocalizedMessage(), false); - layout = new AprilTagFieldLayout(Collections.emptyList(), 0.0, 0.0); - } - return layout; - } + // TODO: Tune this value + // This value mainly exists so that this file stays named CoreVisionConstants. + public static final double maxAcceptedDistanceMeters = 10.0; } diff --git a/vision/src/main/java/coppercore/vision/LayoutUtil.java b/vision/src/main/java/coppercore/vision/LayoutUtil.java new file mode 100644 index 0000000..368fc7e --- /dev/null +++ b/vision/src/main/java/coppercore/vision/LayoutUtil.java @@ -0,0 +1,28 @@ +package coppercore.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.IOException; +import java.util.Collections; + +public class LayoutUtil { + public static AprilTagFieldLayout initLayout(String name) { + AprilTagFieldLayout layout; + // AprilTagFieldLayout's constructor throws an IOException, so we must catch it + // in order to initialize our layout as a static constant + try { + layout = + new AprilTagFieldLayout( + Filesystem.getDeployDirectory().getAbsolutePath() + + "/taglayout/" + + name + + ".json"); + } catch (IOException ioe) { + DriverStation.reportWarning( + "Failed to load AprilTag Layout: " + ioe.getLocalizedMessage(), false); + layout = new AprilTagFieldLayout(Collections.emptyList(), 0.0, 0.0); + } + return layout; + } +}