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

Expose photonVision classes

This commit is contained in:
Tyler Wilcox 2023-03-28 10:39:46 -04:00
parent 4e5588d2a8
commit fde77e106c
2 changed files with 11 additions and 6 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.3.2' def pubVersion = '2023.3.3'
def outputsFolder = file("$buildDir/allOutputs") def outputsFolder = file("$buildDir/allOutputs")

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

@ -11,13 +11,12 @@ 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.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
public class RobotFieldPosition { public class RobotFieldPosition {
AprilTagFieldLayout layout; AprilTagFieldLayout layout;
PhotonPoseEstimator photonPoseEstimator; PhotonPoseEstimator photonPoseEstimator;
PhotonCamera camera;
Optional<EstimatedRobotPose> frozenPose; Optional<EstimatedRobotPose> frozenPose;
boolean frozen = false; boolean frozen = false;
@ -46,8 +45,6 @@ public class RobotFieldPosition {
public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException { public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException {
layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile);
layout.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
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);
} }
@ -74,6 +71,14 @@ public class RobotFieldPosition {
photonPoseEstimator.setLastPose(pose); photonPoseEstimator.setLastPose(pose);
} }
public PhotonPoseEstimator getPhotonPoseEstimator(){
return photonPoseEstimator;
}
public PhotonCamera getPhotonCamera(){
return camera;
}
// For when you want it to stay the same across lines // For when you want it to stay the same across lines
public void freeze() { public void freeze() {
frozen = true; frozen = true;
@ -85,4 +90,4 @@ public class RobotFieldPosition {
} }
} }