1. "Quest Ready" - Indicates if the Oculus Quest SLAM system is connected and initialized + * + *
2. "Tags Ready" - Indicates if the AprilTag vision system is connected and initialized + * + *
3. "System Ready" - Indicates if both localization systems are ready and not in emergency + * state + * + *
Each indicator is configured with a size of 2x1 grid units and positioned vertically in the + * Prematch tab. The indicators automatically update based on system state changes and will show + * as green when true and red when false. + * + *
Layout: + * + *
+ * [Quest Ready] + * [Tags Ready] + * [System Ready] + *+ */ + private void setupShuffleboardLogging() { + prematchTab + .addBoolean("Quest Ready", () -> oculusSource.isConnected() && questInitialized) + .withSize(2, 1) + .withPosition(0, 0); + + prematchTab + .addBoolean("Tags Ready", () -> tagSource.isConnected() && tagInitialized) + .withSize(2, 1) + .withPosition(0, 1); + + prematchTab + .addBoolean( + "System Ready", + () -> + (oculusSource.isConnected() && questInitialized) + && (tagSource.isConnected() && tagInitialized) + && state.getCurrentState() != LocalizationState.State.EMERGENCY) + .withSize(2, 1) + .withPosition(0, 2); + } + + /** + * Gets the starting pose from the currently selected autonomous routine in PathPlanner. + * + * @return The starting Pose2d from the selected auto, or null if none available + */ + private Pose2d getAutoStartingPose() { + try { + String autoName = autoChooser.getSendableChooser().getSelected(); + if (autoName.equals("None")) { + Logger.recordOutput("LocalizationFusion/Event", "No auto selected"); + return null; + } + + Logger.recordOutput("LocalizationFusion/SelectedAuto", autoName); + + var autoPath = PathPlannerAuto.getPathGroupFromAutoFile(autoName); + if (autoPath.isEmpty()) { + Logger.recordOutput("LocalizationFusion/Event", "No paths found in auto: " + autoName); + return null; + } + + var startPose = autoPath.get(0).getStartingHolonomicPose(); + if (startPose.isPresent()) { + if (AutoBuilder.shouldFlip()) { + return FlippingUtil.flipFieldPose(startPose.get()); + } else { + return startPose.get(); + } + } else { + Logger.recordOutput( + "LocalizationFusion/Event", "No starting pose defined in auto: " + autoName); + return null; + } + } catch (Exception e) { + Logger.recordOutput( + "LocalizationFusion/Event", "Error getting auto starting pose: " + e.getMessage()); + return null; + } + } + + /** + * Periodic update method called by the command scheduler. Handles system state updates, source + * initialization, and pose processing. + */ + @Override + public void periodic() { + logSystemStatus(); + handleDriverStationConnection(); + updateAutoSelection(); // Automatically set pose to auto path start pose if we don't have any + // other valid poses + + // Handle initialization of both sources + if (!questInitialized && oculusSource.isConnected()) { + handleQuestInitialization(); + } + if (!tagInitialized && tagSource.isConnected()) { + handleTagInitialization(); + } + + // State-specific handling + double currentTime = Timer.getTimestamp(); + switch (state.getCurrentState()) { + case RESETTING: + handleResettingState(); + break; + case EMERGENCY: + handleEmergencyState(); + break; + case QUEST_PRIMARY: + handleQuestPrimaryState(currentTime); + break; + case TAG_BACKUP: + handleTagBackupState(currentTime); + break; + case UNINITIALIZED: + handleUninitializedState(); + break; + } + + logDetailedStatus(); + } + + // -------------------- State Handlers -------------------- + /** + * Handles the RESETTING state where the system is attempting to align Quest and AprilTag poses. + * Monitors timeout conditions and validates pose alignment before transitioning to primary state. + */ + private void handleResettingState() { + if (resetStartTime > 0 && Timer.getTimestamp() - resetStartTime > RESET_TIMEOUT) { + Logger.recordOutput( + "LocalizationFusion/Event", "Reset sequence timed out - switching to emergency mode"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Reset Failed") + .withDescription("Reset sequence timed out - Check vision systems") + .withDisplaySeconds(10.0)); + resetStartTime = 0.0; + resetQuestInitialization(); + state.transitionTo(LocalizationState.State.EMERGENCY); + return; + } + + // If we have no tag pose, but Quest is connected and initialized + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose == null) { + // If this was triggered by auto selection and Quest is ready, + // we can proceed without tag validation + if (!hasTagValidatedPose && oculusSource.isConnected() && !isResetInProgress()) { + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose != null) { + resetStartTime = 0.0; + lastQuestUpdate = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + return; + } + } + + if (resetStartTime == 0.0) { + resetStartTime = Timer.getTimestamp(); + } + return; + } + + if (!oculusSource.isConnected() || !tagSource.isConnected()) { + return; + } + + if (isResetInProgress()) { + return; + } + + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose == null) { + return; + } + + double poseError = questPose.getTranslation().getDistance(tagPose.getTranslation()); + if (poseError <= INIT_VALIDATION_THRESHOLD) { + resetStartTime = 0.0; + lastQuestUpdate = Timer.getTimestamp(); + hasTagValidatedPose = true; // Now we have tag validation + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + } + } + + /** + * Handles the EMERGENCY state where normal operation has failed. Attempts to recover using + * AprilTag detection and reinitialize the system. + */ + private void handleEmergencyState() { + if (tagSource.isConnected()) { + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + Logger.recordOutput( + "LocalizationFusion/Event", + "Valid AprilTag pose detected in EMERGENCY state - attempting recovery"); + + if (oculusSource.isConnected() && resetToPose(tagPose)) { + resetStartTime = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.RESETTING); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Recovery Attempt") + .withDescription("Valid AprilTags found - Attempting to restore tracking") + .withDisplaySeconds(3.0)); + } else { + // If Quest isn't available, fall back to tag-only tracking + state.transitionTo(LocalizationState.State.TAG_BACKUP); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Partial Recovery") + .withDescription("Switching to AprilTag-only tracking") + .withDisplaySeconds(3.0)); + } + } + } + } + + /** + * Handles the QUEST_PRIMARY state where Quest SLAM is the primary pose source. Validates poses + * and handles Quest disconnection scenarios. + * + * @param currentTime The current system timestamp + */ + private void handleQuestPrimaryState(double currentTime) { + if (currentTime - lastUpdateTime < POSE_UPDATE_INTERVAL) { + return; + } + lastUpdateTime = currentTime; + + if (!oculusSource.isConnected()) { + if (questInitialized) { + lastQuestDisconnectTime = Timer.getTimestamp(); + hadPreviousCalibration = true; + lastQuestInitPose = oculusSource.getCurrentPose(); + } + state.transitionTo(LocalizationState.State.TAG_BACKUP); + return; + } + + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose != null) { + if (validatePose(questPose, false)) { + // Try auto realign if its enabled + if (LocalizationFusionConstants.AutoRealignConstants.ENABLED) { + tryAutoRealign(currentTime); + } + + poseConsumer.accept(questPose, currentTime, oculusSource.getStdDevs()); + lastQuestUpdate = currentTime; + } else { + if (currentTime - lastQuestUpdate > QUEST_INIT_TIMEOUT) { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } + } + } + + /** + * Handles the TAG_BACKUP state where AprilTag detection is the primary pose source. Attempts + * Quest reconnection when possible and processes AprilTag poses. + * + * @param currentTime The current system timestamp + */ + private void handleTagBackupState(double currentTime) { + if (currentTime - lastUpdateTime < POSE_UPDATE_INTERVAL) { + return; + } + lastUpdateTime = currentTime; + + if (oculusSource.isConnected() && hadPreviousCalibration) { + + Pose2d questPose = oculusSource.getCurrentPose(); + if (validatePose(questPose, true)) { + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + Logger.recordOutput( + "LocalizationFusion/Event", + "Quest Reconnected; Pose Validated - maintaining Quest tracking"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Quest Reconnected") + .withDescription("Quest tracking restored - Pose validated") + .withDisplaySeconds(3.0)); + return; + } else { + // Re-init as we couldn't validate pose + Logger.recordOutput( + "LocalizationFusion/Event", + "Quest Reconnected; Pose Failed to Validate - reinitializing Quest tracking"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Quest Validation Failed") + .withDescription("Quest reconnected but pose invalid - Reinitializing") + .withDisplaySeconds(3.0)); + handleQuestInitialization(); + } + } + + if (oculusSource.isConnected() && !hadPreviousCalibration) { + handleQuestInitialization(); + return; + } + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + poseConsumer.accept(tagPose, currentTime, tagSource.getStdDevs()); + } + } + + /** + * Handles the UNINITIALIZED state where the system is starting up. Attempts to initialize using + * AprilTag detection. + */ + private void handleUninitializedState() { + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + if (DriverStation.isDSAttached() && resetToPose(tagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + } + } + } + + // -------------------- Initialization Methods -------------------- + /** + * Handles the initialization of the Quest SLAM system. Manages calibration, validation, and state + * transitions during Quest startup. + */ + private void handleQuestInitialization() { + if (!oculusSource.isConnected()) { + if (questInitialized) { + lastQuestDisconnectTime = Timer.getTimestamp(); + hadPreviousCalibration = true; + lastQuestInitPose = oculusSource.getCurrentPose(); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Quest Disconnected") + .withDescription("Quest tracking system disconnected - Check USB connection") + .withDisplaySeconds(10.0)); + } + resetQuestInitialization(); + return; + } + + // Skip validation during mid-match reboot + if (isMidMatchReboot()) { + questInitialized = true; + lastQuestUpdate = Timer.getTimestamp(); + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + return; + } + + Pose2d questPose = oculusSource.getCurrentPose(); + if (questPose == null) { + return; + } + + if (questInitStartTime == 0.0) { + questInitStartTime = Timer.getTimestamp(); + lastQuestInitPose = questPose; + return; + } + + if (isNewPoseValid(questPose, lastQuestInitPose, INIT_VALIDATION_THRESHOLD)) { + consecutiveValidQuestUpdates++; + lastQuestInitPose = questPose; + } else { + consecutiveValidQuestUpdates = 0; + } + + double initDuration = Timer.getTimestamp() - questInitStartTime; + if (consecutiveValidQuestUpdates >= MIN_QUEST_VALID_UPDATES + && initDuration >= QUEST_INIT_TIMEOUT) { + questInitialized = true; + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Quest Initialized") + .withDescription("Quest tracking system successfully initialized") + .withDisplaySeconds(3.0)); + Pose2d referencePose = getValidReferencePose(); + if (referencePose != null && resetToPose(referencePose)) { + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); + } + } else if (initDuration > QUEST_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Quest Init Failed") + .withDescription("Quest initialization timed out - Check headset") + .withDisplaySeconds(10.0)); + resetQuestInitialization(); + } + } + + /** + * Handles the initialization of the AprilTag vision system. Manages detection validation and + * state transitions during AprilTag startup. + */ + private void handleTagInitialization() { + if (!tagSource.isConnected()) { + if (tagInitialized) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("AprilTag System Disconnected") + .withDescription("AprilTag tracking system disconnected - Check camera connection") + .withDisplaySeconds(10.0)); + } + resetTagInitialization(); + return; + } + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose == null) { + return; + } + + if (tagInitStartTime == 0.0) { + tagInitStartTime = Timer.getTimestamp(); + lastTagInitPose = tagPose; + return; + } + + if (isNewPoseValid(tagPose, lastTagInitPose, INIT_VALIDATION_THRESHOLD)) { + consecutiveValidTagUpdates++; + lastTagInitPose = tagPose; + } else { + consecutiveValidTagUpdates = 0; + } + + double initDuration = Timer.getTimestamp() - tagInitStartTime; + if (consecutiveValidTagUpdates >= MIN_TAG_VALID_UPDATES && initDuration >= TAG_INIT_TIMEOUT) { + tagInitialized = true; + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("AprilTag System Ready") + .withDescription("AprilTag tracking system successfully initialized") + .withDisplaySeconds(3.0)); + + if (!questInitialized || !shouldPreferQuest()) { + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } else if (initDuration > TAG_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("AprilTag Init Failed") + .withDescription("AprilTag initialization timed out - Check camera view of tags") + .withDisplaySeconds(3.0)); + resetTagInitialization(); + } + } + + // -------------------- Connection Management -------------------- + /** + * Checks if we're rebooting during an active match by examining match time. A non-zero match time + * indicates the FMS->DS->Robot connection is active and we're in a match that was already + * running. + * + * @return true if this appears to be a mid-match reboot + */ + private boolean isMidMatchReboot() { + return DriverStation.isEnabled() && Timer.getMatchTime() > 0; + } + + /** + * Initializes the DriverStation connection state tracking. Called during subsystem construction + * to handle pre-connected scenarios and mid-match reboots. + */ + private void initializeConnectionState() { + wasConnected = DriverStation.isDSAttached(); + hadInitialConnection = wasConnected; + + if (isMidMatchReboot()) { + Logger.recordOutput( + "LocalizationFusion/Event", "Mid-match reboot detected - maintaining Quest tracking"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Mid-Match Reboot") + .withDescription("Maintaining Quest tracking after reboot") + .withDisplaySeconds(3.0)); + // Skip initialization and trust Quest's maintained tracking + questInitialized = true; + hadPreviousCalibration = true; + + if (oculusSource.isConnected()) { + // Go straight to Quest primary if available + state.transitionTo(LocalizationState.State.QUEST_PRIMARY); + } else { + // Fallback to tags if Quest isn't ready yet + state.transitionTo(LocalizationState.State.TAG_BACKUP); + } + } else if (hadInitialConnection) { + state.transitionTo(LocalizationState.State.UNINITIALIZED); + needsInitialReset = true; + } + } + + /** + * Handles DriverStation connection state changes and triggers appropriate system responses. + * Manages initial connection validation and pose recalibration during disabled state. + */ + private void handleDriverStationConnection() { + boolean isConnected = DriverStation.isDSAttached(); + + if (!wasConnected && isConnected) { + if (!hadInitialConnection) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Initial Connection") + .withDescription("Driver Station connected - Starting pose validation") + .withDisplaySeconds(3.0)); + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose != null) { + lastValidatedPose = tagPose; + initialPoseValidationStartTime = Timer.getTimestamp(); + initialPoseValidated = false; + } + hadInitialConnection = true; + } else { + Logger.recordOutput( + "LocalizationFusion/Event", "DriverStation reconnected - maintaining current pose"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Connection Restored") + .withDescription("Driver Station reconnected - Maintaining current pose") + .withDisplaySeconds(3.0)); + } + } + + if (isConnected && DriverStation.isDisabled()) { + handleDisabledPoseValidation(); + } + + wasConnected = isConnected; + } + + // -------------------- Validation Methods -------------------- + /** + * Handles pose validation during the disabled state. Only performs validation and resets before + * the match starts to avoid disrupting auto-to-teleop transitions. + */ + private void handleDisabledPoseValidation() { + // Skip disabled validation if match has started + if (DriverStation.getMatchTime() > 0) { + return; + } + + Pose2d currentTagPose = tagSource.getCurrentPose(); + + // If we're in EMERGENCY state and we see tags, attempt to recover + if (state.getCurrentState() == LocalizationState.State.EMERGENCY && currentTagPose != null) { + Logger.recordOutput( + "LocalizationFusion/Event", + "Tags detected while in EMERGENCY state during disabled - attempting recovery"); + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); + return; + } + } + + if (currentTagPose == null) return; + + if (!initialPoseValidated) { + if (Timer.getTimestamp() - initialPoseValidationStartTime >= INITIAL_POSE_STABILITY_TIME) { + if (lastValidatedPose != null) { + double poseChange = + currentTagPose.getTranslation().getDistance(lastValidatedPose.getTranslation()); + + if (poseChange <= INIT_VALIDATION_THRESHOLD) { + Logger.recordOutput( + "LocalizationFusion/Event", + "Initial pose validated after stability period - initiating reset"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Initial Pose Validated") + .withDescription("Robot position confirmed stable - Starting tracking") + .withDisplaySeconds(3.0)); + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); + initialPoseValidated = true; + } + } else { + Logger.recordOutput( + "LocalizationFusion/Event", + "Pose changed during validation - restarting stability timer"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Position Changed") + .withDescription("Robot moved during validation - Restarting stability check") + .withDisplaySeconds(3.0)); + initialPoseValidationStartTime = Timer.getTimestamp(); + lastValidatedPose = currentTagPose; + } + } + } + lastValidatedPose = currentTagPose; + return; + } + + // Only check for pose changes requiring recalibration before match starts + if (lastValidatedPose != null) { + double poseChange = + currentTagPose.getTranslation().getDistance(lastValidatedPose.getTranslation()); + + if (poseChange >= DISABLED_RECALIBRATION_THRESHOLD) { + Logger.recordOutput( + "LocalizationFusion/Event", + String.format( + "Significant pre-match pose change detected while disabled (%.2fm) - recalibrating", + poseChange)); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Pre-Match Position Changed") + .withDescription( + String.format( + "Robot moved %.2f meters while disabled - Recalibrating", poseChange)) + .withDisplaySeconds(3.0)); + + initialPoseValidationStartTime = Timer.getTimestamp(); + lastValidatedPose = currentTagPose; + initialPoseValidated = false; + + // Attempt immediate reset with new pose + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = Timer.getTimestamp(); + } + } + } + + lastValidatedPose = currentTagPose; + } + + /** + * Validates a pose against the current AprilTag reference. Uses different thresholds for + * initialization and normal operation. + * + * @param pose The pose to validate + * @param strict True if we require tags to be present for validation, false to assume valid with + * no tags + * @return true if the pose is valid, false otherwise + */ + private boolean validatePose(Pose2d pose, boolean strict) { + if (pose == null) return false; + + Pose2d tagPose = tagSource.getCurrentPose(); + if (tagPose == null) { + return !strict; + } + + double poseError = tagPose.getTranslation().getDistance(pose.getTranslation()); + boolean isValid; + + if (!questInitialized) { + isValid = poseError <= INIT_VALIDATION_THRESHOLD; + } else { + isValid = poseError <= APRILTAG_VALIDATION_THRESHOLD; + } + + if (isValid) { + hasTagValidatedPose = true; // Update this instead of hasValidPoseOffset + } + + return isValid; + } + + /** + * Validates a new pose against a previous pose using maximum change thresholds. + * + * @param newPose The new pose to validate + * @param lastPose The previous pose to compare against + * @param maxChange The maximum allowed position change in meters + * @return true if the pose change is within acceptable limits + */ + private boolean isNewPoseValid(Pose2d newPose, Pose2d lastPose, double maxChange) { + if (lastPose == null) return true; + + double poseChange = newPose.getTranslation().getDistance(lastPose.getTranslation()); + double rotationChange = + Math.abs(newPose.getRotation().minus(lastPose.getRotation()).getDegrees()); + + return poseChange <= maxChange && rotationChange <= MAX_ROTATION_CHANGE_DEGREES; + } + + /** + * Gets a valid reference pose from the AprilTag system if available. + * + * @return A valid reference Pose2d or null if unavailable + */ + private Pose2d getValidReferencePose() { + if (tagInitialized) { + return tagSource.getCurrentPose(); + } + return null; + } + + // -------------------- Reset Methods -------------------- + /** + * Attempts to perform an auto-realignment if conditions are met. + * + * @param currentTime the current system timestamp + * @return true if realignment was attempted + */ + private boolean tryAutoRealign(double currentTime) { + + if (!LocalizationFusionConstants.AutoRealignConstants.ENABLED + || !DriverStation.isTeleopEnabled()) { + return false; + } + + // Check cooldown period + if (currentTime - lastAutoRealignTime + < LocalizationFusionConstants.AutoRealignConstants.COOLDOWN) { + return false; + } + + // Get current poses + Pose2d questPose = oculusSource.getCurrentPose(); + Pose2d tagPose = tagSource.getCurrentPose(); + + if (questPose == null || tagPose == null) { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + return false; + } + + // Verify stability + if (!checkStability(questPose, currentTime)) { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + return false; + } + + // Check pose error + double poseError = questPose.getTranslation().getDistance(tagPose.getTranslation()); + if (poseError > LocalizationFusionConstants.AutoRealignConstants.THRESHOLD) { + String message = String.format("Auto-realignment triggered (error: %.2fm)", poseError); + Logger.recordOutput("LocalizationFusion/Event", message); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Auto Realignment") + .withDescription(message) + .withDisplaySeconds(3.0)); + + if (resetToPose(tagPose)) { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + state.transitionTo(LocalizationState.State.RESETTING); + resetStartTime = currentTime; + // Reset stability tracking after realignment + stabilityStartTime = 0.0; + stabilityStartPose = null; + return true; + } + } else { + lastAutoRealignTime = currentTime; // Rate limit so we arent dumping calls + Logger.recordOutput( + "LocalizationFusion/Event", + String.format("Auto-realignment skipped! (error: %.2fm)", poseError)); + } + + return false; + } + + /** + * Attempts to reset the Quest pose to match a target pose. + * + * @param pose The target pose to reset to + * @return true if reset was initiated successfully + */ + private boolean resetToPose(Pose2d pose) { + return oculusSource.subsystem.resetToPose(pose); + } + + /** + * Checks if a pose reset operation is currently in progress. + * + * @return true if a reset is in progress + */ + private boolean isResetInProgress() { + return oculusSource.subsystem.isPoseResetInProgress(); + } + + /** + * Manually triggers a pose reset using the current AprilTag pose. + * + * @return true if reset was initiated successfully + */ + public boolean requestResetOculusPoseViaAprilTags() { + Pose2d currentTagPose = tagSource.getCurrentPose(); + if (currentTagPose == null) { + Logger.recordOutput( + "LocalizationFusion/Event", "Manual reset failed - no AprilTag pose available"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Reset Failed") + .withDescription("Manual reset failed - No AprilTags visible") + .withDisplaySeconds(3.0)); + return false; + } + + Logger.recordOutput( + "LocalizationFusion/Event", + "Manual reset requested using AprilTag pose - initiating pose reset"); + if (resetToPose(currentTagPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + hasTagValidatedPose = true; + return true; + } + return false; + } + + /** + * Manually triggers a pose reset to a specific pose. + * + * @param targetPose The pose to reset to + * @return true if reset was initiated successfully + */ + public boolean requestResetOculusPose(Pose2d targetPose) { + if (targetPose == null) { + Logger.recordOutput("LocalizationFusion/Event", "Manual reset failed - null pose provided"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Reset Failed") + .withDescription("Manual reset failed - Invalid pose provided") + .withDisplaySeconds(3.0)); + return false; + } + + Logger.recordOutput( + "LocalizationFusion/Event", + "Manual reset requested with custom pose - initiating pose reset"); + if (resetToPose(targetPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + hasTagValidatedPose = true; // Set on manual reset + return true; + } + return false; + } + + // -------------------- State Management Helpers -------------------- + /** + * Checks if the robot has been stable (minimal pose change) for the required time period. + * + * @param currentPose the current robot pose + * @param currentTime the current system timestamp + * @return true if the robot has been stable for the required duration + */ + private boolean checkStability(Pose2d currentPose, double currentTime) { + // Start stability check if not already started + if (stabilityStartTime == 0.0) { + stabilityStartTime = currentTime; + stabilityStartPose = currentPose; + return false; + } + + // Check if we've moved too much since starting stability check + double poseChange = + currentPose.getTranslation().getDistance(stabilityStartPose.getTranslation()); + if (poseChange > LocalizationFusionConstants.AutoRealignConstants.MAX_MOVEMENT) { + // Reset stability check + stabilityStartTime = currentTime; + stabilityStartPose = currentPose; + return false; + } + + // Check if we've been stable long enough + return (currentTime - stabilityStartTime) + >= LocalizationFusionConstants.AutoRealignConstants.STABILITY_TIME; + } + + /** Updates the stored auto selection and initiates reset if needed */ + private void updateAutoSelection() { + String selectedAuto = autoChooser.get().getName(); + currentAutoSelection = (selectedAuto != null) ? selectedAuto : ""; + + // Update pose if either: + // 1. We don't have any pose source (!hasTagValidatedPose && !hasAutoPose) + // 2. We only have an auto pose (!hasTagValidatedPose && hasAutoPose) and auto changed + if (!hasTagValidatedPose + && (!hasAutoPose || !currentAutoSelection.equals(previousAutoSelection))) { + + Logger.recordOutput( + "LocalizationFusion/Event", + "Auto selection changed from '" + + previousAutoSelection + + "' to '" + + currentAutoSelection + + "'"); + + Pose2d autoPose = getAutoStartingPose(); + if (autoPose != null) { + Logger.recordOutput( + "LocalizationFusion/Event", "Updating pose to new auto starting position"); + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Auto Selection Changed") + .withDescription("Updating robot position for new auto: " + currentAutoSelection) + .withDisplaySeconds(3.0)); + + if (resetToPose(autoPose)) { + state.transitionTo(LocalizationState.State.RESETTING); + lastValidatedPose = autoPose; + hasAutoPose = true; + } + } + } + + previousAutoSelection = currentAutoSelection; + } + + /** Resets all Quest initialization state variables. */ + private void resetQuestInitialization() { + questInitStartTime = 0.0; + consecutiveValidQuestUpdates = 0; + lastQuestInitPose = null; + questInitialized = false; + } + + /** Resets all AprilTag initialization state variables. */ + private void resetTagInitialization() { + tagInitStartTime = 0.0; + consecutiveValidTagUpdates = 0; + lastTagInitPose = null; + tagInitialized = false; + } + + /** + * Determines whether Quest should be preferred as the primary pose source. Considers + * initialization status, match state, and pose validity. + * + * @return true if Quest should be the primary source + */ + private boolean shouldPreferQuest() { + if (!questInitialized) return false; + if (!tagInitialized) return true; + + boolean isMidMatch = + DriverStation.isEnabled() && Timer.getMatchTime() > MATCH_STARTUP_PERIOD_SECONDS; + + if (isMidMatch + && Timer.getTimestamp() - questInitStartTime + < QUEST_INIT_TIMEOUT * QUEST_INIT_GRACE_MULTIPLIER) { + return false; + } + + return validatePose(oculusSource.getCurrentPose(), false); + } + + // -------------------- Logging Methods -------------------- + /** Logs basic system status information including state and connection status. */ + private void logSystemStatus() { + Logger.recordOutput("LocalizationFusion/State", state.getCurrentState().getDescription()); + Logger.recordOutput("LocalizationFusion/OculusConnected", oculusSource.isConnected()); + Logger.recordOutput("LocalizationFusion/AprilTagConnected", tagSource.isConnected()); + } + + /** Logs detailed system status including initialization states and update counts. */ + private void logDetailedStatus() { + Logger.recordOutput("LocalizationFusion/QuestInitialized", questInitialized); + Logger.recordOutput("LocalizationFusion/TagInitialized", tagInitialized); + Logger.recordOutput("LocalizationFusion/QuestHadCalibration", hadPreviousCalibration); + + if (questInitialized) { + Logger.recordOutput("LocalizationFusion/QuestValidUpdates", consecutiveValidQuestUpdates); + Logger.recordOutput("LocalizationFusion/LastQuestUpdate", lastQuestUpdate); + } + if (tagInitialized) { + Logger.recordOutput("LocalizationFusion/TagValidUpdates", consecutiveValidTagUpdates); + } + } + + /** + * Logs state transitions with descriptive messages. + * + * @param from The previous state + * @param to The new state + */ + @Override + public void logTransition(LocalizationState.State from, LocalizationState.State to) { + Logger.recordOutput( + "LocalizationFusion/StateTransition", + String.format("State transition: %s -> %s", from.name(), to.name())); + + // Send appropriate notifications based on state transitions + switch (to) { + case EMERGENCY: + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.ERROR) + .withTitle("Localization Emergency") + .withDescription( + "Localization system entered emergency state - Check vision systems") + .withDisplaySeconds(3.0)); + break; + case RESETTING: + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Localization Reset") + .withDescription("Realigning Quest and AprilTag poses") + .withDisplaySeconds(3.0)); + break; + case QUEST_PRIMARY: + if (from == LocalizationState.State.TAG_BACKUP) { + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.INFO) + .withTitle("Quest Tracking Restored") + .withDescription("Switched back to primary Quest-based tracking") + .withDisplaySeconds(3.0)); + } + break; + case TAG_BACKUP: + Elastic.sendAlert( + new Elastic.ElasticNotification() + .withLevel(Elastic.ElasticNotification.NotificationLevel.WARNING) + .withTitle("Using Backup Tracking") + .withDescription("Switched to AprilTag-based tracking") + .withDisplaySeconds(3.0)); + break; + } + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java new file mode 100644 index 00000000..e6a9a8b0 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationFusionConstants.java @@ -0,0 +1,106 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +import lombok.experimental.UtilityClass; + +/** + * Constants used by the LocalizationFusion subsystem for robot pose estimation and tracking. Groups + * related constants into inner classes for better organization. + */ +@UtilityClass +public class LocalizationFusionConstants { + + /** + * Constants related to pose validation and thresholds. Defines distance and rotation thresholds + * for validating poses from different sources. + */ + @UtilityClass + public static class ValidationThresholds { + /** Maximum acceptable difference between AprilTag and Quest poses for validation (meters). */ + public static final double APRILTAG_VALIDATION_THRESHOLD = 1.0; + + /** Stricter threshold used during initialization phase for validating poses (meters). */ + public static final double INIT_VALIDATION_THRESHOLD = 0.3; + + /** Maximum allowed pose change during disabled state to trigger recalibration (meters). */ + public static final double DISABLED_RECALIBRATION_THRESHOLD = 0.05; + + /** Maximum allowed rotation change between poses (degrees). */ + public static final double MAX_ROTATION_CHANGE_DEGREES = 35.0; + } + + /** + * Constants related to the auto-realignment feature for correcting Quest drift using AprilTags. + * This system monitors robot stability and pose error to trigger automatic realignments when the + * robot is stationary and significant drift is detected. + */ + @UtilityClass + public class AutoRealignConstants { + /** Whether auto-realignment should be enabled. */ + public static final boolean ENABLED = true; + + /** Threshold for auto-realignment when pose error exceeds this value (meters). */ + public static final double THRESHOLD = 0.1; + + /** Maximum pose change over stability period to be considered stable (meters). */ + public static final double MAX_MOVEMENT = 0.05; + + /** Time robot must be stable before realigning (seconds). */ + public static final double STABILITY_TIME = 1.0; + + /** Minimum time between auto-realignments (seconds). */ + public static final double COOLDOWN = 10.0; + } + + /** + * Constants related to timing and update intervals. Defines various timeouts, intervals, and + * timing windows used in the localization system. + */ + @UtilityClass + public static class Timing { + /** Update interval matching Quest's native 120Hz update rate (seconds). */ + public static final double POSE_UPDATE_INTERVAL = 1.0 / 120.0; + + /** Time required for Quest initialization to complete (seconds). */ + public static final double QUEST_INIT_TIMEOUT = 2.0; + + /** Time required for AprilTag initialization to complete (seconds). */ + public static final double TAG_INIT_TIMEOUT = 1.0; + + /** Minimum time required to validate initial pose stability (seconds). */ + public static final double INITIAL_POSE_STABILITY_TIME = 15.0; + + /** Maximum time to wait for reset sequence to complete (seconds). */ + public static final double RESET_TIMEOUT = 5.0; + + /** Time after match start to consider it in mid-match state (seconds). */ + public static final double MATCH_STARTUP_PERIOD_SECONDS = 5.0; + } + + /** + * Constants related to initialization requirements and validation counts. Defines minimum update + * counts and multipliers for system initialization. + */ + @UtilityClass + public static class InitializationRequirements { + /** Minimum number of valid Quest updates required for successful initialization. */ + public static final int MIN_QUEST_VALID_UPDATES = 10; + + /** Minimum number of valid AprilTag updates required for successful initialization. */ + public static final int MIN_TAG_VALID_UPDATES = 3; + + /** Multiplier for extended Quest initialization grace period. */ + public static final double QUEST_INIT_GRACE_MULTIPLIER = 3.0; + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationState.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationState.java new file mode 100644 index 00000000..5ba4acc2 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/LocalizationState.java @@ -0,0 +1,123 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +import lombok.Getter; + +/** + * Manages the state machine for the robot localization system. + * + *
This class implements a hierarchical state machine that controls transitions between different + * pose estimation sources based on their availability and reliability. The system follows this + * hierarchy: + * + *
1. Quest SLAM as primary source (120Hz updates) 2. AprilTags as backup source 3. Odometry as + * emergency fallback + * + *
State transitions are logged for debugging and monitoring purposes. + */ +public class LocalizationState { + + /** + * Represents the possible states of the localization system. States are ordered by initialization + * sequence and fallback hierarchy. + */ + public enum State { + /** + * System is waiting for initial pose acquisition. Valid transitions: - To RESETTING when + * initial AprilTag pose is acquired + */ + UNINITIALIZED("Waiting for initial pose"), + + /** + * System is resetting to a reference pose. Valid transitions: - To QUEST_PRIMARY when reset + * completes successfully - To TAG_BACKUP if Quest becomes unavailable during reset + */ + RESETTING("Resetting to reference pose"), + + /** + * System is using Quest SLAM as primary pose source. Valid transitions: - To TAG_BACKUP if + * Quest connection is lost or validation fails - To EMERGENCY if both Quest and AprilTags are + * unavailable + */ + QUEST_PRIMARY("Using Quest as primary source"), + + /** + * System is using AprilTags as backup pose source. Valid transitions: - To QUEST_PRIMARY when + * Quest connection is restored - To EMERGENCY if AprilTags become unavailable + */ + TAG_BACKUP("Using AprilTags as backup"), + + /** + * System is using only wheel odometry for pose estimation. Valid transitions: - To TAG_BACKUP + * when AprilTags become available - To QUEST_PRIMARY when Quest connection is restored + */ + EMERGENCY("Using only odometry"); + + @Getter private final String description; + + /** + * Creates a new State with the specified description. + * + * @param description Human-readable description of the state + */ + State(String description) { + this.description = description; + } + } + + /** Current state of the localization system */ + @Getter private State currentState = State.UNINITIALIZED; + + /** Logger for state transitions */ + @Getter private final StateTransitionLogger transitionLogger; + + /** + * Creates a new LocalizationState instance. + * + * @param logger Logger to record state transitions + */ + public LocalizationState(StateTransitionLogger logger) { + this.transitionLogger = logger; + } + + /** + * Attempts to transition to a new state. The transition will be logged through the + * StateTransitionLogger. Invalid transitions will be logged but not executed. + * + * @param newState The target state to transition to + */ + public void transitionTo(State newState) { + if (newState != currentState) { + // Log state transition + transitionLogger.logTransition(currentState, newState); + + // Execute transition + currentState = newState; + } + } + + /** + * Checks if the system is currently in any of the specified states. This is useful for checking + * multiple valid states at once. + * + * @param states Variable number of states to check against + * @return true if current state matches any of the specified states + */ + public boolean isInState(State... states) { + for (State state : states) { + if (currentState == state) return true; + } + return false; + } +} diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseSource.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseSource.java new file mode 100644 index 00000000..2d9c4348 --- /dev/null +++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseSource.java @@ -0,0 +1,78 @@ +/* +* ALOTOBOTS - FRC Team 5152 + https://github.com/5152Alotobots +* Copyright (C) 2024 ALOTOBOTS +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Source code must be publicly available on GitHub or an alternative web accessible site +*/ +package frc.alotobots.library.subsystems.vision.localizationfusion; + +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; + +/** + * Standardized interface for pose estimation sources in the robot localization system. + * + *
This interface defines the contract that all pose sources must fulfill to integrate with the + * localization fusion system. It supports various types of pose sources including: + * + *
- Meta Quest SLAM (primary source) - AprilTag vision (secondary source) - Wheel odometry + * (emergency backup) - Other potential sources (e.g., LiDAR, external cameras) + * + *
Each source must provide: - Connection status monitoring - Current pose estimation - + * Measurement uncertainty estimates - Source identification + */ +public interface PoseSource { + + /** + * Checks if the pose source is currently connected and providing valid data. + * + *
This method should: - Verify hardware connectivity (if applicable) - Check data freshness - + * Validate sensor readings - Monitor update frequency + * + * @return true if the source is connected and providing valid data + */ + boolean isConnected(); + + /** + * Gets the most recent pose estimate from this source. + * + *
The returned pose should be: - In field-relative coordinates - Using the standard FRC + * coordinate system: - Origin at field corner - +X towards opposite alliance wall - +Y towards + * driver station - CCW positive rotation + * + *
If no valid pose is available, returns null. + * + * @return The current robot pose estimate, or null if unavailable + */ + Pose2d getCurrentPose(); + + /** + * Gets the standard deviations of measurement uncertainty for this pose source. + * + *
Returns a 3x1 matrix containing standard deviations for: - X position (meters) - Y position + * (meters) - Rotation (radians) + * + *
These values should: - Reflect real measurement uncertainty - Scale with distance/conditions
+ * - Account for systematic errors - Consider environmental factors
+ *
+ * @return 3x1 matrix of standard deviations [x, y, theta]
+ */
+ Matrix Used for: - Logging - Debugging - User interfaces - Status reporting
+ *
+ * @return String identifier for this pose source
+ */
+ String getSourceName();
+}
diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseVisionConsumer.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseVisionConsumer.java
new file mode 100644
index 00000000..fa6c11ae
--- /dev/null
+++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/PoseVisionConsumer.java
@@ -0,0 +1,31 @@
+/*
+* ALOTOBOTS - FRC Team 5152
+ https://github.com/5152Alotobots
+* Copyright (C) 2024 ALOTOBOTS
+*
+* This program is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Source code must be publicly available on GitHub or an alternative web accessible site
+*/
+package frc.alotobots.library.subsystems.vision.localizationfusion;
+
+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;
+
+/** Functional interface for consuming pose updates from the fusion system. */
+@FunctionalInterface
+public interface PoseVisionConsumer {
+ /**
+ * Accepts a new pose update from the fusion system.
+ *
+ * @param pose The current robot pose in field coordinates
+ * @param timestampSeconds The timestamp when this pose was measured
+ * @param stdDevs Standard deviations for x, y, and rotation measurements
+ */
+ void accept(Pose2d pose, double timestampSeconds, Matrix This interface provides a standardized way to record and monitor state machine transitions
+ * within the localization system. It enables:
+ *
+ * - Real-time monitoring of system behavior - Post-match analysis of state changes - Debugging
+ * of unexpected transitions - Performance tracking and optimization - System health monitoring
+ *
+ * Implementations should consider: - Logging to NetworkTables for real-time monitoring -
+ * Persistent storage for post-match analysis - Timestamp recording for timing analysis - Context
+ * preservation for debugging - Rate limiting for high-frequency transitions
+ */
+public interface StateTransitionLogger {
+
+ /**
+ * Records a state transition in the localization system.
+ *
+ * This method is called whenever the system transitions between states. Implementations
+ * should: - Record the timestamp of the transition - Log both the previous and new states -
+ * Include relevant context if available - Handle rapid transitions appropriately - Ensure thread
+ * safety
+ *
+ * Common transitions to monitor: - UNINITIALIZED → RESETTING (Initial pose acquisition) -
+ * RESETTING → QUEST_PRIMARY (Successful initialization) - QUEST_PRIMARY → TAG_BACKUP (Quest
+ * failure or validation error) - TAG_BACKUP → QUEST_PRIMARY (Quest recovery) - Any → EMERGENCY
+ * (System failure)
+ *
+ * @param from The state transitioning from
+ * @param to The state transitioning to
+ */
+ void logTransition(LocalizationState.State from, LocalizationState.State to);
+}
diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java
new file mode 100644
index 00000000..5b0fb5cf
--- /dev/null
+++ b/src/main/java/frc/alotobots/library/subsystems/vision/localizationfusion/commands/RequestPositionResetViaTags.java
@@ -0,0 +1,46 @@
+/*
+* ALOTOBOTS - FRC Team 5152
+ https://github.com/5152Alotobots
+* Copyright (C) 2024 ALOTOBOTS
+*
+* This program is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Source code must be publicly available on GitHub or an alternative web accessible site
+*/
+package frc.alotobots.library.subsystems.vision.localizationfusion.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusion;
+
+public class RequestPositionResetViaTags extends Command {
+ boolean reset = false;
+ LocalizationFusion localizationFusion;
+
+ public RequestPositionResetViaTags(LocalizationFusion localizationFusion) {
+ this.localizationFusion = localizationFusion;
+ addRequirements(localizationFusion);
+ }
+
+ @Override
+ public void initialize() {
+ reset = false;
+ }
+
+ @Override
+ public void execute() {
+ reset = localizationFusion.requestResetOculusPoseViaAprilTags();
+ }
+
+ @Override
+ public boolean isFinished() {
+ return reset;
+ }
+
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+}
diff --git a/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java
new file mode 100644
index 00000000..f4602b55
--- /dev/null
+++ b/src/main/java/frc/alotobots/library/subsystems/vision/oculus/OculusSubsystem.java
@@ -0,0 +1,418 @@
+/*
+* ALOTOBOTS - FRC Team 5152
+ https://github.com/5152Alotobots
+* Copyright (C) 2024 ALOTOBOTS
+*
+* This program is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* Source code must be publicly available on GitHub or an alternative web accessible site
+*/
+package frc.alotobots.library.subsystems.vision.oculus;
+
+import static frc.alotobots.library.subsystems.vision.oculus.constants.OculusConstants.*;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.alotobots.library.subsystems.vision.localizationfusion.PoseSource;
+import frc.alotobots.library.subsystems.vision.oculus.io.OculusIO;
+import frc.alotobots.library.subsystems.vision.oculus.io.OculusIOInputsAutoLogged;
+import lombok.Getter;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Manages communication and pose estimation with a Meta Quest VR headset.
+ *
+ * This subsystem leverages the Quest's inside-out SLAM tracking system to provide high-frequency
+ * (120Hz) robot pose estimation. Key features:
+ *
+ * - Global SLAM-based localization - Field mapping and persistence - Sub-millimeter tracking
+ * precision - High update rate (120Hz) - Drift-free position tracking - Fast relocalization
+ *
+ * The system operates in phases: 1. Pre-match mapping to capture field features 2. Initial pose
+ * acquisition and alignment 3. Continuous pose updates during match 4. Recovery handling if
+ * tracking is lost
+ */
+public class OculusSubsystem extends SubsystemBase implements PoseSource {
+ /** Status indicating system is ready for commands */
+ private static final int STATUS_READY = 0;
+
+ /** Status indicating heading reset completion */
+ private static final int STATUS_HEADING_RESET_COMPLETE = 99;
+
+ /** Status indicating pose reset completion */
+ private static final int STATUS_POSE_RESET_COMPLETE = 98;
+
+ /** Status indicating ping response receipt */
+ private static final int STATUS_PING_RESPONSE = 97;
+
+ /** Connection timeout threshold (seconds) */
+ private static final double CONNECTION_TIMEOUT = 1;
+
+ /** Hardware communication interface */
+ private final OculusIO io;
+
+ /** Logged inputs from Quest hardware */
+ private final OculusIOInputsAutoLogged inputs = new OculusIOInputsAutoLogged();
+
+ /** Flag indicating active pose reset */
+ @Getter private boolean poseResetInProgress = false;
+
+ /** Flag indicating active heading reset */
+ @Getter private boolean headingResetInProgress = false;
+
+ /** Flag indicating active ping operation */
+ @Getter private boolean pingInProgress = false;
+
+ /** Previous connection state */
+ @Getter private boolean wasConnected = false;
+
+ /** Reset operation start timestamp */
+ private double resetStartTime = 0;
+
+ /** Current reset attempt counter */
+ private int currentResetAttempt = 0;
+
+ /** Previous Quest timestamp */
+ private double lastTimestamp = 0.0;
+
+ /** Current Quest timestamp */
+ private double currentTimestamp = 0.0;
+
+ /** Target pose for reset operation */
+ private Pose2d pendingResetPose = null;
+
+ /** Current robot pose estimate */
+ private Pose2d currentPose = null;
+
+ private double lastQuestUpdateTime = Timer.getTimestamp();
+
+ /**
+ * Creates a new OculusSubsystem.
+ *
+ * Initializes communication with Quest hardware and prepares logging systems. The subsystem
+ * starts in an uninitialized state requiring pose calibration.
+ *
+ * @param io Interface for Quest hardware communication
+ */
+ public OculusSubsystem(OculusIO io) {
+ this.io = io;
+ Logger.recordOutput("Oculus/status", "Initialized");
+ }
+
+ /**
+ * Updates subsystem state and processes Quest data.
+ *
+ * Called periodically by the command scheduler. This method: - Updates hardware inputs -
+ * Processes new pose data - Handles state transitions - Manages reset operations - Updates
+ * logging
+ */
+ @Override
+ public void periodic() {
+ io.updateInputs(inputs);
+ Logger.processInputs("Oculus", inputs);
+
+ lastTimestamp = currentTimestamp;
+ currentTimestamp = inputs.timestamp;
+
+ // Update current pose
+ var oculusPose = getOculusPose();
+ currentPose = oculusPose.transformBy(ROBOT_TO_OCULUS.inverse());
+ Logger.recordOutput("Oculus/status/poses/headsetPose", oculusPose);
+ Logger.recordOutput("Oculus/status/poses/robotPose", currentPose);
+
+ handleResetTimeout();
+ handleResetCompletion();
+ handlePingResponse();
+ }
+
+ /**
+ * Manages timeouts for reset operations.
+ *
+ * Reset operations that exceed the timeout threshold are either: - Retried up to the maximum
+ * attempt limit - Abandoned with appropriate status logging
+ */
+ private void handleResetTimeout() {
+ double currentTime = Timer.getTimestamp();
+ if (currentTime - resetStartTime > RESET_TIMEOUT_SECONDS) {
+ if (headingResetInProgress) handleReset(true);
+ if (poseResetInProgress) handleReset(false);
+ }
+ }
+
+ /**
+ * Processes reset operation outcomes.
+ *
+ * @param isHeadingReset true for heading reset, false for pose reset
+ */
+ private void handleReset(boolean isHeadingReset) {
+ if (currentResetAttempt < MAX_RESET_ATTEMPTS) {
+ String resetType = isHeadingReset ? "Heading" : "Pose";
+ Logger.recordOutput(
+ "Oculus/status",
+ resetType + " Reset attempt " + (currentResetAttempt + 1) + " timed out, retrying...");
+ currentResetAttempt++;
+ resetStartTime = Timer.getTimestamp();
+
+ io.setMosi(0); // Clear command
+
+ if (isHeadingReset) {
+ io.setMosi(1); // Request heading reset
+ } else {
+ io.setResetPose(
+ pendingResetPose.getX(),
+ pendingResetPose.getY(),
+ pendingResetPose.getRotation().getDegrees());
+ io.setMosi(2); // Request pose reset
+ }
+ } else {
+ Logger.recordOutput(
+ "Oculus/status",
+ (isHeadingReset ? "Heading" : "Pose")
+ + " Reset failed after "
+ + MAX_RESET_ATTEMPTS
+ + " attempts");
+ if (isHeadingReset) {
+ clearHeadingResetState();
+ } else {
+ clearPoseResetState();
+ }
+ }
+ }
+
+ /**
+ * Handles completion of reset operations.
+ *
+ * Processes successful reset confirmations from Quest and updates system state.
+ */
+ private void handleResetCompletion() {
+ if (headingResetInProgress && inputs.misoValue == STATUS_HEADING_RESET_COMPLETE) {
+ Logger.recordOutput(
+ "Oculus/status",
+ "Heading Reset completed successfully on attempt " + (currentResetAttempt + 1));
+ clearHeadingResetState();
+ }
+ if (poseResetInProgress && inputs.misoValue == STATUS_POSE_RESET_COMPLETE) {
+ Logger.recordOutput(
+ "Oculus/status",
+ "Pose Reset completed successfully on attempt " + (currentResetAttempt + 1));
+ clearPoseResetState();
+ }
+ }
+
+ /**
+ * Handles ping response from Quest.
+ *
+ * Processes communication test responses and updates status.
+ */
+ private void handlePingResponse() {
+ if (inputs.misoValue == STATUS_PING_RESPONSE) {
+ Logger.recordOutput("Oculus/status", "Ping response received");
+ io.setMosi(0); // Clear command
+ pingInProgress = false;
+ }
+ }
+
+ /**
+ * Clears pose reset operation state.
+ *
+ * Resets all state variables related to pose reset operations.
+ */
+ private void clearPoseResetState() {
+ Logger.recordOutput("Oculus/status", "Clearing pose reset state");
+ poseResetInProgress = false;
+ pendingResetPose = null;
+ currentResetAttempt = 0;
+ io.setMosi(0); // Clear command
+ }
+
+ /**
+ * Clears heading reset operation state.
+ *
+ * Resets all state variables related to heading reset operations.
+ */
+ private void clearHeadingResetState() {
+ Logger.recordOutput("Oculus/status", "Clearing heading reset state");
+ headingResetInProgress = false;
+ currentResetAttempt = 0;
+ io.setMosi(0); // Clear command
+ }
+
+ /**
+ * Gets current Quest rotation in field frame.
+ *
+ * Converts Quest IMU data to field-relative rotation.
+ *
+ * @return Current headset rotation
+ */
+ private Rotation2d getOculusYaw() {
+ return Rotation2d.fromDegrees(-inputs.eulerAngles[1]);
+ }
+
+ /**
+ * Gets current Quest position in field frame.
+ *
+ * Converts Quest SLAM position to field coordinates.
+ *
+ * @return Current headset position
+ */
+ private Translation2d getOculusPosition() {
+ return new Translation2d(inputs.position[2], -inputs.position[0]);
+ }
+
+ /**
+ * Gets complete Quest pose in field frame.
+ *
+ * Combines position and rotation data into a field-relative pose.
+ *
+ * @return Current headset pose
+ */
+ private Pose2d getOculusPose() {
+ return new Pose2d(getOculusPosition(), getOculusYaw());
+ }
+
+ /**
+ * Initiates pose reset operation.
+ *
+ * Attempts to reset Quest SLAM origin to align with target pose.
+ *
+ * @param targetPose Desired robot pose after reset
+ * @return true if reset initiated successfully
+ */
+ public boolean resetToPose(Pose2d targetPose) {
+ if (poseResetInProgress) {
+ Logger.recordOutput("Oculus/status", "Cannot reset pose - reset already in progress");
+ return false;
+ }
+
+ if (inputs.misoValue != STATUS_READY) {
+ Logger.recordOutput(
+ "Oculus/status", "Cannot reset pose - Quest busy (MISO=" + inputs.misoValue + ")");
+ return false;
+ }
+
+ targetPose = targetPose.plus(ROBOT_TO_OCULUS);
+ pendingResetPose = targetPose;
+ Logger.recordOutput(
+ "Oculus/status",
+ String.format(
+ "Initiating pose reset to X:%.2f Y:%.2f Rot:%.2f°",
+ targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()));
+
+ io.setResetPose(targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees());
+ poseResetInProgress = true;
+ resetStartTime = Timer.getTimestamp();
+ currentResetAttempt = 0;
+ io.setMosi(2); // Request pose reset
+
+ return true;
+ }
+
+ /**
+ * Initiates heading reset operation.
+ *
+ * Attempts to zero current Quest heading measurement.
+ *
+ * @return true if reset initiated successfully
+ */
+ public boolean zeroHeading() {
+ if (headingResetInProgress) {
+ Logger.recordOutput("Oculus/status", "Cannot zero heading - reset already in progress");
+ return false;
+ }
+
+ if (inputs.misoValue != STATUS_READY) {
+ Logger.recordOutput(
+ "Oculus/status", "Cannot zero heading - Quest busy (MISO=" + inputs.misoValue + ")");
+ return false;
+ }
+
+ Logger.recordOutput("Oculus/status", "Zeroing heading");
+ headingResetInProgress = true;
+ resetStartTime = Timer.getTimestamp();
+ currentResetAttempt = 0;
+ io.setMosi(1); // Request heading reset
+ return true;
+ }
+
+ /**
+ * Tests Quest communication.
+ *
+ * Sends ping command to verify hardware connectivity.
+ *
+ * @return true if ping initiated successfully
+ */
+ public boolean ping() {
+ if (pingInProgress) {
+ Logger.recordOutput("Oculus/status", "Cannot ping - ping already in progress");
+ return false;
+ }
+
+ if (inputs.misoValue != STATUS_READY) {
+ Logger.recordOutput(
+ "Oculus/status", "Cannot ping - system busy (MISO=" + inputs.misoValue + ")");
+ return false;
+ }
+
+ Logger.recordOutput("Oculus/status", "Sending ping...");
+ Logger.recordOutput("Oculus/ping/sendTime", Timer.getTimestamp());
+ pingInProgress = true;
+ io.setMosi(3); // Send ping
+ return true;
+ }
+
+ // PoseSource interface implementation
+
+ /**
+ * {@inheritDoc}
+ *
+ * Quest connection is determined by checking if we've received any new Quest timestamp updates
+ * within our timeout window. Small variations in update timing are allowed, only triggering
+ * disconnect on significant delays.
+ */
+ @Override
+ public boolean isConnected() {
+ // If timestamp has changed since last check, update our last update time
+ if (inputs.timestamp != lastTimestamp) {
+ lastQuestUpdateTime = Timer.getTimestamp();
+ }
+
+ // Only consider disconnected if we haven't seen ANY new timestamps
+ // for longer than our timeout period
+ return (Timer.getTimestamp() - lastQuestUpdateTime) < CONNECTION_TIMEOUT;
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Returns field-relative robot pose from Quest SLAM.
+ */
+ @Override
+ public Pose2d getCurrentPose() {
+ return currentPose;
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Returns constant measurement uncertainty for Quest tracking.
+ */
+ @Override
+ public Matrix This class wraps the OculusSubsystem to provide high-frequency pose estimation through the
+ * standardized PoseSource interface. The Quest serves as: - Primary pose estimation source -
+ * High-frequency (120Hz) updates - Global SLAM-based localization - Drift-free position tracking
+ *
+ * Features: - Sub-millimeter tracking precision - Building-scale SLAM mapping - Robust to
+ * reflective surfaces - Fast relocalization from saved maps - High-speed movement handling
+ */
+public class OculusPoseSource implements PoseSource {
+ /** The underlying Oculus subsystem */
+ public final OculusSubsystem subsystem;
+
+ /**
+ * Creates a new OculusPoseSource.
+ *
+ * @param subsystem The Oculus subsystem to wrap
+ */
+ public OculusPoseSource(OculusSubsystem subsystem) {
+ this.subsystem = subsystem;
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * For Quest SLAM, connection status depends on: - Quest hardware connection - SLAM tracking
+ * quality - Update frequency - Environment mapping status
+ */
+ @Override
+ public boolean isConnected() {
+ return subsystem.isConnected();
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Quest poses are: - Globally consistent via SLAM - Updated at 120Hz - Drift-compensated -
+ * Environment-referenced - Transform-corrected to field frame
+ */
+ @Override
+ public Pose2d getCurrentPose() {
+ return subsystem.getCurrentPose();
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Quest measurement uncertainty: - Sub-millimeter position accuracy - Factory-calibrated IMU -
+ * Environment-dependent SLAM quality - Constant across operating range
+ */
+ @Override
+ public Matrix This class wraps the AprilTagSubsystem to provide pose estimation through the standardized
+ * PoseSource interface. AprilTags serve as: - Secondary pose validation source - Initial pose
+ * calibration reference - Backup pose estimation when Quest is unavailable
+ *
+ * Pose accuracy depends on: - Number of visible AprilTags - Distance to detected tags - Camera
+ * calibration quality - Tag pose ambiguity factors
+ */
+public class AprilTagPoseSource implements PoseSource {
+ /** The underlying AprilTag subsystem */
+ private final AprilTagSubsystem subsystem;
+
+ /**
+ * Creates a new AprilTagPoseSource.
+ *
+ * @param subsystem The AprilTag subsystem to wrap
+ */
+ public AprilTagPoseSource(AprilTagSubsystem subsystem) {
+ this.subsystem = subsystem;
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * For AprilTags, connection status depends on: - Camera connection status - Valid pose
+ * availability - Recent detection history
+ */
+ @Override
+ public boolean isConnected() {
+ return subsystem.isConnected();
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * AprilTag poses are: - Field-relative - Validated for ambiguity - Filtered for physical
+ * feasibility - Updated at camera frame rate
+ */
+ @Override
+ public Pose2d getCurrentPose() {
+ return subsystem.getCurrentPose();
+ }
+
+ /**
+ * {@inheritDoc}
+ *
+ * Measurement uncertainty scales with: - Tag distance - Number of visible tags - Viewing angle
+ * - Tag pose ambiguity
+ */
+ @Override
+ public Matrix