Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-24 01:31:44 +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());
}
}

24
photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java
View File

@@ -24,9 +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.*;
import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.Test;
@@ -81,4 +79,24 @@ class PhotonUtilTest {
Assertions.assertEquals(0, fieldToRobot.getY(), 0.1);
Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1);
}
@Test
public void testAprilTagUtils() {
var cameraToTarget = new Transform3d(new Translation3d(1, 0, 0), new Rotation3d());
var tagPose = new Pose3d(5, 0, 0, new Rotation3d());
var cameraToRobot = new Transform3d();
var fieldToRobot =
PhotonUtils.estimateFieldToRobotAprilTag(cameraToTarget, tagPose, cameraToRobot);
var targetPose =
new Pose2d(
new Translation2d(Units.inchesToMeters(324), Units.inchesToMeters(162)),
new Rotation2d());
var currentPose = new Pose2d(0, 0, Rotation2d.fromDegrees(0));
Assertions.assertEquals(4.0, fieldToRobot.getX());
Assertions.assertEquals(
Math.toDegrees(Math.atan2((Units.inchesToMeters(162)), (Units.inchesToMeters(324)))),
PhotonUtils.getYawToPose(currentPose, targetPose).getDegrees());
}
}