FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

April tag changes

This commit is contained in:
Tyler Wilcox 2023-02-28 11:02:33 -05:00
parent 08c96805fc
commit ac81af8729
2 changed files with 34 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/publish.gradle
View File

@ -4,7 +4,7 @@ apply plugin: 'maven-publish'
ext.licenseFile = files("$rootDir/LICENSE.txt") ext.licenseFile = files("$rootDir/LICENSE.txt")
def pubVersion = '2023.1.0' def pubVersion = '2023.1.1-SNAPSHOT'
def outputsFolder = file("$buildDir/allOutputs") def outputsFolder = file("$buildDir/allOutputs")

36
lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java
View File

@ -9,6 +9,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.EstimatedRobotPose; import org.photonvision.EstimatedRobotPose;
import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
@ -17,22 +18,43 @@ public class RobotFieldPosition {
PhotonPoseEstimator photonPoseEstimator; PhotonPoseEstimator photonPoseEstimator;
/* /*
Example of Transform3d: * Example of Transform3d:
*
Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); * Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
Cam mounted facing forward, half a meter forward of center, half a meter up from center. * new Rotation3d(0,0,0));
*/ * Cam mounted facing forward, half a meter forward of center, half a meter up
* from center.
*/
public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile) throws IOException { public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile) throws IOException {
layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile);
PhotonCamera cam = new PhotonCamera(camera); PhotonCamera cam = new PhotonCamera(camera);
photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam);
} }
public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile, PoseStrategy strategy) throws IOException {
layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile);
PhotonCamera cam = new PhotonCamera(camera);
photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam);
}
public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException {
layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile);
PhotonCamera cam = new PhotonCamera(camera);
photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam);
}
public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields, PoseStrategy strategy) throws IOException {
layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile);
PhotonCamera cam = new PhotonCamera(camera);
photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam);
}
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() { public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
return photonPoseEstimator.update(); return photonPoseEstimator.update();
} }
} }