diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a1801a1..8bbd756 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -183,7 +183,8 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getFieldRelativeHeading, + drive::isPoseAsserted, + () -> drive.getPose().getRotation(), new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), @@ -224,7 +225,8 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getFieldRelativeHeading, + drive::isPoseAsserted, + () -> drive.getPose().getRotation(), new VisionIOPhotonVisionSim( cameraFrontRightName, robotToFrontRightCamera, drive::getPose), new VisionIOPhotonVisionSim( @@ -271,7 +273,8 @@ public Robot() { vision = new Vision( drive::addVisionMeasurement, - drive::getFieldRelativeHeading, + drive::isPoseAsserted, + () -> drive.getPose().getRotation(), new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 61e1e1e..81c7e62 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -78,8 +78,7 @@ public class Drive extends SubsystemBase { }; private SwerveDrivePoseEstimator visionPose = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d()); - private boolean firstVisionEstimate = true; - private boolean poseInitialized = false; + private boolean poseAsserted = false; private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds(); private final SwerveModuleState[] emptyModuleStates = new SwerveModuleState[] {}; @@ -388,12 +387,12 @@ public Pose2d getPose() { } /** - * Returns the field-relative heading for vision yaw validation, or null if the pose has not yet - * been initialized by vision or an auto routine. Returning null causes the yawConsistency test to - * be skipped, avoiding false rejections before the heading has field-relative meaning. + * Returns whether the pose estimator has been authoritatively initialized by an auto routine. + * Before initialization, the gyro heading is robot-relative and should not be used for + * field-relative validation such as the vision yawConsistency test. */ - public Rotation2d getFieldRelativeHeading() { - return poseInitialized ? getPose().getRotation() : null; + public boolean isPoseAsserted() { + return poseAsserted; } public Rotation2d getRawGyroRotation() { @@ -403,7 +402,7 @@ public Rotation2d getRawGyroRotation() { /** Resets the pose estimator to the given pose. */ public void setPose(Pose2d pose) { visionPose.resetPosition(rawGyroRotation, getModulePositions(), pose); - poseInitialized = true; + poseAsserted = true; } /** Adds a new timestamped vision measurement. */ @@ -411,10 +410,11 @@ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - // Initialize pose from the first vision estimate while disabled - if (firstVisionEstimate && RobotState.isDisabled()) { - setPose(visionRobotPoseMeters); - firstVisionEstimate = false; + // Track the latest vision estimate while disabled, before the pose is authoritatively + // initialized by an auto routine. Does not set poseAsserted; yawConsistency is gated + // in Vision.java on poseAsserted or accumulated accepted-observation count. + if (!poseAsserted && RobotState.isDisabled()) { + visionPose.resetPosition(rawGyroRotation, getModulePositions(), visionRobotPoseMeters); } visionPose.addVisionMeasurement( diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index c967e83..eac51e4 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -31,12 +31,14 @@ import java.io.IOException; import java.util.ArrayList; import java.util.EnumSet; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { private final VisionConsumer consumer; - private final Supplier gyroYawSupplier; + private final BooleanSupplier poseAssertedSupplier; + private final Supplier headingSupplier; private final VisionIO[] io; private final VisionInputs[] visionInputs; private final VisionIOInputsAutoLogged[] inputs; @@ -75,12 +77,21 @@ public class Vision extends SubsystemBase { // Cycle counter for throttled logging private int loopCounter = 0; + // Set true when multiple cameras independently agree on a pose, confirming the heading + // is field-relative. Enables yawConsistency without requiring an authoritative pose from auto. + private boolean headingCorroborated = false; + // Vision tests to apply (remove from set to disable specific tests) public static final EnumSet enabledTests = VisionFilter.DEFAULT_ENABLED_TESTS; - public Vision(VisionConsumer consumer, Supplier gyroYawSupplier, VisionIO... io) { + public Vision( + VisionConsumer consumer, + BooleanSupplier poseAssertedSupplier, + Supplier headingSupplier, + VisionIO... io) { this.consumer = consumer; - this.gyroYawSupplier = gyroYawSupplier; + this.poseAssertedSupplier = poseAssertedSupplier; + this.headingSupplier = headingSupplier; this.io = io; // Initialize per-camera velocity tracking arrays @@ -177,7 +188,9 @@ public void periodic() { cameraIndex, lastAcceptedPose[cameraIndex], lastAcceptedTimestamp[cameraIndex], - gyroYawSupplier.get(), + (poseAssertedSupplier.getAsBoolean() || headingCorroborated) + ? headingSupplier.get() + : null, enabledTests); observationBuffer.add(tested); @@ -241,6 +254,11 @@ public void periodic() { double linearStdDev = linearStdDevBaseline * cameraCountFactor / fused.score(); double angularStdDev = angularStdDevBaseline * cameraCountFactor / fused.score(); + // Multi-camera agreement corroborates the heading, enabling yawConsistency + if (fused.cameraCount() > 1) { + headingCorroborated = true; + } + // Send fused vision observation to the pose estimator consumer.accept( fused.pose(),