From f76b6f6f239dcdd30123b11d0234ead525f5ccc2 Mon Sep 17 00:00:00 2001 From: Audrey Date: Sat, 21 Sep 2024 13:18:29 -0700 Subject: [PATCH 1/2] Base Camera Rewrite --- .../java/common/hardware/camera/Camera.java | 289 ++++++++---------- .../common/hardware/camera/NAR_Camera.java | 37 +-- 2 files changed, 143 insertions(+), 183 deletions(-) diff --git a/src/main/java/common/hardware/camera/Camera.java b/src/main/java/common/hardware/camera/Camera.java index 755cbc8..cff7c13 100644 --- a/src/main/java/common/hardware/camera/Camera.java +++ b/src/main/java/common/hardware/camera/Camera.java @@ -1,264 +1,223 @@ package common.hardware.camera; +import java.util.ArrayList; import java.util.LinkedList; import java.util.Optional; import java.util.function.BiConsumer; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import java.util.ArrayList; -import java.util.HashMap; import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import common.core.misc.NAR_Robot; +import common.utility.Log; import common.utility.shuffleboard.NAR_Shuffleboard; import edu.wpi.first.apriltag.AprilTagFieldLayout; +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.Transform3d; -import edu.wpi.first.wpilibj.DriverStation; -import java.util.HashSet; -import edu.wpi.first.apriltag.AprilTagFields; /** * Team 3128's class to control the robot's cameras and vision processing. * - * @since 2022 Rapid React - * @author Mason Lam, William Yuan + * @since 2024 Crescendo + * @author Audrey Zheng */ public class Camera { - private static ArrayList tags = new ArrayList(); + public PhotonCamera camera; - private boolean hasSeenTag; + private static ArrayList targets = new ArrayList(); + public static final LinkedList cameras = new LinkedList(); - public static int updateCounter = 0; + private PhotonPipelineResult result = new PhotonPipelineResult(); + + private static DoubleSupplier gyro; + private static AprilTagFieldLayout aprilTags; + private static BiConsumer odometry; + private static Supplier robotPose; + + private static double distanceThreshold = 5; + private static double ambiguityThreshold = 0.5; public static double validDist = 0.5; public static double overrideThreshold = 5; + public static int updateCounter = 0; - private final PhotonCamera camera; - private final Transform3d offset; - - private boolean isDisabled = false; - private PhotonPipelineResult lastResult; - private EstimatedRobotPose lastPose; + private boolean hasSeenTag; - private static AprilTagFieldLayout aprilTags; - // private static HashMap aprilTags; - private static PoseStrategy calc_strategy; - private static BiConsumer odometry; - private static Supplier robotPose; - private static double ambiguityThreshold = 0.3; + public Pose2d estimatedPose = new Pose2d(); + private static ArrayList tags = new ArrayList(); private static ArrayList ignoredTags = new ArrayList(); - public static final LinkedList cameras = new LinkedList(); + public boolean isDisabled = false; - private static final HashSet reportedErrors = new HashSet(); + public Transform3d offset; - private double distanceThreshold = 3.5; + public Pose2d gyroTest = new Pose2d(); public Camera(String name, double xOffset, double yOffset, double yawOffset, double pitchOffset, double rollOffset) { - camera = new PhotonCamera(name); - + Log.info("Camera", "camera constructer"); + this.offset = new Transform3d(xOffset, yOffset, 0, new Rotation3d(0.0, pitchOffset, 0.0) .rotateBy(new Rotation3d(rollOffset, 0, 0)) .rotateBy(new Rotation3d(0.0,0.0,yawOffset)) ); - if (aprilTags == null || calc_strategy == null || odometry == null || robotPose == null) { - throw new IllegalStateException("Camera not configured"); - } + camera = new PhotonCamera(name); cameras.add(this); - hasSeenTag = false; - } - public double getDistanceGround(){ - // double yOffset =this.offset.getY(); - // double pitchOffset = this.offset.getRotation().getY(); - // double pitchObject = Math.toRadians(target.getPitch()); - PhotonTrackedTarget target = lastResult.getBestTarget(); - double objectDistance = target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm(); - return objectDistance; - + initShuffleboard(); + hasSeenTag = false; } - - public static void configCameras(AprilTagFields aprilTagLayout, PoseStrategy calc_strategy, BiConsumer odometry, Supplier robotPose){ - Camera.aprilTags = aprilTagLayout.loadAprilTagLayoutField(); - Camera.calc_strategy = calc_strategy; + + public static void setResources(DoubleSupplier gyro, BiConsumer odometry, AprilTagFieldLayout aprilTags, Supplier robotPose) { + Camera.gyro = gyro; Camera.odometry = odometry; + Camera.aprilTags = aprilTags; Camera.robotPose = robotPose; } - public static void addIgnoredTags(int ...ignoredTags) { - for(final int tag : ignoredTags) { - Camera.ignoredTags.add(tag); - } - } - - public static void addTags(int ...tags) { - for(final int tag : tags) { - Camera.tags.add(tag); - } + public static void setThresholds(double distanceThreshold, double ambiguityThreshold) { + Camera.distanceThreshold = distanceThreshold; + Camera.ambiguityThreshold = ambiguityThreshold; } - public static boolean seesTag(){ - for (final Camera camera : cameras) { - if(camera.hasSeenTag) return true; - } - return false; - } + public void update() { + if (isDisabled) return; + hasSeenTag = false; + result = camera.getLatestResult(); - public static void updateAll(){ - for (final Camera camera : cameras) { - camera.update(); + if (!result.hasTargets()) { + targets = null; + Logger.recordOutput("Vision/" + camera.getName(), robotPose.get()); + return; } - } - public static void enableAll() { - for (final Camera camera : cameras) { - camera.enable(); - } - } + final Pose2d estPos = getPose(result); + gyroTest = getGyroPose(result); - public static void disableAll() { - for (final Camera camera : cameras) { - camera.disable(); + if(!isGoodEstimate(estPos)) { + updateCounter++; + if (updateCounter <= overrideThreshold) { + hasSeenTag = false; + return; + } } + else { + updateCounter = 0; + } + odometry.accept(gyroTest, result.getTimestampSeconds()); } - public static void setAmbiguityThreshold(double ambiguityThreshold) { - Camera.ambiguityThreshold = ambiguityThreshold; - } - - public void setCamDistanceThreshold(double distThreshold) { - distanceThreshold = distThreshold; - } - - public static void setAllDistanceThreshold(double distThreshold) { - for (final Camera camera : cameras) { - camera.setCamDistanceThreshold(distThreshold); - } + public boolean isGoodEstimate(Pose2d pose){ + return pose.getTranslation().getDistance(robotPose.get().getTranslation()) < validDist; } - private Optional getEstimatedPose(PhotonPipelineResult result) { + public Pose2d getPose(PhotonPipelineResult result) { + double lowestAmbiguityScore = 10; PhotonTrackedTarget lowestAmbiguityTarget = null; - double lowestAmbiguityScore = 10; + if (!result.hasTargets()) return new Pose2d(); for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - double dist = target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm(); - if (dist > distanceThreshold || targetPoseAmbiguity > ambiguityThreshold || ignoredTags.contains(Integer.valueOf(target.getFiducialId()))) continue; - // Make sure the target is a Fiducial target. - - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; + if (isValidTarget(target) && getPoseAmbiguity(target) < lowestAmbiguityScore && getPoseAmbiguity(target) != -1) { + lowestAmbiguityScore = getPoseAmbiguity(target); lowestAmbiguityTarget = target; hasSeenTag = tags.contains(target.getFiducialId()); } } - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = aprilTags.getTagPose(targetFiducialId); - // Optional targetPosition = Optional.of(aprilTags.get(targetFiducialId)); + if (lowestAmbiguityTarget == null) return new Pose2d(); - if (targetPosition.isEmpty()) { - if (NAR_Robot.logWithAdvantageKit) Logger.recordOutput("Vision/" + camera.getName() + "/Target", robotPose.get()); - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } + Optional targetPosition = aprilTags.getTagPose(lowestAmbiguityTarget.getFiducialId()); + estimatedPose = targetPosition.get().transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()).transformBy(offset.inverse()).toPose2d(); + return estimatedPose; + } - if (NAR_Robot.logWithAdvantageKit) Logger.recordOutput("Vision/" + camera.getName() + "/Target", targetPosition.get().toPose2d()); - - return Optional.of( - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(offset.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY)); + public Pose2d getFinalPos(PhotonTrackedTarget target) { + Pose3d test = new Pose3d(); + Pose2d test2 = new Pose2d(); + // test2 = test.transformBy(target.getBestCameraToTarget().inverse()).transformBy(offset.inverse()).getNorm(); + return null; } - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); - reportedErrors.add(fiducialId); - } + public Pose2d getGyroPose(PhotonPipelineResult result) { + Pose2d pose = getPose(result); + Rotation2d gyroAngle = Rotation2d.fromDegrees(MathUtil.angleModulus(getGyroAngle())); + Pose2d updatedPose = new Pose2d(pose.getX(), pose.getY(), gyroAngle); + return updatedPose; } - public void update(){ - if (isDisabled) return; - hasSeenTag = false; - lastResult = camera.getLatestResult(); - if (!lastResult.hasTargets() && NAR_Robot.logWithAdvantageKit) { - Logger.recordOutput("Vision/" + camera.getName() + "/Position", robotPose.get()); - return; + public static void addIgnoredTags(int ...ignoredTags) { + for(final int tag : ignoredTags) { + Camera.ignoredTags.add(tag); } + } - final Optional estimatedPose = getEstimatedPose(lastResult); + public boolean isValidTarget(PhotonTrackedTarget target) { + return !(getDistance(target) > distanceThreshold) && + getPoseAmbiguity(target) < ambiguityThreshold && + !ignoredTags.contains(Integer.valueOf(target.getFiducialId())); + } - if(!estimatedPose.isPresent() && NAR_Robot.logWithAdvantageKit) { - Logger.recordOutput("Vision/" + camera.getName() + "/Position", robotPose.get()); - } + public double getDistance(PhotonTrackedTarget target) { + return target.getBestCameraToTarget().getTranslation().toTranslation2d().getNorm(); + } - lastPose = estimatedPose.get(); - Pose2d estPose = lastPose.estimatedPose.toPose2d(); + public double getPoseAmbiguity(PhotonTrackedTarget target) { + return target.getPoseAmbiguity(); + } - if(!isGoodEstimate(estPose)) { - updateCounter++; - if (updateCounter <= overrideThreshold) { - hasSeenTag = false; - return; - } - } - else { - updateCounter = 0; + public static void updateAll() { + for (final Camera camera : cameras) { + camera.update(); } - - if (NAR_Robot.logWithAdvantageKit) Logger.recordOutput("Vision/" + camera.getName() + "/Position", estPose); - - odometry.accept(estPose, lastPose.timestampSeconds); } - public boolean isGoodEstimate(Pose2d pose){ - return pose.getTranslation().getDistance(robotPose.get().getTranslation()) < validDist; + public static boolean seesTag(){ + for (final Camera camera : cameras) { + if(camera.hasSeenTag) return true; + } + return false; } - - public void initShuffleboard() { - NAR_Shuffleboard.addData(camera.getName(), "Estimated Pose", ()-> lastPose.estimatedPose.toPose2d().toString(), 0, 0, 3, 1); + + public void enable() { + isDisabled = false; } - public PhotonPipelineResult getLatestResult() { - return lastResult; + public void disable(){ + isDisabled = true; } - public Transform3d getOffset() { - return offset; + public static void enableAll() { + for (final Camera camera : cameras) { + camera.enable(); + } } - public void disable(){ - isDisabled = true; + public static void disableAll() { + for (final Camera camera : cameras) { + camera.disable(); + } } - public void enable() { - isDisabled = false; + public double getGyroAngle() { + return gyro.getAsDouble(); } + public void initShuffleboard() { + NAR_Shuffleboard.addData(camera.getName(), "Estimated Pose", () -> estimatedPose.toString(), 0, 0, 3, 1); + NAR_Shuffleboard.addData(camera.getName(), "Is Disabled", () -> isDisabled, 4, 0, 1, 1); + NAR_Shuffleboard.addData(camera.getName(), "Has target", () -> result.hasTargets(), 4, 0, 1, 1); + NAR_Shuffleboard.addData(camera.getName(), "gyro", () -> gyroTest.toString(), 5, 0, 1, 1); + } } \ No newline at end of file diff --git a/src/main/java/common/hardware/camera/NAR_Camera.java b/src/main/java/common/hardware/camera/NAR_Camera.java index 942570c..4a8335f 100644 --- a/src/main/java/common/hardware/camera/NAR_Camera.java +++ b/src/main/java/common/hardware/camera/NAR_Camera.java @@ -1,23 +1,31 @@ // package common.hardware.camera; // import java.util.LinkedList; +// import java.util.ArrayList; // import java.util.HashMap; // import java.util.List; +// import java.util.Optional; // import java.util.function.BiConsumer; // import java.util.function.DoubleSupplier; // import java.util.function.Supplier; +// import org.photonvision.EstimatedRobotPose; // import org.photonvision.PhotonCamera; +// import org.photonvision.PhotonPoseEstimator.PoseStrategy; // import org.photonvision.targeting.PhotonPipelineResult; // import org.photonvision.targeting.PhotonTrackedTarget; +// import common.core.misc.NAR_Robot; +// import edu.wpi.first.apriltag.AprilTagFieldLayout; // 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.Transform2d; // import edu.wpi.first.math.geometry.Transform3d; // import edu.wpi.first.math.geometry.Translation2d; // import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj.DriverStation; // import org.littletonrobotics.junction.Logger; @@ -33,14 +41,21 @@ // public final Camera camera; +// private static ArrayList ignoredTags = new ArrayList(); + // private PhotonPipelineResult result; // private List targets; // private PhotonTrackedTarget bestTarget; +// private boolean hasSeenTag; + // private static DoubleSupplier gyro; // private static BiConsumer odometry; -// private static HashMap aprilTags; +// // private static HashMap aprilTags; +// private static AprilTagFieldLayout aprilTags; + +// private static Supplier robotPose; // private static double angleThreshold = 30; // private static double distanceThreshold = 5; @@ -71,7 +86,7 @@ // * @param aprilTags Sets the AprilTag positions on the field. // * @param poseSupplier Supplies the robot's current pose. // */ -// public static void setResources(DoubleSupplier gyro, BiConsumer odometry, HashMap aprilTags, Supplier poseSupplier) { +// public static void setResources(DoubleSupplier gyro, BiConsumer odometry, AprilTagFieldLayout aprilTags, Supplier poseSupplier) { // NAR_Camera.gyro = gyro; // NAR_Camera.odometry = odometry; // NAR_Camera.aprilTags = aprilTags; @@ -93,26 +108,12 @@ // NAR_Camera.multipleTargets = multipleTargets; // } -// /** -// * Allows the camera to update robot odometry. -// */ -// public void enable() { -// camera.enabled = true; -// } - -// /** -// * Stops the camera from updating robot odometry. -// */ -// public void disable() { -// camera.enabled = false; -// } - // /** // * Gets the latest targets. // */ // public void update() { // //Don't update if camera is disabled -// if (!camera.enabled) return; +// if (camera.isDisabled) return; // //returns the most recent camera frame // result = this.getLatestResult(); @@ -331,7 +332,7 @@ // relTargetAngle = Rotation2d.fromDegrees(robotAngle); // } -// // vector relative to camera coordinate system +// // vector relative to camera coordinate system. // final Translation2d vector = getRelTarget(target).getTranslation().rotateBy(relTargetAngle); // return new Transform2d(vector, relTargetAngle); From eb2021f7e431b92acee4fd3b6d5b8e88113102a6 Mon Sep 17 00:00:00 2001 From: Audrey Date: Mon, 30 Sep 2024 16:50:41 -0700 Subject: [PATCH 2/2] added gyro stabilization, fixed shuffleboard, cleanup --- .../java/common/hardware/camera/Camera.java | 155 ++++--- .../common/hardware/camera/NAR_Camera.java | 379 ------------------ 2 files changed, 108 insertions(+), 426 deletions(-) delete mode 100644 src/main/java/common/hardware/camera/NAR_Camera.java diff --git a/src/main/java/common/hardware/camera/Camera.java b/src/main/java/common/hardware/camera/Camera.java index cff7c13..32d4f70 100644 --- a/src/main/java/common/hardware/camera/Camera.java +++ b/src/main/java/common/hardware/camera/Camera.java @@ -12,13 +12,12 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import common.core.misc.NAR_Robot; import common.utility.Log; import common.utility.shuffleboard.NAR_Shuffleboard; import edu.wpi.first.apriltag.AprilTagFieldLayout; -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.Transform3d; @@ -31,37 +30,32 @@ public class Camera { public PhotonCamera camera; - private static ArrayList targets = new ArrayList(); public static final LinkedList cameras = new LinkedList(); - - private PhotonPipelineResult result = new PhotonPipelineResult(); + private PhotonPipelineResult result = new PhotonPipelineResult(); private static DoubleSupplier gyro; private static AprilTagFieldLayout aprilTags; private static BiConsumer odometry; private static Supplier robotPose; - + private static double distanceThreshold = 5; private static double ambiguityThreshold = 0.5; - public static double validDist = 0.5; public static double overrideThreshold = 5; public static int updateCounter = 0; - + private boolean hasSeenTag; - public Pose2d estimatedPose = new Pose2d(); - private static ArrayList tags = new ArrayList(); private static ArrayList ignoredTags = new ArrayList(); - public boolean isDisabled = false; - public Transform3d offset; + public double distance; - public Pose2d gyroTest = new Pose2d(); - + /** + * Creates a Camera object + */ public Camera(String name, double xOffset, double yOffset, double yawOffset, double pitchOffset, double rollOffset) { Log.info("Camera", "camera constructer"); @@ -78,7 +72,15 @@ public Camera(String name, double xOffset, double yOffset, double yawOffset, dou initShuffleboard(); hasSeenTag = false; } - + + /** + * Sets the necessary resources for cameras to function. + * + * @param gyro Feeds the angle of the robot. + * @param odometry Feeds the robot odometry object for vision estimates to update. + * @param aprilTags Sets the AprilTag positions on the field. + * @param robotPose Supplies the robot's current pose. + */ public static void setResources(DoubleSupplier gyro, BiConsumer odometry, AprilTagFieldLayout aprilTags, Supplier robotPose) { Camera.gyro = gyro; Camera.odometry = odometry; @@ -86,11 +88,19 @@ public static void setResources(DoubleSupplier gyro, BiConsumer Camera.robotPose = robotPose; } + /** + * Sets the necessary thresholds for the cameras to use + * @param distanceThreshold Maximum distance from tag to accept + * @param ambiguityThreshold Maximum tag ambiguity to accept + */ public static void setThresholds(double distanceThreshold, double ambiguityThreshold) { Camera.distanceThreshold = distanceThreshold; Camera.ambiguityThreshold = ambiguityThreshold; } + /** + * Gets the latest camera updates + */ public void update() { if (isDisabled) return; hasSeenTag = false; @@ -98,12 +108,15 @@ public void update() { if (!result.hasTargets()) { targets = null; - Logger.recordOutput("Vision/" + camera.getName(), robotPose.get()); + if (NAR_Robot.logWithAdvantageKit) Logger.recordOutput("Vision/" + camera.getName(), robotPose.get()); return; } - final Pose2d estPos = getPose(result); - gyroTest = getGyroPose(result); + final Pose2d estPos = getPose(result).get(); + + /* + * Checks if the the robot has a good estimate + */ if(!isGoodEstimate(estPos)) { updateCounter++; @@ -114,55 +127,103 @@ public void update() { } else { updateCounter = 0; - } - odometry.accept(gyroTest, result.getTimestampSeconds()); + } + + odometry.accept(estPos, result.getTimestampSeconds()); } - public boolean isGoodEstimate(Pose2d pose){ - return pose.getTranslation().getDistance(robotPose.get().getTranslation()) < validDist; - } - public Pose2d getPose(PhotonPipelineResult result) { + /** + * Gets the latest camera updates + * @param result Latest result from the camera + * @return The estimated robot pose + */ + public Optional getPose(PhotonPipelineResult result) { double lowestAmbiguityScore = 10; PhotonTrackedTarget lowestAmbiguityTarget = null; - if (!result.hasTargets()) return new Pose2d(); + if (!result.hasTargets()) return Optional.empty(); + /* + * Find the target with the lowest ambiguity score + */ for (PhotonTrackedTarget target : result.targets) { if (isValidTarget(target) && getPoseAmbiguity(target) < lowestAmbiguityScore && getPoseAmbiguity(target) != -1) { lowestAmbiguityScore = getPoseAmbiguity(target); lowestAmbiguityTarget = target; - hasSeenTag = tags.contains(target.getFiducialId()); + hasSeenTag = tags.contains(target.getFiducialId()); } } - if (lowestAmbiguityTarget == null) return new Pose2d(); + if (lowestAmbiguityTarget == null) return Optional.empty(); + distance = getDistance(lowestAmbiguityTarget); + + /* + * Finds the pose of the lowest ambiguity target and uses the gyro to determine the estimated pose + */ Optional targetPosition = aprilTags.getTagPose(lowestAmbiguityTarget.getFiducialId()); - estimatedPose = targetPosition.get().transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()).transformBy(offset.inverse()).toPose2d(); - return estimatedPose; + estimatedPose = getGyroStablilization(targetPosition, lowestAmbiguityTarget); + + return Optional.of(estimatedPose); } - public Pose2d getFinalPos(PhotonTrackedTarget target) { - Pose3d test = new Pose3d(); - Pose2d test2 = new Pose2d(); - // test2 = test.transformBy(target.getBestCameraToTarget().inverse()).transformBy(offset.inverse()).getNorm(); - return null; + /** + * Uses the gyro angle and target position to determine robot pose + * @param targetPosition The Pose3d of the target + * @param bestTarget Lowest ambiguity target + * @return The estimated Pose2d of the robot + */ + public Pose2d getGyroStablilization(Optional targetPosition, PhotonTrackedTarget bestTarget) { + double gyroAngle = getGyroAngle() * Math.PI / 180; + + Transform3d targetToCamera = bestTarget.getBestCameraToTarget().inverse(); + Rotation3d gyroRotation3d = new Rotation3d(0, 0, gyroAngle); + Pose3d target = targetPosition.get(); + + /* + * Determines the position of the camera using the position of the target and the gyro rotation + */ + Pose3d cameraPose = new Pose3d(target.getTranslation().plus( + targetToCamera.getTranslation().rotateBy(target.getRotation())), + gyroRotation3d.plus(target.getRotation()) + ); + + /* + * Determines the position of the robot using the position of the camera and its offset + */ + Pose3d robotPose = new Pose3d(cameraPose.getTranslation().plus( + offset.inverse().getTranslation().rotateBy(cameraPose.getRotation())), + offset.inverse().getRotation().plus(cameraPose.getRotation()) + ); + + return robotPose.toPose2d(); } - public Pose2d getGyroPose(PhotonPipelineResult result) { - Pose2d pose = getPose(result); - Rotation2d gyroAngle = Rotation2d.fromDegrees(MathUtil.angleModulus(getGyroAngle())); - Pose2d updatedPose = new Pose2d(pose.getX(), pose.getY(), gyroAngle); - return updatedPose; + /** + * Returns if an estimate is within a valid distance from the robot pose + * @param pose Estimated pose + * @return If the estimated pose is valid + */ + public boolean isGoodEstimate(Pose2d pose){ + return pose.getTranslation().getDistance(robotPose.get().getTranslation()) < validDist; } + /** + * Blacklists tags from use + * @param ignoredTags IDs of ignored tags + */ public static void addIgnoredTags(int ...ignoredTags) { for(final int tag : ignoredTags) { Camera.ignoredTags.add(tag); } } + /** + * Returns true if the target is within the distance and ambiguity thresholds and is not blacklisted + * @param target + * @return If the target is valid + */ public boolean isValidTarget(PhotonTrackedTarget target) { return !(getDistance(target) > distanceThreshold) && getPoseAmbiguity(target) < ambiguityThreshold && @@ -177,6 +238,10 @@ public double getPoseAmbiguity(PhotonTrackedTarget target) { return target.getPoseAmbiguity(); } + public double getGyroAngle() { + return gyro.getAsDouble(); + } + public static void updateAll() { for (final Camera camera : cameras) { camera.update(); @@ -210,14 +275,10 @@ public static void disableAll() { } } - public double getGyroAngle() { - return gyro.getAsDouble(); - } - public void initShuffleboard() { - NAR_Shuffleboard.addData(camera.getName(), "Estimated Pose", () -> estimatedPose.toString(), 0, 0, 3, 1); - NAR_Shuffleboard.addData(camera.getName(), "Is Disabled", () -> isDisabled, 4, 0, 1, 1); - NAR_Shuffleboard.addData(camera.getName(), "Has target", () -> result.hasTargets(), 4, 0, 1, 1); - NAR_Shuffleboard.addData(camera.getName(), "gyro", () -> gyroTest.toString(), 5, 0, 1, 1); + NAR_Shuffleboard.addData(camera.getName(), "Estimated Pose", () -> estimatedPose.toString(), 0, 0, 4, 1); + NAR_Shuffleboard.addData(camera.getName(), "Distance", () -> distance, 0, 1, 2, 1); + NAR_Shuffleboard.addData(camera.getName(), "Is Disabled", () -> isDisabled, 2, 1, 1, 1); + NAR_Shuffleboard.addData(camera.getName(), "Has target", () -> result.hasTargets(), 3, 2, 1, 1); } } \ No newline at end of file diff --git a/src/main/java/common/hardware/camera/NAR_Camera.java b/src/main/java/common/hardware/camera/NAR_Camera.java deleted file mode 100644 index 4a8335f..0000000 --- a/src/main/java/common/hardware/camera/NAR_Camera.java +++ /dev/null @@ -1,379 +0,0 @@ -// package common.hardware.camera; - -// import java.util.LinkedList; -// import java.util.ArrayList; -// import java.util.HashMap; -// import java.util.List; -// import java.util.Optional; -// import java.util.function.BiConsumer; -// import java.util.function.DoubleSupplier; -// import java.util.function.Supplier; - -// import org.photonvision.EstimatedRobotPose; -// import org.photonvision.PhotonCamera; -// import org.photonvision.PhotonPoseEstimator.PoseStrategy; -// import org.photonvision.targeting.PhotonPipelineResult; -// import org.photonvision.targeting.PhotonTrackedTarget; - -// import common.core.misc.NAR_Robot; -// import edu.wpi.first.apriltag.AprilTagFieldLayout; -// 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.Transform2d; -// import edu.wpi.first.math.geometry.Transform3d; -// import edu.wpi.first.math.geometry.Translation2d; -// import edu.wpi.first.math.util.Units; -// import edu.wpi.first.wpilibj.DriverStation; - -// import org.littletonrobotics.junction.Logger; - -// /** -// * Team 3128's streamlined {@link PhotonCamera} class that provides additional functionality and ease of use. -// *

Geometry: https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/index.html. -// *

Photonvision: https://docs.photonvision.org/en/latest/. -// * -// * @since 2023 Charged Up -// * @author Mason Lam, William Yuan, Lucas Han, Audrey Zheng -// */ -// public class NAR_Camera extends PhotonCamera { - -// public final Camera camera; - -// private static ArrayList ignoredTags = new ArrayList(); - -// private PhotonPipelineResult result; - -// private List targets; -// private PhotonTrackedTarget bestTarget; - -// private boolean hasSeenTag; - -// private static DoubleSupplier gyro; -// private static BiConsumer odometry; -// // private static HashMap aprilTags; -// private static AprilTagFieldLayout aprilTags; - -// private static Supplier robotPose; - -// private static double angleThreshold = 30; -// private static double distanceThreshold = 5; -// private static double ambiguityThreshold = 0.5; -// private static boolean multipleTargets = false; - -// private static Supplier poseSupplier; - -// private final double FIELD_X_LENGTH = Units.inchesToMeters(648); -// private final double FIELD_Y_LENGTH = Units.inchesToMeters(324); - -// /** -// * Creates a NAR_Camera object. -// * -// * @param camera Specs of the camera. -// */ -// public NAR_Camera(Camera camera) { -// super(camera.name); -// this.camera = camera; -// setVersionCheckEnabled(false); -// } - -// /** -// * Sets the necessary resources for cameras to function. -// * -// * @param gyro Feeds the angle of the robot. -// * @param odometry Feeds the robot odometry object for vision estimates to update. -// * @param aprilTags Sets the AprilTag positions on the field. -// * @param poseSupplier Supplies the robot's current pose. -// */ -// public static void setResources(DoubleSupplier gyro, BiConsumer odometry, AprilTagFieldLayout aprilTags, Supplier poseSupplier) { -// NAR_Camera.gyro = gyro; -// NAR_Camera.odometry = odometry; -// NAR_Camera.aprilTags = aprilTags; -// NAR_Camera.poseSupplier = poseSupplier; -// } - -// /** -// * Sets the thresholds for camera pose estimates. -// *

Defaults: 30 degrees, 5 meters, 0.5, false. -// * @param angleThreshold Angle offset to stop using estimates. -// * @param distanceThreshold Distance to stop accepting updates. -// * @param ambiguityThreshold Pose ambiguity from 0.0 to 1.0 to reject estimates. -// * @param multipleTargets Update position using multiple tags. -// */ -// public static void setThresholds(double angleThreshold, double distanceThreshold, double ambiguityThreshold, boolean multipleTargets) { -// NAR_Camera.angleThreshold = angleThreshold; -// NAR_Camera.distanceThreshold = distanceThreshold; -// NAR_Camera.ambiguityThreshold = ambiguityThreshold; -// NAR_Camera.multipleTargets = multipleTargets; -// } - -// /** -// * Gets the latest targets. -// */ -// public void update() { -// //Don't update if camera is disabled -// if (camera.isDisabled) return; - -// //returns the most recent camera frame -// result = this.getLatestResult(); - -// // if camera sees no target, set values to null and return -// if (!result.hasTargets()) { -// targets = null; -// bestTarget = null; -// Logger.recordOutput("Vision/" + camera.name, poseSupplier.get()); -// return; -// } - -// targets = result.getTargets(); -// bestTarget = result.getBestTarget(); - -// updatePose(); -// } - -// /** -// * Sends the camera's pose estimate. -// */ -// private void updatePose() { -// final LinkedList possiblePoses = new LinkedList(); -// final LinkedList possibleTargets = new LinkedList(); - -// // add valid targets to possiblePoses -// for (final PhotonTrackedTarget curTarget : targets) { -// final Pose2d estimate = getPos(curTarget); - -// // if target is tolerable, add to possiblePoses -// if (!estimate.equals(new Pose2d()) && isValidTarget(curTarget)) { - -// possiblePoses.add(estimate); -// possibleTargets.add(curTarget); - -// } - -// // if camera has multiple targets disabled, break after adding first valid target -// if (!multipleTargets) break; -// } - -// if(possiblePoses.isEmpty()) Logger.recordOutput("Vision/" + camera.name, poseSupplier.get()); - -// // updates robot with all acceptable poses from possiblePoses -// for (final Pose2d curPos : possiblePoses) { -// if (translationOutOfBounds(curPos.getTranslation())) return; -// odometry.accept(curPos, result.getTimestampSeconds()); - -// int index = possiblePoses.indexOf(curPos); -// Logger.recordOutput("Vision/" + camera.name, aprilTags.get(targetId(possibleTargets.get(index)))); -// } -// } - -// /** -// * Returns whether or not to accept the pose estimate. -// * @param target An AprilTag. -// * @return If the AprilTag is within set constraints. -// */ -// private boolean isValidTarget(PhotonTrackedTarget target) { -// final Rotation2d relativeAngle = getAccTarget(target).getRotation(); -// return targetAmbiguity(target) < ambiguityThreshold -// && !(getDistance(target) > distanceThreshold || Math.abs(relativeAngle.getDegrees()) < 180 - angleThreshold); -// } - -// /** -// * Returns whether a translation is on the field. -// * @param translation A calculated translation. -// * @return If the Translation2d is within the bounds of the field. -// */ -// private boolean translationOutOfBounds(Translation2d translation) { -// return translation.getX() > FIELD_X_LENGTH || translation.getX() < 0 || translation.getY() > FIELD_Y_LENGTH -// || translation.getY() < 0; -// } - -// /** -// * Returns whether or not the camera sees a target. -// * @return If camera sees any targets. -// */ -// public boolean hasTarget() { -// return targets != null; -// } - -// /** -// * Returns the distance from the best target to the camera. -// * @return The distance in meters. -// */ -// public double getDistance() { -// return getDistance(bestTarget); -// } - -// /** -// * Returns the distance from an AprilTag to the camera. -// * @param target An AprilTag. -// * @return The distance in meters. -// */ -// private double getDistance(PhotonTrackedTarget target) { -// if (!hasTarget()) return -1; - -// final Transform2d transform = getRelTarget(target); -// return Math.hypot(transform.getX(), transform.getY()); -// } - -// /** -// * Returns the ID of the best target. -// * @return The target ID as an integer. -// */ -// public int targetId() { -// return targetId(bestTarget); -// } - -// /** -// * Returns the ID of an AprilTag. -// * @param target An AprilTag -// * @return The target ID as an integer. -// */ -// private int targetId(PhotonTrackedTarget target) { -// return hasTarget() ? target.getFiducialId() : 0; -// } -// /** -// * Returns the ambiguity of the best target with lower being more accurate. -// * @return A value from 0.0 to 1.0 representing the accuracy of the target. -// */ -// public double targetAmbiguity() { -// return targetAmbiguity(bestTarget); -// } - -// /** -// * Returns the ambiguity of an AprilTag with lower being more accurate. -// * @param target An AprilTag. -// * @return A value from 0.0 to 1.0 representing the accuracy of the target. -// */ -// private double targetAmbiguity(PhotonTrackedTarget target) { -// return hasTarget() ? target.getPoseAmbiguity() : 0; -// } - -// /** -// * Returns the best target to camera vector as a Transform3d. -// * @return Transform3d with coordinate system relative to camera. -// */ -// public Transform3d getTarget3d() { -// return getTarget3d(bestTarget); -// } - -// /** -// * Returns an AprilTag to camera vector as a Transform3d. -// * @param target An AprilTag. -// * @return Transform3d with coordinate system relative to camera. -// */ -// private Transform3d getTarget3d(PhotonTrackedTarget target) { -// return hasTarget() ? target.getBestCameraToTarget() : new Transform3d(); -// } - -// /** -// * Returns the best target to camera vector as a Transform2d. -// *

-// * Factors in camera pitch offset. -// * @return Transform2d with coordinate system relative to camera. -// */ -// public Transform2d getRelTarget() { -// return getRelTarget(bestTarget); -// } - -// /** -// * Returns an AprilTag to camera vector as a Transform2d. -// *

-// * Factors in camera pitch offset. -// * @param target An AprilTag. -// * @return Transform2d with coordinate system relative to camera. -// */ -// private Transform2d getRelTarget(PhotonTrackedTarget target) { -// if (!hasTarget()) return new Transform2d(); - -// final Transform3d transform = getTarget3d(target); - -// Translation2d translation2d = transform.getTranslation().toTranslation2d(); - -// //rotates the x value based on camera pitch offset -// translation2d = new Translation2d(translation2d.getX() * Math.cos(Units.degreesToRadians(camera.cameraPitch)), translation2d.getY()); - -// final Rotation2d rotation2d = transform.getRotation().toRotation2d().unaryMinus(); - -// return new Transform2d(translation2d, rotation2d); -// } - -// /** -// * Returns the best target to camera vector as a Transform2d. -// * @return Transform2d with coordinate system relative to target. -// */ -// public Transform2d getAccTarget() { -// return getAccTarget(bestTarget); -// } - -// /** -// * Returns an AprilTag to camera vector as a Transform2d. -// * @param target An AprilTag. -// * @return Transform2d with coordinate system relative to target. -// */ -// private Transform2d getAccTarget(PhotonTrackedTarget target) { -// // if no valid target, return empty Transform2d -// if (!hasTarget() || !aprilTags.containsKey(targetId(target))) return new Transform2d(); - -// // angle of the AprilTag relative to the camera -// final Rotation2d relTargetAngle; - -// // use gyro if resource is supplied -// if (gyro == null) { -// relTargetAngle = getRelTarget(target).getRotation(); -// } -// else { -// // angle of the AprilTag relative to the field -// final double fieldTargetAngle = aprilTags.get(targetId(target)).getRotation().getDegrees(); - -// // angle of the robot relative to the AprilTag using gyro measurement -// final double robotAngle = MathUtil.inputModulus(gyro.getAsDouble() + fieldTargetAngle + camera.offset.getRotation().getDegrees() + 180,-180,180); - -// relTargetAngle = Rotation2d.fromDegrees(robotAngle); -// } - -// // vector relative to camera coordinate system. -// final Translation2d vector = getRelTarget(target).getTranslation().rotateBy(relTargetAngle); - -// return new Transform2d(vector, relTargetAngle); -// } - -// /** -// * Returns position of the robot on the field based on the best target. -// * @return Pose estimate. -// */ -// public Pose2d getPos() { -// return getPos(bestTarget); -// } - -// /** -// * Returns position of the robot on the field based on an AprilTag. -// * @param target An AprilTag. -// * @return Pose estimate. -// */ -// private Pose2d getPos(PhotonTrackedTarget target) { -// final Pose2d AprilTag = aprilTags.get(targetId(target)); - -// // if no valid target, return empty Pose2d -// if (!hasTarget() || !aprilTags.containsKey(targetId(target)) || target == null) return new Pose2d(); - -// // vector from target to camera rel to target coordinate system -// final Transform2d transform = getAccTarget(target); - -// // vector from field origin to camera rel to field coordinate system -// final Translation2d coord = AprilTag.getTranslation().plus(transform.getTranslation().rotateBy(AprilTag.getRotation())); - -// // angle of the camera rel to field coordinate system -// final Rotation2d angle = AprilTag.getRotation().plus(transform.getRotation()); - -// // turn field origin to camera vector to field origin to robot vector -// return new Pose2d(coord, angle).transformBy(camera.offset); -// } - -// /** -// * @return The name of the camera. -// */ -// public String getName() { -// return camera.name; -// } -// } \ No newline at end of file