Package frc.alotobots.library.subsystems.vision.localizationfusion

Class LocalizationFusion

java.lang.Object

edu.wpi.first.wpilibj2.command.SubsystemBase

frc.alotobots.library.subsystems.vision.localizationfusion.LocalizationFusion

All Implemented Interfaces:

Sendable, Subsystem, StateTransitionLogger

public class LocalizationFusion
extends SubsystemBase
implements StateTransitionLogger

The LocalizationFusion subsystem manages robot pose estimation by fusing data from multiple sources. It primarily uses Oculus Quest SLAM for continuous tracking, with AprilTag detection as a backup and validation source. The system handles initialization, validation, and switching between sources based on their availability and reliability.

Constructor Summary

Constructors

Constructor	Description
LocalizationFusion(PoseVisionConsumer poseConsumer, OculusPoseSource oculusSource, AprilTagPoseSource aprilTagSource, org.littletonrobotics.junction.networktables.LoggedDashboardChooser<Command> autoChooser)	Creates a new LocalizationFusion subsystem.

Method Summary

Modifier and Type	Method	Description
void	logTransition(LocalizationState.State from, LocalizationState.State to)	Logs state transitions with descriptive messages.
void	periodic()	Periodic update method called by the command scheduler.
boolean	requestResetOculusPose(Pose2d targetPose)	Manually triggers a pose reset to a specific pose.
boolean	requestResetOculusPoseViaAprilTags()	Manually triggers a pose reset using the current AprilTag pose.

Methods inherited from class edu.wpi.first.wpilibj2.command.SubsystemBase

addChild, getName, getSubsystem, initSendable, setName, setSubsystem

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Methods inherited from interface edu.wpi.first.wpilibj2.command.Subsystem

defer, getCurrentCommand, getDefaultCommand, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun

Constructor Details

LocalizationFusion

public LocalizationFusion(PoseVisionConsumer poseConsumer,
 OculusPoseSource oculusSource,
 AprilTagPoseSource aprilTagSource,
 org.littletonrobotics.junction.networktables.LoggedDashboardChooser<Command> autoChooser)

Creates a new LocalizationFusion subsystem.

Parameters:

poseConsumer - The consumer interface that will receive pose updates

oculusSource - The primary pose source using Quest SLAM

aprilTagSource - The secondary pose source using AprilTags

Method Details

periodic

public void periodic()

Periodic update method called by the command scheduler. Handles system state updates, source initialization, and pose processing.

Specified by:

periodic in interface Subsystem

requestResetOculusPoseViaAprilTags

public boolean requestResetOculusPoseViaAprilTags()

Manually triggers a pose reset using the current AprilTag pose.

Returns:

true if reset was initiated successfully

requestResetOculusPose

public boolean requestResetOculusPose(Pose2d targetPose)

Manually triggers a pose reset to a specific pose.

Parameters:

targetPose - The pose to reset to

Returns:

true if reset was initiated successfully

logTransition

public void logTransition(LocalizationState.State from,
 LocalizationState.State to)

Logs state transitions with descriptive messages.

Specified by:

logTransition in interface StateTransitionLogger

Parameters:

from - The previous state

to - The new state