Package frc.robot.subsystems.robotpose

Class RobotPoseSubsystem

java.lang.Object
edu.wpi.first.wpilibj2.command.SubsystemBase
frc.robot.shared.subsystems.AbstractSubsystem<RobotPoseSubsystemConfig>
frc.robot.subsystems.robotpose.RobotPoseSubsystem
All Implemented Interfaces:
edu.wpi.first.util.sendable.Sendable, edu.wpi.first.wpilibj2.command.Subsystem
public class RobotPoseSubsystem extends AbstractSubsystem<RobotPoseSubsystemConfig>
Centralized pose tracking subsystem that serves as the single authority for robot field pose.

All pose sources (cameras, LIDAR, future sensors) funnel their measurements through this subsystem via addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d, double, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3, edu.wpi.first.math.numbers.N1>). Fusion math is delegated to the drivebase's internal YAGSL SwerveDrivePoseEstimator, which provides Kalman-filter-based fusion with latency compensation and uncertainty weighting. This subsystem reads the fused result each cycle and exposes it as the authoritative pose for commands and other subsystems.

  • Field Summary

    Fields inherited from class frc.robot.shared.subsystems.AbstractSubsystem

    className, config, enabled, kDt, log, verbose

  • Constructor Summary

    Constructors
    Constructor
    Description
    RobotPoseSubsystem(RobotPoseSubsystemConfig config, Supplier<edu.wpi.first.math.geometry.Pose2d> fusedPoseSupplier, Supplier<edu.wpi.first.math.geometry.Pose2d> odometryOnlyPoseSupplier, VisionMeasurementConsumer visionForwarder, Consumer<edu.wpi.first.math.geometry.Pose2d> odometryResetConsumer)
    Creates the Robot Pose subsystem with all cross-subsystem dependencies.

  • Method Summary

    Modifier and Type
    Method
    Description
    void
    addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestampSeconds, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> standardDeviations)
    Accepts a vision-based robot pose measurement and forwards it to the drivebase's pose estimator for fusion.
    double
    getDistanceToPointMeters(edu.wpi.first.math.geometry.Translation2d targetFieldPositionMeters)
    Computes the straight-line distance from the robot's current estimated position to a field-relative target point.
    edu.wpi.first.math.geometry.Pose2d
    getEstimatedPose()
    Returns the current fused pose estimate.
    void
    periodic()
    Reads the latest fused pose from the drivebase estimator and logs telemetry each robot loop.
    void
    resetPose(edu.wpi.first.math.geometry.Pose2d pose)
    Resets all pose tracking to the provided pose.
    void
    resetPoseFromVision()
    Resets the robot pose to the most recent vision measurement.

    Methods inherited from class frc.robot.shared.subsystems.AbstractSubsystem

    getConfig, isFMSAttached, isSimulation, isSubsystemDisabled, isVerbose, isVerboseLoggingEnabled, logDisabled, reportWarning

    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, idle, register, removeDefaultCommand, run, runEnd, runOnce, setDefaultCommand, simulationPeriodic, startEnd, startRun

  • Constructor Details

    • RobotPoseSubsystem

      public RobotPoseSubsystem(RobotPoseSubsystemConfig config, Supplier<edu.wpi.first.math.geometry.Pose2d> fusedPoseSupplier, Supplier<edu.wpi.first.math.geometry.Pose2d> odometryOnlyPoseSupplier, VisionMeasurementConsumer visionForwarder, Consumer<edu.wpi.first.math.geometry.Pose2d> odometryResetConsumer)
      Creates the Robot Pose subsystem with all cross-subsystem dependencies.

      This subsystem does not own hardware, but it still follows the command-based lifecycle for consistent updates and logging. Dependencies are injected here so every required wiring is enforced at compile time.

      Parameters:
      config - configuration values for vision fusion gating and logging
      fusedPoseSupplier - supplier returning the Kalman-filtered pose from the drivebase in meters and radians
      odometryOnlyPoseSupplier - supplier returning the raw wheel+gyro pose without vision corrections in meters and radians
      visionForwarder - consumer that forwards vision measurements to the drivebase's pose estimator
      odometryResetConsumer - consumer that resets the drivebase odometry to a given pose in meters and radians

  • Method Details

    • periodic

      public void periodic()
      Reads the latest fused pose from the drivebase estimator and logs telemetry each robot loop.

    • addVisionMeasurement

      public void addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d robotPose, double timestampSeconds, edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> standardDeviations)
      Accepts a vision-based robot pose measurement and forwards it to the drivebase's pose estimator for fusion.

      All pose sources (cameras, LIDAR, etc.) should call this single method. The measurement is forwarded to the drivebase's YAGSL SwerveDrivePoseEstimator which handles Kalman-filter-based fusion, latency compensation, and uncertainty weighting. When vision fusion is disabled in config, measurements are recorded for logging but not forwarded.

      Parameters:
      robotPose - pose measurement in meters and radians
      timestampSeconds - FPGA timestamp of the measurement in seconds
      standardDeviations - uncertainty in x (meters), y (meters), and rotation (radians)

    • resetPose

      public void resetPose(edu.wpi.first.math.geometry.Pose2d pose)
      Resets all pose tracking to the provided pose.

      Use this at the start of autonomous or after localization resets so odometry and vision agree on the robot's position. The reset is forwarded to the drivebase so the internal YAGSL estimator is also reset.

      Parameters:
      pose - pose to apply to odometry and the fused estimate in meters and radians

    • resetPoseFromVision

      public void resetPoseFromVision()
      Resets the robot pose to the most recent vision measurement.

      Call this at the start of a match to seed the pose estimator with the camera-derived position before switching to fused odometry-plus-vision tracking. If no vision measurement has been received yet, the reset is skipped and a warning is logged so operators can diagnose camera issues.

    • getEstimatedPose

      public edu.wpi.first.math.geometry.Pose2d getEstimatedPose()
      Returns the current fused pose estimate.

      Commands and subsystems should use this pose for field-relative decisions. This is the single authoritative source of robot pose for the entire codebase.

      Returns:
      latest fused pose in meters and radians

    • getDistanceToPointMeters

      public double getDistanceToPointMeters(edu.wpi.first.math.geometry.Translation2d targetFieldPositionMeters)
      Computes the straight-line distance from the robot's current estimated position to a field-relative target point.

      Use this for distance-based calculations such as shooter RPM scaling or approach detection.

      Parameters:
      targetFieldPositionMeters - target position on the field in meters
      Returns:
      distance in meters from the robot to the target point