diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69b284a..c218b9f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,6 +73,7 @@ //import frc.robot.subsystems.TiltSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.logging.io.gyro.RealGyroIo; import frc.robot.utils.logging.io.gyro.ThreadedGyro; @@ -123,7 +124,7 @@ public class RobotContainer { private AutoFactory autoFactory; private static AutoRoutine straightRoutine; private static AutoTrajectory straightTrajectory; - private final VisionTruster truster = new ConstantVisionTruster(visionMeasurementStdDevs2); + private final VisionTruster truster = new SquareVisionTruster(visionMeasurementStdDevs2, Constants.VISION_STD_DEV_CONST); // Replace with CommandPS4Controller or CommandJoystick if needed // new CommandXboxController(OperatorConstants.kDriverControllerPort);private diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 80cc9d0..21c7bda 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -1,5 +1,6 @@ package frc.robot.apriltags; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N3; @@ -38,12 +39,12 @@ public void simReadings() { for (Apriltag tag : Apriltag.values()) { Pose3d cameraPos = new Pose3d(pose).transformBy(Constants.ROBOT_TO_CAMERA); if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { - Pose3d adjPose = tag.getPose().relativeTo(cameraPos); - double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); - double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); - if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { - VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0); - Vector stdDevs = truster.calculateTrust(measurement); + Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation(); + double distance = adjPose2.getNorm(); + double distanceTimesCosIncidenceAngle = adjPose2.getX(); + if (distanceTimesCosIncidenceAngle!=0 && distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0, tag.number()); + Vector stdDevs = truster.calculateTrust(measurement, cameraPos); double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); @@ -51,7 +52,7 @@ public void simReadings() { distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); if (BasicVisionFilter.inBounds(readingPos)) { addReading(new ApriltagReading(readingX, readingY, readingYaw, - distance,0,tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, stdDevs.get(0),Logger.getTimestamp() / 1000.0)); + distance,Math.acos(distanceTimesCosIncidenceAngle/distance),tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, stdDevs.get(0),Logger.getTimestamp() / 1000.0)); } } } diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java index 6281909..4e31327 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import java.io.DataInputStream; import java.io.IOException; +import frc.robot.constants.Constants; public class TCPApriltagServer extends TCPServer { @@ -21,7 +22,7 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc double poseYaw = -1; double distanceToTag = -1; double cameraToTagAngle = -1; - double timestamp = -1; + double latency = -1; double stdDev = -1; int apriltagNumber = -1; double now = 0; @@ -31,18 +32,18 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc && distanceToTag == -1 && cameraToTagAngle == -1 && apriltagNumber == -1 - && timestamp == -1 + && latency == -1 && stdDev == -1) { posX = stream.readDouble(); posY = stream.readDouble(); poseYaw = stream.readDouble(); distanceToTag = stream.readDouble(); cameraToTagAngle = stream.readDouble(); - timestamp = stream.readDouble(); + latency = stream.readDouble(); stdDev = stream.readDouble(); apriltagNumber = stream.readInt(); - now = Timer.getFPGATimestamp() * 1000; + now = Timer.getFPGATimestamp() * 1000-Constants.AVERAGE_PIR_LATENCY-latency; } - return new ApriltagReading(posX, posY, poseYaw, distanceToTag, cameraToTagAngle, apriltagNumber, timestamp, stdDev, now); + return new ApriltagReading(posX, posY, poseYaw, distanceToTag, cameraToTagAngle, apriltagNumber, latency, stdDev, now); } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 89565c7..c043e67 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick @@ -169,6 +169,9 @@ public enum Mode { public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double MAX_VISION_DISTANCE_SIMULATION = 6; + public static final double AVERAGE_PIR_LATENCY = 20; //ms + public static final double VISION_STD_THRESHOLD = 0.25; + public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later public static final double MAX_HUB_DISTANCE = 5.18; public static final double MIN_HUB_DISTANCE = 1.42; diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ef7becb..df0365f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -9,6 +9,7 @@ import static edu.wpi.first.units.Units.Meter; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -318,6 +319,9 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public Pose3d getCameraPose() { + return new Pose3d(getPose()).transformBy(Constants.ROBOT_TO_CAMERA); + } public double getError() { return getPose().getTranslation().getDistance((getSimulationPose().get().getTranslation())); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 59cec74..567765c 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -1,11 +1,13 @@ package frc.robot.subsystems.swervedrive.vision.estimation; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.FilterResult; @@ -26,6 +28,7 @@ */ public class FilterablePoseManager extends PoseManager { private final VisionFilter filter; + private List acceptedTagPoses = new ArrayList<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, @@ -57,28 +60,40 @@ public FilterablePoseManager( public void processQueue() { List validMeasurementsPose = new ArrayList<>(); List invalidMeasurementsPose = new ArrayList<>(); - - LinkedHashMap filteredData = + if (Constants.currentMode == Constants.Mode.SIM) { + acceptedTagPoses = new ArrayList<>(); + } + for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { + Queue visionMeasurementQueue = queueEntry.getValue(); + LinkedHashMap filteredData = filter.filter(visionMeasurementQueue); - visionMeasurementQueue.clear(); - for (Map.Entry filterEntry : filteredData.entrySet()) { - VisionMeasurement v = filterEntry.getKey(); - FilterResult r = filterEntry.getValue(); - switch (r) { - case ACCEPTED -> { - setVisionSTD(getVisionSTD(v)); - validMeasurementsPose.add(v.measurement()); - addVisionMeasurement(v); + visionMeasurementQueue.clear(); - } - case NOT_PROCESSED -> visionMeasurementQueue.add(v); - case REJECTED -> { - invalidMeasurementsPose.add(v.measurement()); + for (Map.Entry filterEntry : filteredData.entrySet()) { + VisionMeasurement v = filterEntry.getKey(); + FilterResult r = filterEntry.getValue(); + switch (r) { + case ACCEPTED -> { + setVisionSTD(getVisionSTD(v)); + validMeasurementsPose.add(v.measurement()); + addVisionMeasurement(v); + if (Constants.currentMode == Constants.Mode.SIM) { + acceptedTagPoses.add(Apriltag.of(v.tagId()).getPose()); + } + + } + case NOT_PROCESSED -> visionMeasurementQueue.add(v); + case REJECTED -> { + invalidMeasurementsPose.add(v.measurement()); + } } } } Logger.recordOutput("Apriltag/numberAccepted", validMeasurementsPose.size()); Logger.recordOutput("Apriltag/numberRejected", invalidMeasurementsPose.size()); + if (Constants.currentMode == Constants.Mode.SIM) { + Logger.recordOutput("Apriltag/acceptedTags", acceptedTagPoses.toArray(Pose3d[]::new)); + } } public VisionTruster getVisionTruster() { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 4824cc4..c83afa8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -51,7 +51,7 @@ public class PoseEstimator { private static final double visionStdRateOfChange = 1; /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ - public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.3, 0.3, 100); + public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0,0,100); private final FilterablePoseManager poseManager; public PoseEstimator( @@ -145,6 +145,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { //double timestamp = 0; // latency is not right we are assuming zero double timestamp = apriltagSystem.getIO().getInputs().timestamp[index]; double stdDevFromCamera = apriltagSystem.getIO().getInputs().stdDev[index]; + int tagId = apriltagSystem.getIO().getInputs().apriltagNumber[index]; Pose2d visionPose; if (getEstimatedPose()!=null) { visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); @@ -152,7 +153,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { visionPose = new Pose2d(pos[0], pos[1], poseManager.getRotation()); } double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; - return new VisionMeasurement(visionPose, distanceFromTag, stdDevFromCamera, timestamp/1000); + return new VisionMeasurement(visionPose, distanceFromTag, stdDevFromCamera,timestamp/1000, tagId); } /** @@ -206,7 +207,7 @@ public FilterablePoseManager getPoseManager() { public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( - new VisionMeasurement(getEstimatedPose(), 0, 0, Logger.getTimestamp() / 1e6)); + new VisionMeasurement(getEstimatedPose(), 0, 0, Logger.getTimestamp() / 1e6, 0)); } public VisionTruster getVisionTruster() { return poseManager.getVisionTruster(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index ad4e244..83a3ba1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -15,6 +16,7 @@ import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import org.littletonrobotics.junction.Logger; +import java.util.LinkedHashMap; import java.util.LinkedList; import java.util.Optional; import java.util.Queue; @@ -26,7 +28,7 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; - protected final Queue visionMeasurementQueue = new LinkedList<>(); + protected final LinkedHashMap> visionMeasurementQueueMap = new LinkedHashMap<>(); private final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; @@ -69,19 +71,21 @@ public void registerVisionMeasurement(VisionMeasurement measurement) { if (measurement == null) { return; } - while (visionMeasurementQueue.size() >= 3) { - visionMeasurementQueue.poll(); + while (visionMeasurementQueueMap.computeIfAbsent(measurement.tagId(),k -> new LinkedList<>()).size() >= 3) { + visionMeasurementQueueMap.get(measurement.tagId()).poll(); } - visionMeasurementQueue.add(measurement); + visionMeasurementQueueMap.get(measurement.tagId()).add(measurement); } // override for filtering public void processQueue() { - VisionMeasurement m = visionMeasurementQueue.poll(); - while (m != null) { - setVisionSTD(getVisionSTD(m)); - addVisionMeasurement(m); - m = visionMeasurementQueue.poll(); + for (Queue visionMeasurementQueue : visionMeasurementQueueMap.values()) { + VisionMeasurement m = visionMeasurementQueue.poll(); + while (m != null) { + setVisionSTD(getVisionSTD(m)); + addVisionMeasurement(m); + m = visionMeasurementQueue.poll(); + } } } @@ -94,7 +98,8 @@ protected Vector getVisionSTD(VisionMeasurement measurement) { if (Constants.USE_CAMERA_APRILTAG_STD_DEV) { return VecBuilder.fill(StdDev, StdDev, StdDev); } - return visionTruster.calculateTrust(measurement); + Pose3d cameraPos = drivebase.getCameraPose(); + return visionTruster.calculateTrust(measurement, cameraPos); } protected void addVisionMeasurement(VisionMeasurement measurement) { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java index 388715c..b706b6a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -51,10 +51,8 @@ public LinkedHashMap filter( Filter poses ------------------------------------------------------- */ - Pose2d vision1Pose = getVisionPose(m1); - Pose2d vision2Pose = getVisionPose(m2); boolean valid1 = - filterVision(vision1Pose, vision2Pose, m1.timeOfMeasurement(), m2.timeOfMeasurement()); + filterVision(m1,m2); resultMap.put(m1, valid1 ? FilterResult.ACCEPTED : FilterResult.REJECTED); m1 = measurements.poll(); m2 = measurements.peek(); @@ -63,19 +61,22 @@ public LinkedHashMap filter( return resultMap; } - private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double m2Time) { - Optional odomPoseAtVis1 = poseBuffer.getSample(m1Time); - Optional odomPoseAtVis2 = poseBuffer.getSample(m2Time); + private boolean filterVision(VisionMeasurement m1, VisionMeasurement m2) { + Optional odomPoseAtVis1 = poseBuffer.getSample(m1.timeOfMeasurement()); + Optional odomPoseAtVis2 = poseBuffer.getSample(m1.timeOfMeasurement()); if (odomPoseAtVis1.isEmpty() || odomPoseAtVis2.isEmpty()) { return false; } - if (!inBounds(m1Pose) || !inBounds(m2Pose)) { + if (!inBounds(m1.measurement()) || !inBounds(m2.measurement())) { return false; } double odomDiff1To2 = odomPoseAtVis1.get().getTranslation().getDistance(odomPoseAtVis2.get().getTranslation()); - double visionDiff1To2 = m1Pose.getTranslation().getDistance(m2Pose.getTranslation()); + double visionDiff1To2 = m1.measurement().getTranslation().getDistance(m2.measurement().getTranslation()); double diff = Math.abs(odomDiff1To2 - visionDiff1To2); + if (m1.stdDev() > Constants.VISION_STD_THRESHOLD) { + return false; + } return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java index 3017ccb..fe133bb 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; @@ -11,7 +12,7 @@ public ConstantVisionTruster(Vector initialSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { return initialSTD; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java index e7f75ea..78e6f09 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public class LinearVisionTruster extends DistanceVisionTruster { @@ -14,7 +15,7 @@ public LinearVisionTruster(Vector initialSTD, double slopeSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { double std = measurement.distanceFromTag() * slopeSTD; return initialSTD.plus(VecBuilder.fill(std, std, std)); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java index 7a925fc..98d59a8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java @@ -2,7 +2,10 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N3; +import frc.robot.utils.Apriltag; public class SquareVisionTruster extends DistanceVisionTruster { @@ -15,8 +18,10 @@ public SquareVisionTruster(Vector initialSTD, double constant) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { - double std = Math.pow(measurement.distanceFromTag() - 0.4572, 2) * constant; - return initialSTD.plus(VecBuilder.fill(std, std, std)); + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { + Translation3d adjPose2 = cameraPose.relativeTo(Apriltag.of(measurement.tagId()).getPose()).getTranslation(); + double distanceTimesCosIncidenceAngle = adjPose2.getX()/adjPose2.getNorm(); + double std = Math.pow(measurement.distanceFromTag(), 2) * constant / distanceTimesCosIncidenceAngle; + return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java index e020739..d8493ff 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N3; import frc.robot.utils.Apriltag; /** @@ -11,4 +13,4 @@ * @param timeOfMeasurement time when the pose was measured (seconds) */ public record VisionMeasurement( - Pose2d measurement, double distanceFromTag, double stdDev, double timeOfMeasurement) {} + Pose2d measurement, double distanceFromTag, double stdDev, double timeOfMeasurement, int tagId) {} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java index 7ce7c3d..3627917 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public interface VisionTruster { - Vector calculateTrust(VisionMeasurement measurement); + Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose); }