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")
def pubVersion = '2023.1.0'
def pubVersion = '2023.1.1-SNAPSHOT'
def outputsFolder = file("$buildDir/allOutputs")

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

@ -9,30 +9,52 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.EstimatedRobotPose;
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.Transform3d;
public class RobotFieldPosition {
public class RobotFieldPosition {
AprilTagFieldLayout layout;
PhotonPoseEstimator photonPoseEstimator;
/*
Example of Transform3d:
Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0));
Cam mounted facing forward, half a meter forward of center, half a meter up from center.
*/
* Example of Transform3d:
*
* Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
* 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 {
layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile);
PhotonCamera cam = new PhotonCamera(camera);
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() {
return photonPoseEstimator.update();
}
}
}