diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java new file mode 100644 index 0000000..784b522 --- /dev/null +++ b/src/main/java/frc/robot/vision/Vision.java @@ -0,0 +1,124 @@ +package frc.robot.vision; + +import static frc.robot.vision.VisionConfig.multiTagStdDevs; +import static frc.robot.vision.VisionConfig.singleTagStdDevs; +import static frc.robot.vision.VisionConfig.tagLayout; + +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/* + * This code is a modified version of Photonvision's own example and 7028's vision code: + * https://github.com/PhotonVision/photonvision/blob/main/photonlib-java-examples/poseest + * https://github.com/STMARobotics/frc-7028-2025 + */ + +public class Vision { + + private final PhotonCamera cam; + private final PhotonPoseEstimator photonEstimator; + private Matrix curStdDevs; + + //Construct Vision Instance + public Vision(String cameraName, Transform3d camTransform){ + cam = new PhotonCamera(cameraName); + + photonEstimator = new PhotonPoseEstimator( + tagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camTransform); + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

+ * Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + for (var change : cam.getAllUnreadResults()) { + visionEst = photonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + } + return visionEst; + } + /** + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + * deviations based on number of tags, estimation strategy, and distance from the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs(Optional estimatedPose, List targets){ + if (estimatedPose.isEmpty()){ + //No pose input. Default to single-tag std devs + curStdDevs = singleTagStdDevs; + + } else { + //Pose present, start Heuristic + var estStdDevs = singleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + //Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()){ + continue; + } + numTags++; + avgDist += tagPose.get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0){ + //No tags visible. Default to single-tag std devs + curStdDevs = singleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + estStdDevs = multiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) { + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + } else { + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + } + curStdDevs = estStdDevs; + } + } + } + + /** + * Returns the latest standard deviations of the estimated pose from {@link + * #getEstimatedGlobalPose()}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; + } +} diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index d7db065..9d58d6b 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -2,55 +2,38 @@ import org.photonvision.PhotonCamera; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; public class VisionConfig { - //Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard - public static final String centerPoseCamName = "Center_Pose_Cam"; - public static final String leftPoseCamName = "Left_Pose_Cam"; - public static final String rightPoseCamName = "Right_Pose_Cam"; - public static final String driveCamName = "Drive_Cam"; - - - //Creates all Photoncameras that will be used in pose estimator and commands - public static PhotonCamera centerCam = new PhotonCamera(centerPoseCamName); - public static PhotonCamera leftCam = new PhotonCamera(leftPoseCamName); - public static PhotonCamera rightCam = new PhotonCamera(rightPoseCamName); - public static PhotonCamera driveCam = new PhotonCamera(driveCamName); - - //Creates field layout for AprilTags - public static AprilTagFields aprilTagField = AprilTagFields.k2025Reefscape; - - - //TODO: config - public static final Transform3d robotToCenterCam = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d robotToLeftCam = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d robotToRightCam = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - public static final Transform3d robotToDriveCam = - new Transform3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), - new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); + // Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard + public static final String[] camNames = new String[] {"Center_Cam", "Left_Cam", "Right_Cam", "Drive_Cam"}; + + //Camera Positions + // TODO: config camera transforms + public static final Transform3d[] robotToCamTransforms = new Transform3d[] { + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()) + }; + + // Creates field layout for AprilTags + public static final AprilTagFieldLayout tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + + // Standard deviation of vision poses, this helps with correction or something idk thats what photon said + // TODO: experiment with standard deviation values and set them to whatever gives the most correct pose + public static final Matrix singleTagStdDevs = VecBuilder.fill(4, 4, 8); // TODO: example values, change when testing + public static final Matrix multiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); //TODO: change values when testing -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionConsumer.java b/src/main/java/frc/robot/vision/VisionConsumer.java new file mode 100644 index 0000000..b26bbb8 --- /dev/null +++ b/src/main/java/frc/robot/vision/VisionConsumer.java @@ -0,0 +1,25 @@ +package frc.robot.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +@FunctionalInterface +public interface VisionConsumer { + /** + * Adds a vision measurement. + *

+ * Note that the vision measurement standard deviations passed into this method + * will continue to apply to future measurements. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement in the form [x, y, theta]ᵀ, with + * units in meters and radians. + */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs); +} diff --git a/src/main/java/frc/robot/vision/VisionPoseEstimator.java b/src/main/java/frc/robot/vision/VisionPoseEstimator.java deleted file mode 100644 index fa10600..0000000 --- a/src/main/java/frc/robot/vision/VisionPoseEstimator.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; - -public class VisionPoseEstimator { - -} diff --git a/src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java b/src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java new file mode 100644 index 0000000..231672e --- /dev/null +++ b/src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java @@ -0,0 +1,42 @@ +package frc.robot.vision.commands; + +import static frc.robot.vision.VisionConfig.camNames; +import static frc.robot.vision.VisionConfig.robotToCamTransforms; + +import java.util.Arrays; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.vision.Vision; +import frc.robot.vision.VisionConfig.*; +import frc.robot.vision.VisionConsumer; + +public class PhotonEstimatorCommand extends Command{ + private final Vision[] visions; + private final VisionConsumer visionConsumer; + + public PhotonEstimatorCommand(VisionConsumer consumer){ + visions = new Vision[camNames.length]; + for (int i = 0; i < camNames.length; i++){ + visions[i] = new Vision(camNames[i], robotToCamTransforms[i]); + } + this.visionConsumer = consumer; + } + + @Override + public void execute(){ + Arrays.stream(visions).forEach(vision -> { + var visionEst = vision.getEstimatedGlobalPose(); + + visionEst.ifPresent(est -> { + var estStdDevs = vision.getEstimationStdDevs(); + + visionConsumer.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); + }); + }); + } + + @Override + public boolean runsWhenDisabled(){ + return true; + } +}