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

Additions to Fix Crashes & Improve Code

This commit is contained in:
Declipsonator 2023-02-16 16:34:00 -08:00
parent ff0b1f258c
commit a8b7ee06aa
3 changed files with 76 additions and 48 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.
implementation 'com.google.guava:guava:30.1.1-jre'
implementation 'edu.wpi.first.cscore:cscore-java:2023.2.1'
implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.2.1'
implementation 'edu.wpi.first.ntcore:ntcore-java:2023.2.1'
implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.2.1'
implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.2.1'
implementation 'edu.wpi.first.wpimath:wpimath-java:2023.2.1'
implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.2.1'
implementation 'edu.wpi.first.hal:hal-java:2023.2.1'
implementation "edu.wpi.first.apriltag:apriltag-java:2023.2.1"
implementation 'edu.wpi.first.cscore:cscore-java:2023.+'
implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.+'
implementation 'edu.wpi.first.ntcore:ntcore-java:2023.+'
implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.+'
implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.+'
implementation 'edu.wpi.first.wpimath:wpimath-java:2023.+'
implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.+'
implementation 'edu.wpi.first.hal:hal-java:2023.+'
implementation "edu.wpi.first.apriltag:apriltag-java:2023.+"
implementation "org.ejml:ejml-simple:0.38"
implementation "com.fasterxml.jackson.core:jackson-annotations: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 'com.ctre.phoenix:api-java:5.30.3'
implementation 'com.ctre.phoenix:wpiapi-java:5.30.3'
implementation "org.photonvision:PhotonLib-java:v2023.2.1"
implementation "org.photonvision:PhotonTargeting-java:v2023.2.1"
implementation "org.photonvision:PhotonLib-java:v2023.+"
implementation "org.photonvision:PhotonTargeting-java:v2023.+"
}
tasks.named('test') {

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

@ -8,9 +8,11 @@ import org.photonvision.targeting.PhotonTrackedTarget;
import edu.wpi.first.math.geometry.Transform3d;
public class AprilTags {
public class AprilTags implements Cloneable {
// an ArrayList to store the cameras
public final ArrayList<PhotonCamera> cameras = new ArrayList<>();
public final ArrayList<PhotonTrackedTarget> frozenResults = new ArrayList<>();
public boolean frozen = false;
public AprilTags(String...cameraNames) {
for(String cameraName : cameraNames) {
@ -18,66 +20,95 @@ public class AprilTags {
}
}
// Method to get a list of all targets from all cameras
//Method to get a list of all targets without any filtering
public List<PhotonTrackedTarget> getAllTargets() {
return frozen ? frozenResults : cameras.stream().flatMap((camera) -> camera.getLatestResult().getTargets().stream()).toList();
}
// Method to get a list of all targets with duplicates removed
public List<PhotonTrackedTarget> getTargets() {
List<PhotonTrackedTarget> targets = new ArrayList<>();
for(PhotonCamera camera : cameras) {
for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) {
boolean toAdd = true;
// check if the target is already in the list and if it has a higher pose ambiguity
for(PhotonTrackedTarget arTarget : targets) {
if(target.getFiducialId() == arTarget.getFiducialId() && target.getPoseAmbiguity() < arTarget.getPoseAmbiguity()) {
// if it does, remove it from the list
targets.remove(arTarget);
break;
} else if(target.getFiducialId() == arTarget.getFiducialId()) {
// if the target is already in the list and has the same ID, don't add it
toAdd = false;
break;
}
for(PhotonTrackedTarget target : getAllTargets()) {
boolean toAdd = true;
List<PhotonTrackedTarget> tempTargets = targets;
// check if the target is already in the list and if it has a higher pose ambiguity
for(PhotonTrackedTarget arTarget : targets) {
if(target.getFiducialId() == arTarget.getFiducialId() && target.getPoseAmbiguity() < arTarget.getPoseAmbiguity()) {
// if it does, remove it from the list
tempTargets.remove(arTarget);
break;
} else if(target.getFiducialId() == arTarget.getFiducialId()) {
// if the target is already in the list and has the same ID, don't add it
toAdd = false;
break;
}
if(toAdd) targets.add(target);
}
targets.addAll(camera.getLatestResult().getTargets());
if(toAdd) { tempTargets.add(target); }
targets = tempTargets;
}
return targets;
}
// method to check if any camera has any targets
public boolean hasTargets() {
for(PhotonCamera camera : cameras) {
// if a camera has targets, return true
if(camera.getLatestResult().hasTargets()) return true;
}
return false;
return getAllTargets().size() != 0;
}
// method to get the closest target
public PhotonTrackedTarget getClosestTarget() {
if(!hasTargets()) return null;
if(!hasTargets()) { return null; }
PhotonTrackedTarget closestTarget = null;
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;
}
// method to get the target with the lowest pose ambiguity
public PhotonTrackedTarget getLowestAmbiguityTarget() {
if(!hasTargets()) return null;
if(!hasTargets()) { return null; }
PhotonTrackedTarget lowestAmbiguity = null;
for(PhotonTrackedTarget target : getTargets()) {
// 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;
}
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());
}
/*
* In cases where data needs to be referenced more than once, use this function
* to keep the data consistent and prevent any errors.
*
* Very Important!
*/
public void freeze() {
frozenResults.addAll(getAllTargets());
frozen = true;
}
//Unfreeze opposite of freeze
public void unFreeze() {
frozenResults.clear();
frozen = false;
}
// There might be a case where you need this so I'm keeping it
public Object cloneAndFreeze() {
try{
AprilTags clone = (AprilTags) super.clone();
clone.freeze();
return clone;
}catch(Exception e){
return null;
}
}
}

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 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;
@ -23,12 +22,10 @@ public class RobotFieldPosition {
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) {
try {
layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile);
} catch(IOException exception) {
exception.printStackTrace();
}
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);
}