Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add distance, yaw, and robot pose methods to photonlib (#642)

Browse Source
This commit is contained in:
Mohammad Durrani
2022-12-25 00:00:00 -05:00
committed by GitHub
parent 96006fc501
commit 915f784d9d
2 changed files with 61 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
photon-lib/src/main/java/org/photonvision/PhotonUtils.java
View File

@@ -24,10 +24,7 @@
package org.photonvision;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.*;
public final class PhotonUtils {
private PhotonUtils() {
@@ -170,4 +167,43 @@ public final class PhotonUtils {
var targetToCamera = cameraToTarget.inverse();
return fieldToTarget.transformBy(targetToCamera);
}
/**
* Estimates the pose of the robot in the field coordinate system, given the pose of the fiducial
* tag, the robot relative to the camera, and the target relative to the camera.
*
* @param fieldRelativeTagPose Pose3D the field relative pose of the target
* @param cameraToRobot Transform3D of the robot relative to the camera. Origin of the robot is
* defined as the center.
* @param cameraToTarget Transform3D of the target relative to the camera, returned by
* PhotonVision
* @return Transform3d Robot position relative to the field
*/
public static Pose3d estimateFieldToRobotAprilTag(
Transform3d cameraToTarget, Pose3d fieldRelativeTagPose, Transform3d cameraToRobot) {
return fieldRelativeTagPose.plus(cameraToTarget.inverse()).plus(cameraToRobot);
}
/**
* Returns the yaw between your robot and a target.
*
* @param robotPose Current pose of the robot
* @param targetPose Pose of the target on the field
* @return double Yaw to the target
*/
public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) {
Translation2d relativeTrl = targetPose.relativeTo(robotPose).getTranslation();
return new Rotation2d(relativeTrl.getX(), relativeTrl.getY());
}
/**
* Returns the distance between two poses
*
* @param robotPose Pose of the robot
* @param targetPose Pose of the target
* @return
*/
public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) {
return robotPose.getTranslation().getDistance(targetPose.getTranslation());
}
}