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

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());
}
}