Package gg.questnav.questnav

Class QuestNav

java.lang.Object
gg.questnav.questnav.QuestNav
public class QuestNav extends Object
The QuestNav class provides a comprehensive interface for communicating with an Oculus/Meta Quest VR headset for robot localization and tracking in FRC robotics applications.

This class handles all aspects of Quest-robot communication including:

  • Real-time pose tracking and localization data
  • Command sending and response handling
  • Device status monitoring (battery, tracking state, connectivity)
  • NetworkTables-based communication protocol

Basic Usage


 // Create QuestNav instance
 QuestNav questNav = new QuestNav();

 // Set initial robot pose (required for field-relative tracking)
 Pose2d initialPose = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(90));
 questNav.setPose(initialPose);

 // In robot periodic methods
 public void robotPeriodic() {
   questNav.commandPeriodic(); // Process command responses

   // Get latest pose data
   PoseFrame[] newFrames = questNav.getAllUnreadPoseFrames();
   for (PoseFrame frame : newFrames) {
     // Use frame.questPose() and frame.dataTimestamp() with pose estimator
   }

   // Monitor connection and device status
   if (questNav.isConnected() && questNav.isTracking()) {
     // Quest is connected and tracking - safe to use pose data
   }
 }

Coordinate Systems

QuestNav uses the WPILib field coordinate system:

  • X-axis: Forward direction (towards opposing alliance)
  • Y-axis: Left direction (when facing forward)
  • Rotation: Counter-clockwise positive (standard mathematical convention)
  • Units: Meters for translation, radians for rotation

Threading and Performance

This class is designed for use in FRC robot code and follows WPILib threading conventions:

  • All methods are thread-safe for typical FRC usage patterns
  • Uses cached objects to minimize garbage collection pressure
  • NetworkTables handles the underlying communication asynchronously
  • Call commandPeriodic() regularly to process command responses

Error Handling

The class provides robust error handling:

  • Methods return Optional types when data might not be available
  • Connection status can be checked with isConnected()
  • Tracking status can be monitored with isTracking()
  • Command failures are reported through DriverStation error logging
