From ffc49821c0a9b3a053560e17057e6cb459a1a3f7 Mon Sep 17 00:00:00 2001 From: Jacob Hotz <77470805+Jacob1010-h@users.noreply.github.com> Date: Wed, 22 Nov 2023 16:42:28 -0800 Subject: [PATCH] Refactor LimelightCameraUtil and LimelightCamera classes for limelight support --- .../frc/robot/commands/AutoAlignment.java | 69 +++++--- .../robot/subsystems/PhotonCameraUtil.java | 147 ------------------ .../subsystems/camera/LimelightCamera.java | 50 ++++-- .../camera/LimelightCameraUtil.java | 109 ------------- 4 files changed, 85 insertions(+), 290 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/PhotonCameraUtil.java delete mode 100644 src/main/java/frc/robot/subsystems/camera/LimelightCameraUtil.java diff --git a/src/main/java/frc/robot/commands/AutoAlignment.java b/src/main/java/frc/robot/commands/AutoAlignment.java index f6c876d8..d0420a10 100644 --- a/src/main/java/frc/robot/commands/AutoAlignment.java +++ b/src/main/java/frc/robot/commands/AutoAlignment.java @@ -1,5 +1,6 @@ package frc.robot.commands; +import java.io.IOException; import java.util.Optional; import java.util.function.DoubleSupplier; @@ -10,18 +11,20 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.DriverUI; -import frc.robot.subsystems.PhotonCameraUtil; import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.camera.LimelightCamera; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.ClawConstants; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.PlacementConstants; -import frc.robot.util.Constants.VisionConstants; import edu.wpi.first.wpilibj.Timer; -import org.photonvision.EstimatedRobotPose; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -42,7 +45,9 @@ public class AutoAlignment { */ Swerve swerve; - PhotonCameraUtil photonVision; + LimelightCamera frontCam; + + AprilTagFieldLayout aprilTagFieldLayout; private int tagID; private int coneOffset = 0; @@ -53,9 +58,15 @@ public class AutoAlignment { // tag private double currentNorm = -1; - public AutoAlignment(Swerve swerve, PhotonCameraUtil photonVision) { + public AutoAlignment(Swerve swerve, LimelightCamera limelight) { this.swerve = swerve; - this.photonVision = photonVision; + this.frontCam = limelight; + + try { + aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); + } catch (IOException e) { + System.out.println("AprilTag field layout not found!"); + } } /** @@ -66,25 +77,39 @@ public Command calibrateOdometry() { return Commands.run(() -> { // Create an "Optional" object that contains the estimated pose of the robot // This can be present (see's tag) or not present (does not see tag) - Optional result = photonVision.getEstimatedRobotPose(); + + // TODO: I don't think that optionals work because technically the object wont be null + // this will probably need to get changed. + Optional result = Optional.ofNullable(frontCam.getBotPose(false).toPose2d()); // If the result of the estimatedRobotPose exists, and the skew of the tag is // less than 3 degrees (to prevent false results) if (result.isPresent()) { - EstimatedRobotPose camEstimatedPose = result.get(); - // Add the vision measurement to the pose estimator to update the odometry swerve.getPoseEstimator().addVisionMeasurement( - camEstimatedPose.estimatedPose.toPose2d(), - Timer.getFPGATimestamp() - VisionConstants.LATENCY); + result.get(), + frontCam.getCaptureLatency()); } - if (photonVision.aprilTagFieldLayout.getTagPose(tagID).isPresent()) { + Optional targets = Optional.ofNullable(frontCam.getTargetPose(false)); + + if ( targets.isPresent() ) { // Get the target pose (the pose of the tag we want to go to) - Pose2d targetPose = photonVision.aprilTagFieldLayout.getTagPose(tagID).get().toPose2d(); + Pose2d targetPose = new Pose3d( + targets.get()[0], + targets.get()[1], + targets.get()[2], + new Rotation3d( + targets.get()[3], + targets.get()[4], + targets.get()[5] + ) + ).toPose2d(); + + frontCam.getTargetPose(false); targetPose = getModifiedTargetPose(targetPose); currentNorm = swerve.getPose().minus(targetPose).getTranslation().getNorm(); } - }, photonVision + }, frontCam ); } @@ -95,9 +120,9 @@ public void setNearestAlignmentOffset() { return; } - if (photonVision.aprilTagFieldLayout.getTagPose(tagID).isPresent()) { + if (aprilTagFieldLayout.getTagPose(tagID).isPresent()) { - Pose2d aprilTagPose2d = photonVision.aprilTagFieldLayout.getTagPose(tagID).get().toPose2d(); + Pose2d aprilTagPose2d = aprilTagFieldLayout.getTagPose(tagID).get().toPose2d(); double tagXOffset = getTagXOffset(); double tagYOffset = getTagYOffset(); @@ -156,9 +181,9 @@ public ChassisSpeeds getAutoAlignChassisSpeeds() { Pose2d targetPose = swerve.getPose(); // Check if our tagID is valid... (Assume it is for logic purposes) - if (photonVision.aprilTagFieldLayout.getTagPose(tagID).isPresent()) { + if (aprilTagFieldLayout.getTagPose(tagID).isPresent()) { // Get the target pose (the pose of the tag we want to go to) - targetPose = photonVision.aprilTagFieldLayout.getTagPose(tagID).get().toPose2d(); + targetPose = aprilTagFieldLayout.getTagPose(tagID).get().toPose2d(); } // If we are on the left side of the field: we need to add the grid offset + @@ -227,9 +252,9 @@ public int getNearestTag() { // This if a statement prevents the robot from crashing if we input an absurd // tag ID, // but it should be assumed that the tag location is present. - if (photonVision.aprilTagFieldLayout.getTagPose(i).isPresent()) { + if (aprilTagFieldLayout.getTagPose(i).isPresent()) { currentDistance = currentPosition.getDistance( - photonVision.aprilTagFieldLayout.getTagPose(i).get().toPose2d().getTranslation()); + aprilTagFieldLayout.getTagPose(i).get().toPose2d().getTranslation()); } if (currentDistance < nearestDistance) { nearestDistance = currentDistance; @@ -249,9 +274,9 @@ public int getNearestTag() { // This if a statement prevents the robot from crashing if we input an absurd // tag ID, // but it should be assumed that the tag location is present. - if (photonVision.aprilTagFieldLayout.getTagPose(i).isPresent()) { + if (aprilTagFieldLayout.getTagPose(i).isPresent()) { currentDistance = currentPosition.getDistance( - photonVision.aprilTagFieldLayout.getTagPose(i).get().toPose2d().getTranslation()); + aprilTagFieldLayout.getTagPose(i).get().toPose2d().getTranslation()); } if (currentDistance < nearestDistance) { nearestDistance = currentDistance; diff --git a/src/main/java/frc/robot/subsystems/PhotonCameraUtil.java b/src/main/java/frc/robot/subsystems/PhotonCameraUtil.java deleted file mode 100644 index bc5c281a..00000000 --- a/src/main/java/frc/robot/subsystems/PhotonCameraUtil.java +++ /dev/null @@ -1,147 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.DriverUI; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.targeting.PhotonPipelineResult; -import java.io.IOException; -import java.util.Optional; - -import frc.robot.util.Constants.FieldConstants; -import frc.robot.util.Constants.VisionConstants; - -public class PhotonCameraUtil extends SubsystemBase { - - public PhotonCamera cam1; - public PhotonCamera cam2; - public PhotonPoseEstimator cam1PoseEstimator; - public PhotonPoseEstimator cam2PoseEstimator; - public AprilTagFieldLayout aprilTagFieldLayout; - - public PhotonCameraUtil() { - - // Load the AprilTag field layout from the "resource file" at - // edu.wpi.first.apriltag.AprilTagFieldLayout - try { - aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); - } catch (IOException e) { - System.out.println("AprilTag field layout not found!"); - } - - try { - cam1 = new PhotonCamera(VisionConstants.CAMERA_1_NAME); - } catch (Exception e) { - System.out.println("Camera 1 (front) not found!"); - } - - try { - cam2 = new PhotonCamera(VisionConstants.CAMERA_2_NAME); - } catch (Exception e) { - System.out.println("Camera 2 (back) not found!"); - } - - cam1PoseEstimator = new PhotonPoseEstimator( - aprilTagFieldLayout, - PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY, - cam1, - VisionConstants.CAMERA_1_POSITION); - - cam2PoseEstimator = new PhotonPoseEstimator( - aprilTagFieldLayout, - PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY, - cam2, - VisionConstants.CAMERA_2_POSITION); - - } - - // Look through both cameras and set the reference pose with - // the camera that has the lowest ambiguity if both cameras can see a target, - // or, - // the camera that can see a target if only one camera can see a target - // only if it's ambiguity is lower than the threshold - public Optional getEstimatedRobotPose() { - - if (FieldConstants.IS_SIMULATION) { - return Optional.empty(); - } - - // First, update the pose estimators - Optional cam1Pose = cam1PoseEstimator.update(); - // Optional cam2Pose = cam2PoseEstimator.update(); - // Then, get the current camera data - Optional cam1 = getCam1(); - // Optional cam2 = getCam2(); - // Make ambiguity doubles to compare the results - double cam1Ambiguity = 1; - // double cam2Ambiguity = 1; - - // Get the ambiguity of each camera, if applicable - if (cam1.isPresent()) { - PhotonPipelineResult result = cam1.get().getLatestResult(); - if (result.hasTargets()) { - cam1Ambiguity = result.getBestTarget().getPoseAmbiguity(); - DriverUI.hasTargets = true; - if (cam1Ambiguity < VisionConstants.AMBIGUITY_THRESHOLD) { - return cam1Pose; - } - } - } - /* - * if (cam2.isPresent()) { - * PhotonPipelineResult result = cam2.get().getLatestResult(); - * if (result.hasTargets()) { - * cam2Ambiguity = result.getBestTarget().getPoseAmbiguity(); - * DriverUI.hasTargets = true; - * } - * } - * - * // If both cameras have results, and both cameras can see a target, compare - * them - * if (cam1Pose.isPresent() && cam2Pose.isPresent()) { - * // Return the camera with the lower ambiguity (only if under the minimum - * threshold) - * // Recall that the lower the ambiguity, the more confident we are in the pose - * if (cam1Ambiguity < cam2Ambiguity && cam1Ambiguity < - * VisionConstants.AMBIGUITY_THRESHOLD) { - * return cam1Pose; - * } else if (cam2Ambiguity < VisionConstants.AMBIGUITY_THRESHOLD) { - * return cam2Pose; - * } - * } - * - * // If only the front camera has a result, and it can see a target, return it - * // * if it has low ambiguity - * if (cam1Pose.isPresent() && cam1Ambiguity < - * VisionConstants.AMBIGUITY_THRESHOLD) { - * return cam1Pose; - * } - * - * // If only the back camera has a result, and it can see a target, return it - * // * if it has low ambiguity - * if (cam2Pose.isPresent() && cam2Ambiguity < - * VisionConstants.AMBIGUITY_THRESHOLD) { - * return cam2Pose; - * } - */ - DriverUI.hasTargets = false; - // return a null Optional if no camera can see a target - return Optional.empty(); - } - - public Optional getCam1() { - return Optional.ofNullable(cam1); - } - - public Optional getCam2() { - return Optional.ofNullable(cam2); - } - - @Override - public void periodic() { - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/camera/LimelightCamera.java b/src/main/java/frc/robot/subsystems/camera/LimelightCamera.java index 39a45930..f00dc871 100644 --- a/src/main/java/frc/robot/subsystems/camera/LimelightCamera.java +++ b/src/main/java/frc/robot/subsystems/camera/LimelightCamera.java @@ -1,18 +1,20 @@ package frc.robot.subsystems.camera; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.camera.LightMode.SnapMode; import java.util.*; // https://github.com/NAHSRobotics-Team5667/2020-FRC/blob/master/src/main/java/frc/robot/utils/LimeLight.java -public class LimelightCamera { - - private String name; +public class LimelightCamera extends SubsystemBase{ private NetworkTable table; @@ -25,9 +27,7 @@ public class LimelightCamera { public SendableChooser snapshotChooser = new SendableChooser<>(); public SendableChooser lightChooser = new SendableChooser<>(); - public LimelightCamera(String name) { - this.name = name; - + public LimelightCamera() { table = NetworkTableInstance.getDefault().getTable("limelight"); tx = table.getEntry("tx"); @@ -41,10 +41,6 @@ public LimelightCamera(String name) { lightChooser.setDefaultOption("Off", false); lightChooser.addOption("On", true); } - - public String getName() { - return name; - } /** * @return ID of the primary in-view AprilTag @@ -53,6 +49,20 @@ public double[] getTagID() { return table.getEntry("tid").getDoubleArray(new double[6]); } + + + public Pose3d convertBotEntry(double[] entry) { + return new Pose3d( + entry[0], + entry[1], + entry[2], + new Rotation3d( + Units.degreesToRadians(entry[3]), + Units.degreesToRadians(entry[4]), + Units.degreesToRadians(entry[5]) + )); + } + /** * Robot transform in field-space. * Translation (X,Y,Z) @@ -62,8 +72,14 @@ public double[] getTagID() { * @param targetSpace if the bot pose should be send in targetSpace. * @return the robot pose */ - public double[] getBotPose(boolean targetSpace) { - return table.getEntry((!targetSpace) ? "botpose" : "botpose_targetspace").getDoubleArray(new double[6]); + public Pose3d getBotPose(boolean targetSpace) { + return convertBotEntry( getRawBotPose(targetSpace) ); + } + + public double[] getRawBotPose(boolean targetSpace) { + return table.getEntry( + (!targetSpace) ? "botpose" + : "botpose_targetspace").getDoubleArray(new double[6]); } /** @@ -74,6 +90,7 @@ public double[] getBotPose(boolean targetSpace) { * * @return the robot pose with the blue driverstation WPILIB origin */ + @Deprecated public double[] getBluePose() { return table.getEntry("botpose_wpiblue").getDoubleArray(new double[6]); } @@ -86,6 +103,7 @@ public double[] getBluePose() { * * @return the robot pose with the red driverstation WPILIB origin */ + @Deprecated public double[] getRedPose() { return table.getEntry("botpose_wpired").getDoubleArray(new double[6]); } @@ -119,6 +137,14 @@ public double[] getCameraPose(boolean targetSpace) { public double[] getTargetPose(boolean cameraSpace) { return table.getEntry((cameraSpace) ? "targetpose_cameraspace" : "targetpose_robotspace").getDoubleArray(new double[6]); } + + public boolean containsTagID(boolean cameraSpace, int tagID) { + double[] targetPose = getTargetPose(cameraSpace); + for (double pose : targetPose) { + if (pose == tagID) { return true; } + } + return false; + } /** * Are we currently tracking any potential targets diff --git a/src/main/java/frc/robot/subsystems/camera/LimelightCameraUtil.java b/src/main/java/frc/robot/subsystems/camera/LimelightCameraUtil.java deleted file mode 100644 index ab60c48e..00000000 --- a/src/main/java/frc/robot/subsystems/camera/LimelightCameraUtil.java +++ /dev/null @@ -1,109 +0,0 @@ -package frc.robot.subsystems.camera; - -import java.io.IOException; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.DriverUI; -import frc.robot.util.Constants.FieldConstants; -import frc.robot.util.Constants.VisionConstants; - -import java.util.Optional; - -public class LimelightCameraUtil extends SubsystemBase{ - - public LimelightCamera cam1; - public LimelightCamera cam2; - - public SwerveDrivePoseEstimator cam1PoseEstimator; - public SwerveDrivePoseEstimator cam2PoseEstimator; - - public AprilTagFieldLayout aprilTagFieldLayout; - - public LimelightCameraUtil() { - // Load the AprilTag field layout from the "resource file" at - // edu.wpi.first.apriltag.AprilTagFieldLayout - try { - aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); - } catch (IOException e) { - System.out.println("AprilTag field layout not found!"); - } - - try { - cam1 = new LimelightCamera(VisionConstants.CAMERA_1_NAME); - } catch (Exception e) { - System.out.println("Camera 1 (front) not found!"); - } - - try { - cam2 = new LimelightCamera(VisionConstants.CAMERA_2_NAME); - } catch (Exception e) { - System.out.println("Camera 2 (back) not found!"); - } - - //TODO: replace nulls - cam1PoseEstimator = new SwerveDrivePoseEstimator( - null, - null, - null, - null, - null, - null); - - //TODO: replace nulls - cam2PoseEstimator = new SwerveDrivePoseEstimator( - null, - null, - null, - null, - null, - null); - } - - public Optional getEstimatedRobotPose() { - - if (FieldConstants.IS_SIMULATION) { - return Optional.empty(); - } - - // First, update the pose estimators - - // TODO: replace nulls with gyro angles and mod positions - // TODO: use update or add vision mesurement with respect to tl and cl - Optional cam1Pose = - Optional.ofNullable( cam1PoseEstimator.update(null, null)) ; - - // Optional cam2Pose = Optional.ofNullable(cam2PoseEstimator.update(null, null)); - - // Then, get the current camera data - Optional cam1 = getCam1(); - // Optional cam2 = getCam2(); - // Make ambiguity doubles to compare the results - - // double cam2Ambiguity = 1; - - // TODO: send the cam1 pose or send the cam1 Pose with other table settings? - if (cam1.isPresent() && cam1.get().hasValidTarget()) { - return cam1Pose; - } - - DriverUI.hasTargets = false; - // return a null Optional if no camera can see a target - return Optional.empty(); - } - - public Optional getCam1() { - return Optional.ofNullable(cam1); - } - - public Optional getCam2() { - return Optional.ofNullable(cam2); - } - - @Override - public void periodic() { - } -}