Using Target Data
A PhotonUtils
class with helpful common calculations is included within PhotonLib
to aid teams in using target data in order to get positional information on the field. This class contains two methods, calculateDistanceToTargetMeters()
/CalculateDistanceToTarget()
and estimateTargetTranslation2d()
/EstimateTargetTranslation()
(Java and C++ respectively).
Calculating Distance to Target
If your camera is at a fixed height on your robot and the height of the target is fixed, you can calculate the distance to the target based on your camera’s pitch and the pitch to the target.
if (xboxController.getAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
var result = camera.getLatestResult();
if (result.hasTargets()) {
// First calculate range
double range =
PhotonUtils.calculateDistanceToTargetMeters(
CAMERA_HEIGHT_METERS,
TARGET_HEIGHT_METERS,
CAMERA_PITCH_RADIANS,
Units.degreesToRadians(result.getBestTarget().getPitch()));
// Use this range as the measurement we give to the PID controller.
// -1.0 required to ensure positive PID controller effort _increases_ range
forwardSpeed = -controller.calculate(range, GOAL_RANGE_METERS);
if (xboxController.GetAButton()) {
// Vision-alignment mode
// Query the latest result from PhotonVision
photonlib::PhotonPipelineResult result = camera.GetLatestResult();
if (result.HasTargets()) {
// First calculate range
units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget(
CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH,
units::degree_t{result.GetBestTarget().GetPitch()});
// Use this range as the measurement we give to the PID controller.
forwardSpeed =
-controller.Calculate(range.value(), GOAL_RANGE_METERS.value());
Note
The C++ version of PhotonLib uses the Units library. For more information, see here.
Calculating Distance Between Two Poses
getDistanceToPose(Pose2d robotPose, Pose2d targetPose)
allows you to calculate the distance between two poses. This is useful when using AprilTags, given that there may not be an AprilTag directly on the target.
double distanceToTarget = PhotonUtils.getDistanceToPose(robotPose, targetPose);
//TODO
Estimating Camera Translation to Target
You can get a translation to the target based on the distance to the target (calculated above) and angle to the target (yaw).
// Calculate a translation from the camera to the target.
Translation2d translation = PhotonUtils.estimateCameraToTargetTranslation(
distanceMeters, Rotation2d.fromDegrees(-target.getYaw()));
// Calculate a translation from the camera to the target.
frc::Translation2d translation = photonlib::PhotonUtils::EstimateCameraToTargetTranslationn(
distance, frc::Rotation2d(units::degree_t(-target.GetYaw())));
Note
We are negating the yaw from the camera from CV (computer vision) conventions to standard mathematical conventions. In standard mathematical conventions, as you turn counter-clockwise, angles become more positive.
Estimating Field Relative Pose (Traditional)
You can get your robot’s Pose2D
on the field using various camera data, target yaw, gyro angle, target pose, and camera position. This method estimates the target’s relative position using estimateCameraToTargetTranslation
(which uses pitch and yaw to estimate range and heading), and the robot’s gyro to estimate the rotation of the target.
// Calculate robot's field relative pose
Pose2D robotPose = PhotonUtils.estimateFieldToRobot(
kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, Rotation2d.fromDegrees(-target.getYaw()), gyro.getRotation2d(), targetPose, cameraToRobot);
// Calculate robot's field relative pose
frc::Pose2D robotPose = photonlib::EstimateFieldToRobot(
kCameraHeight, kTargetHeight, kCameraPitch, kTargetPitch, frc::Rotation2d(units::degree_t(-target.GetYaw())), frc::Rotation2d(units::degree_t(gyro.GetRotation2d)), targetPose, cameraToRobot);
Getting the Yaw To a Pose
getYawToPose(Pose2d robotPose, Pose2d targetPose)
returns the Rotation2d
between your robot and a target. This is useful when turning towards an arbitrary target on the field (ex. the center of the hub in 2022).
Rotation2d targetYaw = PhotonUtils.getYawToPose(robotPose, targetPose);
//TODO