Since:
2025.1.0
See Also:

  • Constructor Details

    • QuestNav

      public QuestNav()
      Creates a new QuestNav instance for communicating with a Quest headset.

      This constructor initializes all necessary NetworkTables subscribers and publishers for communication with the Quest device. The instance is ready to use immediately, but you should call setPose(Pose3d) to establish field-relative tracking before relying on pose data.

      The constructor sets up:

      • NetworkTables communication on the "QuestNav" table
      • Protobuf serialization for efficient data transfer
      • Cached objects to minimize garbage collection
      • Subscribers for frame data, device data, and command responses
      • Publisher for sending commands to the Quest

  • Method Details

    • setPose

      public void setPose(edu.wpi.first.math.geometry.Pose3d pose)
      Sets the field-relative pose of the Quest headset by commanding it to reset its tracking.

      This method sends a pose reset command to the Quest headset, telling it where the Quest is currently located on the field. This is essential for establishing field-relative tracking and should be called:

      • At the start of autonomous or teleop when the Quest position is known
      • When the robot (and Quest) is placed at a known location (e.g., against field walls)
      • After significant tracking drift is detected
      • When integrating with other localization systems (vision, odometry)

      Important: This should be the Quest's pose, not the robot's pose. If you know the robot's pose, you need to apply the mounting offset to get the Quest's pose before calling this method.

      The command is sent asynchronously. Monitor command success/failure by calling commandPeriodic() regularly, which will log any errors to the DriverStation.

      Usage Example:

      
 // Set Quest pose at autonomous start (if you know the Quest's position directly)
 Pose3d questPose = new Pose3d(1.5, 5.5, new Rotation3d(0.0, 0.0, 0.0)));
 questNav.setPose(questPose);

 // If you know the robot pose, apply mounting offset to get Quest pose
 Pose2d robotPose = poseEstimator.getEstimatedPosition();
 Pose3d questPose = new Pose3d(robotPose).transformBy(mountingOffset); // Apply your mounting offset
 questNav.setPose(questPose);
      Parameters:
      pose - The Quest's current field-relative pose in WPILib coordinates (meters for translation, radians for rotation)
      Throws:
      IllegalArgumentException - if pose contains NaN or infinite values
      See Also:

    • getBatteryPercent

      public OptionalInt getBatteryPercent()
      Returns the Quest headset's current battery level as a percentage.

      This method provides real-time battery status information from the Quest device, which is useful for:

      • Monitoring device health during matches
      • Implementing low-battery warnings or behaviors
      • Planning charging schedules between matches
      • Triggering graceful shutdown procedures when battery is critical

      Battery level guidelines:

      • 80-100%: Excellent - full match capability
      • 50-80%: Good - normal operation expected
      • 20-50%: Fair - consider charging after match
      • 10-20%: Low - charge soon, monitor closely
      • 0-10%: Critical - immediate charging required
      Returns:
      An OptionalInt containing the battery percentage (0-100), or empty if no device data is available or Quest is disconnected
      See Also:

    • isTracking

      public boolean isTracking()
      Gets the current tracking state of the Quest headset.

      This method indicates whether the Quest's visual-inertial tracking system is currently functioning and providing reliable pose data. Tracking can be lost due to:

      • Poor lighting conditions (too dark or too bright)
      • Lack of visual features in the environment
      • Rapid motion or high acceleration
      • Camera occlusion or obstruction
      • Hardware issues or overheating

      Important: When tracking is lost, pose data becomes unreliable and should not be used for robot control. Implement fallback localization methods (wheel odometry, vision, etc.) for when Quest tracking is unavailable.

      To recover tracking:

      • Improve lighting conditions
      • Move to an area with more visual features
      • Reduce robot motion to allow re-initialization
      • Clear any obstructions from Quest cameras
      • Call setPose(Pose3d) once tracking recovers
      Returns:
      true if the Quest is actively tracking and pose data is reliable, false if tracking is lost or no device data is available
      See Also:

    • getFrameCount

      public OptionalInt getFrameCount()
      Gets the current frame count from the Quest headset.
      Returns:
      The frame count value

    • getTrackingLostCounter

      public OptionalInt getTrackingLostCounter()
      Gets the number of tracking lost events since the Quest connected to the robot.
      Returns:
      The tracking lost counter value

    • isConnected

      public boolean isConnected()
      Determines if the Quest headset is currently connected to the robot. Connection is determined by how stale the last received frame from the Quest is.
      Returns:
      Boolean indicating if the Quest is connected (true) or not (false)

    • getLatency

      public double getLatency()
      Gets the latency of the Quest > Robot Connection. Returns the latency between the current time and the last frame data update.
      Returns:
      The latency in milliseconds

    • getAppTimestamp

      public OptionalDouble getAppTimestamp()
      Returns the Quest app's uptime timestamp for debugging and diagnostics.

      Important: For integration with a pose estimator, use the timestamp from PoseFrame.dataTimestamp() instead! This method provides the Quest's internal application timestamp, which is useful for:

      • Debugging timing issues between Quest and robot
      • Calculating Quest-side processing latency
      • Monitoring Quest application uptime
      • Correlating with Quest-side logs

      The timestamp represents seconds since the Quest application started and is independent of the robot's clock. For pose estimation, always use the NetworkTables timestamp from PoseFrame.dataTimestamp() which is synchronized with robot time.

      Returns:
      An OptionalDouble containing the Quest app uptime in seconds, or empty if no frame data is available
      See Also:

    • getAllUnreadPoseFrames

      public PoseFrame[] getAllUnreadPoseFrames()
      Retrieves all new pose frames received from the Quest since the last call to this method.

      This is the primary method for integrating QuestNav with FRC pose estimation systems. It returns an array of PoseFrame objects containing pose data and timestamps that can be fed directly into a PoseEstimator.

      Each frame contains:

      • Pose data: Robot position and orientation in field coordinates
      • NetworkTables timestamp: When the data was received (use this for pose estimation)
      • App timestamp: Quest internal timestamp (for debugging only)
      • Frame count: Sequential frame number for detecting drops

      Important: This method consumes the frame queue, so each frame is only returned once. Call this method regularly (every robot loop) to avoid missing frames.

      Integration with Pose Estimator:

      
 // In robotPeriodic() or subsystem periodic()
 PoseFrame[] newFrames = questNav.getAllUnreadPoseFrames();
 for (PoseFrame frame : newFrames) {
   if (questNav.isTracking() && questNav.isConnected()) {
     // Add vision measurement to pose estimator
     poseEstimator.addVisionMeasurement(
       frame.questPose(),           // Measured pose
       frame.dataTimestamp(),       // When measurement was taken
       VecBuilder.fill(0.1, 0.1, 0.05)  // Standard deviations (tune these)
     );
   }
 }

      Performance notes:

      • Returns a new array each call - consider caching if called multiple times per loop
      • Frame rate is exactly 100 Hz (every 10 milliseconds)
      • Empty array returned when no new frames are available
      Returns:
      Array of new PoseFrame objects received since the last call. Empty array if no new frames are available or Quest is disconnected.
      See Also:

    • commandPeriodic

      public void commandPeriodic()
      Processes command responses from the Quest headset and handles any errors.

      This method must be called regularly (typically in robotPeriodic()) to:

      • Process responses to commands sent via setPose(Pose3d)
      • Log command failures to the DriverStation for debugging
      • Maintain proper command/response synchronization
      • Prevent command response queue overflow

      The method automatically handles:

      • Success responses: Silently acknowledged
      • Error responses: Logged to DriverStation with error details
      • Duplicate responses: Ignored to prevent spam
      • Out-of-order responses: Handled gracefully

      Usage Pattern:

      
 public class Robot extends TimedRobot {
   private QuestNav questNav = new QuestNav();

   @Override
   public void robotPeriodic() {
     questNav.commandPeriodic(); // Call every loop

     // Other periodic tasks...
   }
 }

      Performance: This method is lightweight and safe to call every robot loop (20ms). It only processes new responses and exits quickly when none are available.

      See Also: