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

resolved issues

This commit is contained in:
Declipsonator 2023-01-30 20:52:23 -05:00
parent db7a68e8ed
commit 98ced09d82
3 changed files with 25 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
lib/build.gradle
View File

@ -29,15 +29,15 @@ dependencies {
// This dependency is used internally, and not exposed to consumers on their own compile classpath. // This dependency is used internally, and not exposed to consumers on their own compile classpath.
implementation 'com.google.guava:guava:30.1.1-jre' implementation 'com.google.guava:guava:30.1.1-jre'
implementation 'edu.wpi.first.cscore:cscore-java:2023.2.1' implementation 'edu.wpi.first.cscore:cscore-java:2023.+'
implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.2.1' implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.+'
implementation 'edu.wpi.first.ntcore:ntcore-java:2023.2.1' implementation 'edu.wpi.first.ntcore:ntcore-java:2023.+'
implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.2.1' implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.+'
implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.2.1' implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.+'
implementation 'edu.wpi.first.wpimath:wpimath-java:2023.2.1' implementation 'edu.wpi.first.wpimath:wpimath-java:2023.+'
implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.2.1' implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.+'
implementation 'edu.wpi.first.hal:hal-java:2023.2.1' implementation 'edu.wpi.first.hal:hal-java:2023.+'
implementation "edu.wpi.first.apriltag:apriltag-java:2023.2.1" implementation "edu.wpi.first.apriltag:apriltag-java:2023.+"
implementation "org.ejml:ejml-simple:0.38" implementation "org.ejml:ejml-simple:0.38"
implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0" implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0"
implementation "com.fasterxml.jackson.core:jackson-core:2.10.0" implementation "com.fasterxml.jackson.core:jackson-core:2.10.0"
@ -45,8 +45,8 @@ dependencies {
implementation 'edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:4.5.2-1' implementation 'edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:4.5.2-1'
implementation 'com.ctre.phoenix:api-java:5.30.3' implementation 'com.ctre.phoenix:api-java:5.30.3'
implementation 'com.ctre.phoenix:wpiapi-java:5.30.3' implementation 'com.ctre.phoenix:wpiapi-java:5.30.3'
implementation "org.photonvision:PhotonLib-java:v2023.2.1" implementation "org.photonvision:PhotonLib-java:v2023.+"
implementation "org.photonvision:PhotonTargeting-java:v2023.2.1" implementation "org.photonvision:PhotonTargeting-java:v2023.+"
} }
tasks.named('test') { tasks.named('test') {

24
lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java
View File

@ -25,11 +25,12 @@ public class AprilTags {
for(PhotonCamera camera : cameras) { for(PhotonCamera camera : cameras) {
for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) {
boolean toAdd = true; boolean toAdd = true;
List<PhotonTrackedTarget> tempTargets = targets;
// check if the target is already in the list and if it has a higher pose ambiguity // check if the target is already in the list and if it has a higher pose ambiguity
for(PhotonTrackedTarget arTarget : targets) { for(PhotonTrackedTarget arTarget : targets) {
if(target.getFiducialId() == arTarget.getFiducialId() && target.getPoseAmbiguity() < arTarget.getPoseAmbiguity()) { if(target.getFiducialId() == arTarget.getFiducialId() && target.getPoseAmbiguity() < arTarget.getPoseAmbiguity()) {
// if it does, remove it from the list // if it does, remove it from the list
targets.remove(arTarget); tempTargets.remove(arTarget);
break; break;
} else if(target.getFiducialId() == arTarget.getFiducialId()) { } else if(target.getFiducialId() == arTarget.getFiducialId()) {
// if the target is already in the list and has the same ID, don't add it // if the target is already in the list and has the same ID, don't add it
@ -37,10 +38,10 @@ public class AprilTags {
break; break;
} }
} }
if(toAdd) { tempTargets.add(target); }
if(toAdd) targets.add(target); targets = tempTargets;
} }
targets.addAll(camera.getLatestResult().getTargets());
} }
return targets; return targets;
@ -48,36 +49,31 @@ public class AprilTags {
// method to check if any camera has any targets // method to check if any camera has any targets
public boolean hasTargets() { public boolean hasTargets() {
for(PhotonCamera camera : cameras) { return cameras.stream().anyMatch((camera) -> camera.getLatestResult().hasTargets() == true);
// if a camera has targets, return true
if(camera.getLatestResult().hasTargets()) return true;
}
return false;
} }
// method to get the closest target // method to get the closest target
public PhotonTrackedTarget getClosestTarget() { public PhotonTrackedTarget getClosestTarget() {
if(!hasTargets()) return null; if(!hasTargets()) { return null; }
PhotonTrackedTarget closestTarget = null; PhotonTrackedTarget closestTarget = null;
for(PhotonTrackedTarget target : getTargets()) { for(PhotonTrackedTarget target : getTargets()) {
if(closestTarget == null || calculateDistance(target.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget()) ) closestTarget = target; if(closestTarget == null || calculateDistance(closestTarget.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget())) { closestTarget = target; }
} }
return closestTarget; return closestTarget;
} }
// method to get the target with the lowest pose ambiguity // method to get the target with the lowest pose ambiguity
public PhotonTrackedTarget getLowestAmbiguityTarget() { public PhotonTrackedTarget getLowestAmbiguityTarget() {
if(!hasTargets()) return null; if(!hasTargets()) { return null; }
PhotonTrackedTarget lowestAmbiguity = null; PhotonTrackedTarget lowestAmbiguity = null;
for(PhotonTrackedTarget target : getTargets()) { for(PhotonTrackedTarget target : getTargets()) {
// check if the target has a lower pose ambiguity than the current lowest ambiguity target // check if the target has a lower pose ambiguity than the current lowest ambiguity target
if(lowestAmbiguity == null || lowestAmbiguity.getPoseAmbiguity() > target.getPoseAmbiguity()) lowestAmbiguity = target; if(lowestAmbiguity == null || lowestAmbiguity.getPoseAmbiguity() > target.getPoseAmbiguity()) { lowestAmbiguity = target; }
} }
return lowestAmbiguity; return lowestAmbiguity;
} }
private double calculateDistance(Transform3d t) { private double calculateDistance(Transform3d t) {
return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY() + t.getZ()*t.getZ()); return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY());
} }
} }

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

@ -9,7 +9,6 @@ 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;
@ -23,12 +22,10 @@ public class RobotFieldPosition {
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), new Rotation3d(0,0,0));
Cam mounted facing forward, half a meter forward of center, half a meter up from center. Cam mounted facing forward, half a meter forward of center, half a meter up from center.
*/ */
public RobotFieldPosition(String camera, Transform3d robotToCam) { public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile) throws IOException {
try {
layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile); layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile);
} catch(IOException exception) {
exception.printStackTrace();
}
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);
} }