From 32bc319a8aea5c4e66cb1fc064489545bcdc0c0d Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Mon, 13 Jan 2025 20:29:45 -0500 Subject: [PATCH 1/4] what is goin on photon? --- .../java/frc/robot/vision/VisionConfig.java | 6 +- .../frc/robot/vision/VisionPoseEstimator.java | 63 ++++++++++++++++++- 2 files changed, 64 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index d7db065..3232249 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -50,7 +50,5 @@ public class VisionConfig { new Transform3d( new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)) - ); - - -} + ); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/VisionPoseEstimator.java b/src/main/java/frc/robot/vision/VisionPoseEstimator.java index fa10600..27d4cb3 100644 --- a/src/main/java/frc/robot/vision/VisionPoseEstimator.java +++ b/src/main/java/frc/robot/vision/VisionPoseEstimator.java @@ -1,7 +1,68 @@ package frc.robot.vision; +import java.util.List; +import java.util.concurrent.atomic.AtomicReference; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; + import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class VisionPoseEstimator { +public class VisionPoseEstimator extends SubsystemBase { + public class PhotonRun implements Runnable { + + private final PhotonCamera photonCamera; + private final Transform3d robotToCam; + private final PhotonPoseEstimator poseEstimator; + private final AtomicReference atomicRobotPose = new AtomicReference(); + + public PhotonRun(PhotonCamera cam, Transform3d robotToCam) { + //declares photoncamera, camera position, and prepares the pose estimator for initialization + //photon camera not needed anymore apparently? needs testing + this.photonCamera = cam; + this.robotToCam = robotToCam; + PhotonPoseEstimator photonEstimator = null; + + try { + var layout = AprilTagFieldLayout.loadField(VisionConfig.aprilTagField); + layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + + if (robotToCam != null){ + photonEstimator = new PhotonPoseEstimator(layout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + robotToCam); + } + + } catch (Exception e) { + DriverStation.reportError("Failed to load AprilTag field", e.getStackTrace()); + photonEstimator = null; + } + this.poseEstimator = photonEstimator; + } + + + @Override + public void run() { + if (poseEstimator != null && photonCamera != null){ + var photonResults = photonCamera.getAllUnreadResults(); + PhotonPipelineResult target = new PhotonPipelineResult(); + + + } + } + + public EstimatedRobotPose getLatestEstimatedPose(){ + return atomicRobotPose.getAndSet(getLatestEstimatedPose()); + } + + } + } From ac19b343ff809ddacefad3bea0f9eb5460ec40e8 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Fri, 17 Jan 2025 20:01:50 -0500 Subject: [PATCH 2/4] Vision 2025 real code fr fr fr --- src/main/java/frc/robot/vision/Vision.java | 102 ++++++++++++++++++ .../java/frc/robot/vision/VisionConfig.java | 35 +++--- .../frc/robot/vision/VisionPoseEstimator.java | 68 ------------ 3 files changed, 124 insertions(+), 81 deletions(-) create mode 100644 src/main/java/frc/robot/vision/Vision.java delete mode 100644 src/main/java/frc/robot/vision/VisionPoseEstimator.java 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..e0444e1 --- /dev/null +++ b/src/main/java/frc/robot/vision/Vision.java @@ -0,0 +1,102 @@ +package frc.robot.vision; + +import static frc.robot.vision.VisionConfig.*; + +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.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Vision extends SubsystemBase { + + + /** + * This code is a modified version of the example found in: + * https://github.com/PhotonVision/photonvision/blob/main/photonlib-java-examples/poseest + * + * The modifications (should) allow it to run pose estimators using multiple cameras and detect objects + */ + + public class VisionEstimator { + private final PhotonCamera photonCamera; + private final Transform3d robotToCam; + private final PhotonPoseEstimator poseEstimator; + private Matrix stdDevs; + + + public VisionEstimator(PhotonCamera camName, Transform3d camLocation) { + this.photonCamera = camName; + this.robotToCam = camLocation; + + // declares a new photon pose estimator using the given parameters + // also sets the fall back strategy to LOWEST_AMBIGUITY in case MULTI_TAG_PNP_ON_COPROCESSOR fails + poseEstimator = new PhotonPoseEstimator( + tagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camLocation); + poseEstimator.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 : photonCamera.getAllUnreadResults()) { + visionEst = poseEstimator.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 + stdDevs = singleTagStdDevs; + + } else { + // Pose present. Start running 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 = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += tagPose.get().toPose2d().getTranslation().getDistance( + estimatedPose.get().estimatedPose.toPose2d().getTranslation() + ); + } + + } + + } + } + } diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index 3232249..2b17e7d 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -2,32 +2,41 @@ 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 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 all Photoncameras that will be used in pose estimator and commands + public static final PhotonCamera centerCam = new PhotonCamera("Center_Pose_Cam"); + public static final PhotonCamera leftCam = new PhotonCamera("Left_Pose_Cam"); + public static final PhotonCamera rightCam = new PhotonCamera("Right_Pose_Cam"); + public static final PhotonCamera driveCam = new PhotonCamera("Drive_Cam"); - //Creates field layout for AprilTags - public static AprilTagFields aprilTagField = AprilTagFields.k2025Reefscape; + // 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 - //TODO: config + // TODO: config public static final Transform3d robotToCenterCam = new Transform3d( new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)), 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 27d4cb3..0000000 --- a/src/main/java/frc/robot/vision/VisionPoseEstimator.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc.robot.vision; - -import java.util.List; -import java.util.concurrent.atomic.AtomicReference; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class VisionPoseEstimator extends SubsystemBase { - - public class PhotonRun implements Runnable { - - private final PhotonCamera photonCamera; - private final Transform3d robotToCam; - private final PhotonPoseEstimator poseEstimator; - private final AtomicReference atomicRobotPose = new AtomicReference(); - - public PhotonRun(PhotonCamera cam, Transform3d robotToCam) { - //declares photoncamera, camera position, and prepares the pose estimator for initialization - //photon camera not needed anymore apparently? needs testing - this.photonCamera = cam; - this.robotToCam = robotToCam; - PhotonPoseEstimator photonEstimator = null; - - try { - var layout = AprilTagFieldLayout.loadField(VisionConfig.aprilTagField); - layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide); - - if (robotToCam != null){ - photonEstimator = new PhotonPoseEstimator(layout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCam); - } - - } catch (Exception e) { - DriverStation.reportError("Failed to load AprilTag field", e.getStackTrace()); - photonEstimator = null; - } - this.poseEstimator = photonEstimator; - } - - - @Override - public void run() { - if (poseEstimator != null && photonCamera != null){ - var photonResults = photonCamera.getAllUnreadResults(); - PhotonPipelineResult target = new PhotonPipelineResult(); - - - } - } - - public EstimatedRobotPose getLatestEstimatedPose(){ - return atomicRobotPose.getAndSet(getLatestEstimatedPose()); - } - - } - -} From 075b8ed3b544089864fa6086a75afa91dbcfdbca Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Sat, 15 Feb 2025 11:59:57 -0500 Subject: [PATCH 3/4] New way to estimate pose w multicam 7028 is goated and im not --- src/main/java/frc/robot/vision/Vision.java | 148 +++++++++--------- .../java/frc/robot/vision/VisionConfig.java | 42 ++--- 2 files changed, 84 insertions(+), 106 deletions(-) diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index e0444e1..7a571c9 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -1,6 +1,8 @@ package frc.robot.vision; -import static frc.robot.vision.VisionConfig.*; +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; @@ -12,91 +14,91 @@ 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; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Vision extends SubsystemBase { +/* + * 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 { - /** - * This code is a modified version of the example found in: - * https://github.com/PhotonVision/photonvision/blob/main/photonlib-java-examples/poseest - * - * The modifications (should) allow it to run pose estimators using multiple cameras and detect objects - */ + private final PhotonCamera cam; + private final PhotonPoseEstimator photonEstimator; + private Matrix curStdDevs; - public class VisionEstimator { - private final PhotonCamera photonCamera; - private final Transform3d robotToCam; - private final PhotonPoseEstimator poseEstimator; - private Matrix stdDevs; - + //Construct Vision Instance + public Vision(String cameraName, Transform3d camTransform){ + cam = new PhotonCamera(cameraName); - public VisionEstimator(PhotonCamera camName, Transform3d camLocation) { - this.photonCamera = camName; - this.robotToCam = camLocation; + photonEstimator = new PhotonPoseEstimator( + tagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camTransform); + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } - // declares a new photon pose estimator using the given parameters - // also sets the fall back strategy to LOWEST_AMBIGUITY in case MULTI_TAG_PNP_ON_COPROCESSOR fails - poseEstimator = new PhotonPoseEstimator( - tagLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camLocation); - poseEstimator.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; - /** - * 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 : photonCamera.getAllUnreadResults()) { - visionEst = poseEstimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets()); + //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; } - return visionEst; + numTags++; + avgDist += tagPose.get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); } - - /** - * 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 - stdDevs = singleTagStdDevs; - - } else { - // Pose present. Start running 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 = poseEstimator.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 } } } + +} diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index 2b17e7d..9d58d6b 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -16,17 +16,16 @@ 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"; + public static final String[] camNames = new String[] {"Center_Cam", "Left_Cam", "Right_Cam", "Drive_Cam"}; - - // Creates all Photoncameras that will be used in pose estimator and commands - public static final PhotonCamera centerCam = new PhotonCamera("Center_Pose_Cam"); - public static final PhotonCamera leftCam = new PhotonCamera("Left_Pose_Cam"); - public static final PhotonCamera rightCam = new PhotonCamera("Right_Pose_Cam"); - public static final PhotonCamera driveCam = new PhotonCamera("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); @@ -36,28 +35,5 @@ public class VisionConfig { 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 - // 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)) - ); } \ No newline at end of file From a4846f6fa93fe7d3750fbf608d1c42bbd878c2a0 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Fri, 21 Feb 2025 19:48:41 -0500 Subject: [PATCH 4/4] vision final --- src/main/java/frc/robot/vision/Vision.java | 24 ++++++++++- .../java/frc/robot/vision/VisionConsumer.java | 25 +++++++++++ .../commands/PhotonEstimatorCommand.java | 42 +++++++++++++++++++ 3 files changed, 89 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/vision/VisionConsumer.java create mode 100644 src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 7a571c9..784b522 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -96,9 +96,29 @@ private void updateEstimationStdDevs(Optional estimatedPose, //No tags visible. Default to single-tag std devs curStdDevs = singleTagStdDevs; } else { - //One or more tags visible, run the full heuristic + // 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/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/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; + } +}