Package com.titaniumtigers4829
Class TigerHelpers
java.lang.Object
com.titaniumtigers4829.TigerHelpers
TigerHelpers is built on top of LimelightHelpers, providing a set of static methods and classes
for interfacing with Limelights, with a dedicated focus on AprilTag pose estimation via
Networktables.
It includes additional helper methods to simplify common tasks, improve integration with unit testing, and enhance usability while staying up to date with new Limelight features.
-
Constructor Summary
Constructors -
Method Summary
Modifier and TypeMethodDescriptionstatic double[]
getBotPose
(String limelightName) static edu.wpi.first.math.geometry.Pose2d
getBotPose2d
(String limelightName) Gets the Pose2d with the blue-side origin for use with Odometry vision pose estimator (addVisionMeasurement).static edu.wpi.first.math.geometry.Pose2d
getBotPose2d
(String limelightName, Botpose botpose) Gets the specified Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement).static edu.wpi.first.math.geometry.Pose3d
getBotPose3d
(String limelightName) static edu.wpi.first.math.geometry.Pose3d
getBotPose3d_TargetSpace
(String limelightName) Deprecated.static edu.wpi.first.math.geometry.Pose3d
getBotPose3d_wpiBlue
(String limelightName) Deprecated.static edu.wpi.first.math.geometry.Pose3d
getBotPose3d_wpiRed
(String limelightName) Deprecated.static PoseEstimate
getBotPoseEstimate
(String limelightName) Gets the PoseEstimate with the blue-side origin using MegaTag1.static PoseEstimate
getBotPoseEstimate
(String limelightName, Botpose botpose) Gets the PoseEstimate for the specifiedBotpose
type.static PoseEstimate
getBotPoseEstimate_wpiBlue
(String limelightName) Deprecated.static PoseEstimate
getBotPoseEstimate_wpiBlue_MegaTag2
(String limelightName) Deprecated.static PoseEstimate
getBotPoseEstimate_wpiRed
(String limelightName) Deprecated.static PoseEstimate
getBotPoseEstimate_wpiRed_MegaTag2
(String limelightName) Deprecated.static edu.wpi.first.math.geometry.Pose3d
getCameraPose3d_RobotSpace
(String limelightName) Deprecated.static edu.wpi.first.math.geometry.Pose3d
getCameraPose3d_TargetSpace
(String limelightName) Deprecated.static double
getFiducialID
(String limelightName) static IMUData
getIMUData
(String limelightName) Gets the current IMU data from NetworkTables.static double
getLatencyCapture
(String limelightName) Gets the capture latency.static double
getLatencyPipeline
(String limelightName) Gets the pipeline's processing latency contribution.static RawFiducial[]
getRawFiducials
(String limelightName) Gets the latest raw fiducial/AprilTag detection results from NetworkTables.static double
Gets the target area as a percentage of the image (0-100%).static edu.wpi.first.math.geometry.Pose3d
getTargetPose3d_CameraSpace
(String limelightName) Deprecated.static edu.wpi.first.math.geometry.Pose3d
getTargetPose3d_RobotSpace
(String limelightName) Deprecated.static boolean
Gets if the Limelight have a valid target?static double
Gets the horizontal offset from the crosshair to the target in degrees.static double
Gets the horizontal offset from the principal pixel/point to the target in degrees.static double
Gets the vertical offset from the crosshair to the target in degrees.static double
Gets the vertical offset from the principal pixel/point to the target in degrees.static void
setBotPoseEstimate
(PoseEstimate poseEstimate, String limelightName) Sets the network table entry for the blue-side, MegaTag1 botpose data.static void
setBotPoseEstimate
(PoseEstimate poseEstimate, String limelightName, Botpose botpose) Sets the network table entry for the botpose data.static void
setCameraPoseRobotSpace
(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) Sets the camera pose relative to the robot.static void
setCropWindow
(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) Sets the crop window for the camera.static void
setFidcuial3DOffset
(String limelightName, double x, double y, double z) Sets the 3D point-of-interest offset for the current fiducial pipeline.static void
setFiducial3DOffset
(String limelightName, double offsetX, double offsetY, double offsetZ) Sets the offset of the april tag tracking point,static void
setFiducialDownscalingOverride
(String limelightName, float downscale) Sets the downscaling factor for AprilTag detection.static void
setFiducialIDFiltersOverride
(String limelightName, int[] validIDs) Overrides the valid AprilTag IDs that will be used for localization.static void
setIMUAssistAlpha
(String limelightName, double alpha) Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4)static void
setIMUMode
(String limelightName, int imuMode) Deprecated.static void
setIMUMode
(String limelightName, IMUData.IMUMode imuMode) Configures the IMU mode for MegaTag2 Localizationstatic void
setLimelightThrottle
(String limelightName, int throttle) Sets the throttle for the Limelight camera.static void
setPriorityTagID
(String limelightName, int ID) Sets the priority tag ID for the Limelight camera.static void
setRawFiducials
(RawFiducial[] rawFiducials, String limelightName) Sets the network table entry for the raw fiducials.static void
setRobotOrientation
(String limelightName, double yaw) Sets robot yaw used by MegaTag2 localization algorithm.static void
setRobotOrientation
(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate) Sets robot orientation values used by MegaTag2 localization algorithm.
-
Constructor Details
-
TigerHelpers
public TigerHelpers()
-
-
Method Details
-
getTV
Gets if the Limelight have a valid target?- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- True if a valid target is present, false otherwise
-
getTX
Gets the horizontal offset from the crosshair to the target in degrees.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Horizontal offset angle in degrees
-
getTY
Gets the vertical offset from the crosshair to the target in degrees.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Vertical offset angle in degrees
-
getTXNC
Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Horizontal offset angle in degrees
-
getTYNC
Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Vertical offset angle in degrees
-
getTA
Gets the target area as a percentage of the image (0-100%).- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Target area percentage (0-100%)
-
getLatencyPipeline
Gets the pipeline's processing latency contribution.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pipeline latency in milliseconds
-
getLatencyCapture
Gets the capture latency.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Capture latency in milliseconds
-
getRawFiducials
Gets the latest raw fiducial/AprilTag detection results from NetworkTables.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Array of RawFiducial objects containing detection details
-
getBotPose
-
getFiducialID
-
getBotPose3d
-
getBotPose3d_wpiRed
@Deprecated public static edu.wpi.first.math.geometry.Pose3d getBotPose3d_wpiRed(String limelightName) Deprecated.(Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pose3d object representing the robot's position and orientation in Red Alliance field space
-
getBotPose3d_wpiBlue
@Deprecated public static edu.wpi.first.math.geometry.Pose3d getBotPose3d_wpiBlue(String limelightName) Deprecated.(Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pose3d object representing the robot's position and orientation in Blue Alliance field space
-
getBotPose3d_TargetSpace
@Deprecated public static edu.wpi.first.math.geometry.Pose3d getBotPose3d_TargetSpace(String limelightName) Deprecated.Gets the robot's 3D pose with respect to the currently tracked target's coordinate system.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pose3d object representing the robot's position and orientation relative to the target
-
getCameraPose3d_TargetSpace
@Deprecated public static edu.wpi.first.math.geometry.Pose3d getCameraPose3d_TargetSpace(String limelightName) Deprecated.Gets the camera's 3D pose with respect to the currently tracked target's coordinate system.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pose3d object representing the camera's position and orientation relative to the target
-
getTargetPose3d_CameraSpace
@Deprecated public static edu.wpi.first.math.geometry.Pose3d getTargetPose3d_CameraSpace(String limelightName) Deprecated.Gets the target's 3D pose with respect to the camera's coordinate system.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pose3d object representing the target's position and orientation relative to the camera
-
getTargetPose3d_RobotSpace
@Deprecated public static edu.wpi.first.math.geometry.Pose3d getTargetPose3d_RobotSpace(String limelightName) Deprecated.Gets the target's 3D pose with respect to the robot's coordinate system.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pose3d object representing the target's position and orientation relative to the robot
-
getCameraPose3d_RobotSpace
@Deprecated public static edu.wpi.first.math.geometry.Pose3d getCameraPose3d_RobotSpace(String limelightName) Deprecated.Gets the camera's 3D pose with respect to the robot's coordinate system.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- Pose3d object representing the camera's position and orientation relative to the robot
-
getBotPoseEstimate_wpiBlue
Deprecated.Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system.- Parameters:
limelightName
-- Returns:
-
getBotPoseEstimate_wpiBlue_MegaTag2
Deprecated.Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling {setRobotOrientation()} before calling this method.- Parameters:
limelightName
-- Returns:
-
getBotPoseEstimate_wpiRed
Deprecated.Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED alliance- Parameters:
limelightName
-- Returns:
-
getBotPoseEstimate_wpiRed_MegaTag2
Deprecated.Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED alliance- Parameters:
limelightName
-- Returns:
-
getBotPose2d
public static edu.wpi.first.math.geometry.Pose2d getBotPose2d(String limelightName, Botpose botpose) Gets the specified Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement).- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)botpose
- the type of botpose to get- Returns:
- the Pose2d of the robot relative to the origin specified by the botpose
-
getBotPose2d
Gets the Pose2d with the blue-side origin for use with Odometry vision pose estimator (addVisionMeasurement).- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- the Pose2d of the robot relative to the blue-side origin
-
getBotPoseEstimate
Gets the PoseEstimate for the specifiedBotpose
type.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)botpose
- the type of botpose to get- Returns:
- the PoseEstimate object
-
getBotPoseEstimate
Gets the PoseEstimate with the blue-side origin using MegaTag1.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- the PoseEstimate object
-
setBotPoseEstimate
public static void setBotPoseEstimate(PoseEstimate poseEstimate, String limelightName, Botpose botpose) Sets the network table entry for the botpose data. This is useful for setting values for unit testing. ThePoseEstimate
does not contain values for the z coordinate, roll, and pitch, so these will be set to 0.- Parameters:
poseEstimate
- the pose estimate to setlimelightName
- The name of the Limelight set in the UI ("" for default)botpose
- the type of botpose to set (e.g., BLUE_MEGATAG1)
-
setBotPoseEstimate
Sets the network table entry for the blue-side, MegaTag1 botpose data. This is useful for setting values for unit testing. ThePoseEstimate
does not contain values for the z coordinate, roll, and pitch, so these will be set to 0.- Parameters:
poseEstimate
- the pose estimate to setlimelightName
- The name of the Limelight set in the UI ("" for default)
-
setRawFiducials
Sets the network table entry for the raw fiducials. This is useful for setting values for unit testing.- Parameters:
rawFiducials
- the array of raw fiducials to setlimelightName
- The name of the Limelight set in the UI ("" for default)
-
getIMUData
Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, roll, pitch, yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or unavailable.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)- Returns:
- IMUData object containing all current IMU data
-
setPriorityTagID
Sets the priority tag ID for the Limelight camera. This is used to prioritize a specific tag for detection, which can be useful for tracking a specific target.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)ID
- ID of the tag to prioritize
-
setCropWindow
public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) Sets the crop window for the camera. The crop window in the UI must be completely open. This can be useful for increasing the Limelight's frame rate as the camera will only process the cropped area.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)cropXMin
- Minimum X value (-1 to 1)cropXMax
- Maximum X value (-1 to 1)cropYMin
- Minimum Y value (-1 to 1)cropYMax
- Maximum Y value (-1 to 1)
-
setFiducial3DOffset
public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) Sets the offset of the april tag tracking point,- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)offsetX
- Offset in the X direction in metersoffsetY
- Offset in the Y direction in metersoffsetZ
- Offset in the Z direction in meters
-
setRobotOrientation
public static void setRobotOrientation(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate) Sets robot orientation values used by MegaTag2 localization algorithm.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)yaw
- Robot yaw in degrees. 0 = robot facing red alliance wall in FRCyawRate
- (Unnecessary) Angular velocity of robot yaw in degrees per secondpitch
- (Unnecessary) Robot pitch in degreespitchRate
- (Unnecessary) Angular velocity of robot pitch in degrees per secondroll
- (Unnecessary) Robot roll in degreesrollRate
- (Unnecessary) Angular velocity of robot roll in degrees per second
-
setRobotOrientation
Sets robot yaw used by MegaTag2 localization algorithm. Puts 0 for all other orientation values.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)yaw
- Robot yaw in degrees. 0 = robot facing red alliance wall in FRC
-
setIMUMode
Deprecated.Configures the IMU mode for MegaTag2 Localization. This method is deprecated, usesetIMUMode(String, IMUMode)
instead.- Parameters:
limelightName
- Name/identifier of the LimelightimuMode
- IMU mode, 0-4.
-
setIMUMode
Configures the IMU mode for MegaTag2 Localization- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)imuMode
- The IMU mode to set, uses theIMUData.IMUMode
enum.
-
setLimelightThrottle
Sets the throttle for the Limelight camera. This is used to reduce the camera's processing load, which can be useful for decreasing temperatures.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)throttle
- Number of frames to skip between each processed frame. 0 = process every frame, 1 = process every other frame, etc.
-
setIMUAssistAlpha
Configures the complementary filter alpha value for IMU Assist Modes (Modes 3 and 4)- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)alpha
- Defaults to .001. Higher values will cause the internal IMU to converge onto the assist source more rapidly.
-
setFidcuial3DOffset
Sets the 3D point-of-interest offset for the current fiducial pipeline. https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)x
- X offset in metersy
- Y offset in metersz
- Z offset in meters
-
setFiducialIDFiltersOverride
Overrides the valid AprilTag IDs that will be used for localization. Tags not in this list will be ignored for robot pose estimation.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)validIDs
- Array of valid AprilTag IDs to track
-
setFiducialDownscalingOverride
Sets the downscaling factor for AprilTag detection. Increasing downscale can improve performance at the cost of potentially reduced detection range.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)downscale
- Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control.
-
setCameraPoseRobotSpace
public static void setCameraPoseRobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) Sets the camera pose relative to the robot.- Parameters:
limelightName
- The name of the Limelight set in the UI ("" for default)forward
- Forward offset in metersside
- Side offset in metersup
- Up offset in metersroll
- Roll angle in degreespitch
- Pitch angle in degreesyaw
- Yaw angle in degrees
-