From 61f16245aadbb1d260d2ef88b5fee661402c5730 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <114535782+mimizh2418@users.noreply.github.com> Date: Fri, 2 Feb 2024 16:58:03 -0800 Subject: [PATCH] AprilTag pose estimation (#18) * feat: use SwerveDrivePoseEstimator for odometry tracking * stop using scuffed AdvantageKit Twist2d way * feat: AprilTag vision IO and implementation for Limelight * feat: AprilTagVision subsystem * feat: fuse vision poses with drivetrain poses * fix: null vision poses no longer causing issues * fix: actually fix null vision poses * feat: limelight positioning adjusts based on elevator height * fix: properly update rear camera tag poses * feat: block vision updates with moving elevator * fix: correctly pass in suppliers * im dumb lol * fix: use optionals for getting vision pose * feat: change how vision pose accepter is used * we can now change the code to filter out input from a single camera if the elevator is moving, instead of all cameras * feat: untested vision sim io with photonvision sim support * fix: use vision pose in sim * fix: sim values as constants * refactor: remove unnecessary IO input * refactor: april tag layout declared in sim io instead of constants * fix: correctly check robot rotational velocity --- .../org/team1540/robot2024/Constants.java | 25 + .../team1540/robot2024/RobotContainer.java | 26 + .../subsystems/drive/Drivetrain.java | 76 +- .../subsystems/vision/AprilTagVision.java | 119 +++ .../subsystems/vision/AprilTagVisionIO.java | 24 + .../vision/AprilTagVisionIOLimelight.java | 61 ++ .../vision/AprilTagVisionIOSim.java | 98 +++ .../util/vision/LimelightHelpers.java | 769 ++++++++++++++++++ .../util/vision/TimestampedVisionPose.java | 44 + .../util/vision/VisionPoseAcceptor.java | 41 + vendordeps/photonlib-json-1.0.json | 57 ++ 11 files changed, 1312 insertions(+), 28 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java create mode 100644 src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java create mode 100644 src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java create mode 100644 src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java create mode 100644 vendordeps/photonlib-json-1.0.json diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index d93ad667..951deafa 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,6 +1,8 @@ package org.team1540.robot2024; +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.util.Units; /** @@ -62,6 +64,29 @@ public static class Drivetrain { public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; } + public static class Vision { + public static final String FRONT_CAMERA_NAME = "limelight-front"; + public static final String REAR_CAMERA_NAME = "limelight-rear"; + + // TODO: measure these offsets + public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d()); + public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0, 0, 0.5, new Rotation3d(0, 0, Math.PI)); + + // TODO: find these values + public static final double MAX_VISION_DELAY_SECS = 0.08; + public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0; + public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0; + public static final double MIN_ACCEPTED_NUM_TAGS = 1; + public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0; + public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05; + + public static final int SIM_RES_WIDTH = 1280; + public static final int SIM_RES_HEIGHT = 960; + public static final Rotation2d SIM_DIAGONAL_FOV = Rotation2d.fromDegrees(100); + public static final double SIM_FPS = 14.5; + public static final double SIM_AVG_LATENCY_MS = 67.0; + } + public static class Elevator { public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(21.0); } diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 4e9674a9..6f6706d5 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -14,9 +14,14 @@ import org.team1540.robot2024.commands.SwerveDriveCommand; import org.team1540.robot2024.subsystems.drive.*; import org.team1540.robot2024.subsystems.shooter.*; +import org.team1540.robot2024.subsystems.vision.AprilTagVision; +import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO; +import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight; +import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim; import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.swerve.SwerveFactory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.team1540.robot2024.util.vision.VisionPoseAcceptor; import static org.team1540.robot2024.Constants.SwerveConfig; @@ -30,6 +35,7 @@ public class RobotContainer { // Subsystems public final Drivetrain drivetrain; public final Shooter shooter; + public final AprilTagVision aprilTagVision; // Controller public final CommandXboxController driver = new CommandXboxController(0); @@ -59,6 +65,12 @@ public RobotContainer() { new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher), new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher)); shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX()); + aprilTagVision = new AprilTagVision( + new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE), + new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE), + drivetrain::addVisionMeasurement, + () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE + new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE break; case SIM: @@ -71,6 +83,13 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim()); shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim()); + aprilTagVision = + new AprilTagVision( + new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose), + new AprilTagVisionIOSim(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE, drivetrain::getPose), + ignored -> {}, + () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE + new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE break; default: @@ -83,6 +102,13 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {}); + aprilTagVision = + new AprilTagVision( + new AprilTagVisionIO() {}, + new AprilTagVisionIO() {}, + (ignored) -> {}, + () -> 0.0, + new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); break; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java index 7615d5b4..91333edc 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java +++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java @@ -5,10 +5,11 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -18,6 +19,7 @@ import org.team1540.robot2024.util.LocalADStarAK; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.util.vision.TimestampedVisionPose; import static org.team1540.robot2024.Constants.Drivetrain.*; @@ -27,10 +29,11 @@ public class Drivetrain extends SubsystemBase { private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Pose2d pose = new Pose2d(); - private Rotation2d lastGyroRotation = new Rotation2d(); + private Rotation2d rawGyroRotation = new Rotation2d(); private boolean forceModuleAngleChange = false; + private final SwerveDrivePoseEstimator poseEstimator; + public Drivetrain( GyroIO gyroIO, ModuleIO flModuleIO, @@ -43,6 +46,15 @@ public Drivetrain( modules[2] = new Module(blModuleIO, 2); modules[3] = new Module(brModuleIO, 3); + poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + rawGyroRotation, + getModulePositions(), + new Pose2d(), + // TODO: tune std devs (scale vision by distance?) + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags + // Configure AutoBuilder for PathPlanner AutoBuilder.configureHolonomic( this::getPose, @@ -60,6 +72,7 @@ public Drivetrain( (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); } + @Override public void periodic() { gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drivetrain/Gyro", gyroInputs); @@ -67,37 +80,27 @@ public void periodic() { module.periodic(); } - // Stop moving when disabled - if (DriverStation.isDisabled()) { - for (Module module : modules) { - module.stop(); - } - } - // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { + // Stop moving when disabled + for (Module module : modules) module.stop(); + + // Log empty setpoint states when disabled Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{}); } - // Update odometry + // Calculate module deltas SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { wheelDeltas[i] = modules[i].getPositionDelta(); } - // The twist represents the motion of the robot since the last - // loop cycle in x, y, and theta based only on the modules, - // without the gyro. The gyro is always disconnected in simulation. - Twist2d twist = kinematics.toTwist2d(wheelDeltas); - if (gyroInputs.connected) { - // If the gyro is connected, replace the theta component of the twist - // with the change in angle since the last loop cycle. - twist = - new Twist2d( - twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians()); - lastGyroRotation = gyroInputs.yawPosition; - } - // Apply the twist (change since last loop cycle) to the current pose - pose = pose.exp(twist); + // Use gyro rotation if gyro is connected, otherwise use module deltas to calculate rotation delta + rawGyroRotation = + gyroInputs.connected ? + gyroInputs.yawPosition + : rawGyroRotation.plus(Rotation2d.fromRadians(kinematics.toTwist2d(wheelDeltas).dtheta)); + // Update odometry + poseEstimator.update(rawGyroRotation, getModulePositions()); } /** @@ -164,6 +167,10 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } + /** * Returns the module states (turn angles and drive velocities) for all the modules. */ @@ -181,21 +188,34 @@ private SwerveModuleState[] getModuleStates() { */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { - return pose; + return poseEstimator.getEstimatedPosition(); } /** * Returns the current odometry rotation. */ public Rotation2d getRotation() { - return pose.getRotation(); + return getPose().getRotation(); } /** * Resets the current odometry pose. */ public void setPose(Pose2d pose) { - this.pose = pose; + poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + } + + public void addVisionMeasurement(TimestampedVisionPose visionPose) { + poseEstimator.addVisionMeasurement(visionPose.poseMeters(), visionPose.timestampSecs()); + } + + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[]{ + modules[0].getPosition(), + modules[1].getPosition(), + modules[2].getPosition(), + modules[3].getPosition(), + }; } /** diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java new file mode 100644 index 00000000..fe748ccb --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java @@ -0,0 +1,119 @@ +package org.team1540.robot2024.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.team1540.robot2024.util.vision.TimestampedVisionPose; +import org.team1540.robot2024.util.vision.VisionPoseAcceptor; + +import java.util.Arrays; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.function.Supplier; + +import static org.team1540.robot2024.Constants.Vision.*; + +public class AprilTagVision extends SubsystemBase { + private final AprilTagVisionIO frontCameraIO; + private final AprilTagVisionIOInputsAutoLogged frontCameraInputs = new AprilTagVisionIOInputsAutoLogged(); + + private final AprilTagVisionIO rearCameraIO; + private final AprilTagVisionIOInputsAutoLogged rearCameraInputs = new AprilTagVisionIOInputsAutoLogged(); + + private final Consumer visionPoseConsumer; + private final Supplier elevatorHeightSupplierMeters; + private final VisionPoseAcceptor poseAcceptor; + + private TimestampedVisionPose frontPose = + new TimestampedVisionPose(-1, new Pose2d(), new int[0], new Pose2d[0], true, false); + private TimestampedVisionPose rearPose = + new TimestampedVisionPose(-1, new Pose2d(), new int[0], new Pose2d[0], false, true); + + public AprilTagVision( + AprilTagVisionIO frontCameraIO, + AprilTagVisionIO rearCameraIO, + Consumer visionPoseConsumer, + Supplier elevatorHeightSupplierMeters, + VisionPoseAcceptor poseAcceptor) { + this.frontCameraIO = frontCameraIO; + this.rearCameraIO = rearCameraIO; + this.visionPoseConsumer = visionPoseConsumer; + this.elevatorHeightSupplierMeters = elevatorHeightSupplierMeters; + this.poseAcceptor = poseAcceptor; + } + + @Override + public void periodic() { + // TODO: need to change if one camera is stationary + frontCameraIO.setPoseOffset( + new Pose3d( + FRONT_CAMERA_POSE.getX(), + FRONT_CAMERA_POSE.getY(), + FRONT_CAMERA_POSE.getZ() + elevatorHeightSupplierMeters.get(), + FRONT_CAMERA_POSE.getRotation()) + ); + rearCameraIO.setPoseOffset( + new Pose3d( + REAR_CAMERA_POSE.getX(), + REAR_CAMERA_POSE.getY(), + REAR_CAMERA_POSE.getZ() + elevatorHeightSupplierMeters.get(), + REAR_CAMERA_POSE.getRotation()) + ); + + frontCameraIO.updateInputs(frontCameraInputs); + rearCameraIO.updateInputs(rearCameraInputs); + Logger.processInputs("Vision/FrontCamera", frontCameraInputs); + Logger.processInputs("Vision/RearCamera", rearCameraInputs); + + if (frontCameraInputs.lastMeasurementTimestampSecs > frontPose.timestampSecs()) { + frontPose = new TimestampedVisionPose( + frontCameraInputs.lastMeasurementTimestampSecs, + frontCameraInputs.estimatedPoseMeters, + frontCameraInputs.seenTagIDs, + frontCameraInputs.tagPosesMeters, + true, + false); + } + if (rearCameraInputs.lastMeasurementTimestampSecs > rearPose.timestampSecs()) { + rearPose = new TimestampedVisionPose( + rearCameraInputs.lastMeasurementTimestampSecs, + rearCameraInputs.estimatedPoseMeters, + rearCameraInputs.seenTagIDs, + rearCameraInputs.tagPosesMeters, + false, + true); + } + + Optional latestPose = getEstimatedPose(); + latestPose.ifPresent(visionPose -> Logger.recordOutput("Vision/EstimatedPose", visionPose.poseMeters())); + latestPose.ifPresent(visionPoseConsumer); + } + + /** + * Gets the estimated pose by fusing individual computed poses from each camera. + * Returns null if no tags seen, in simulation, or if the elevator is moving + * too fast + */ + public Optional getEstimatedPose() { + boolean useFrontPose = poseAcceptor.shouldAcceptVision(frontPose); + boolean useRearPose = poseAcceptor.shouldAcceptVision(rearPose); + + if (useFrontPose && useRearPose) { + int[] allTagIDs = Arrays.copyOf(frontPose.seenTagIDs(), frontPose.getNumTagsSeen() + rearPose.getNumTagsSeen()); + System.arraycopy(rearPose.seenTagIDs(), 0, allTagIDs, frontPose.getNumTagsSeen(), rearPose.getNumTagsSeen()); + Pose2d[] allTagPoses = Arrays.copyOf(frontPose.tagPosesMeters(), frontPose.getNumTagsSeen() + rearPose.getNumTagsSeen()); + System.arraycopy(rearPose.tagPosesMeters(), 0, allTagPoses, frontPose.getNumTagsSeen(), rearPose.getNumTagsSeen()); + + return Optional.of(new TimestampedVisionPose( + (frontPose.timestampSecs() + rearPose.timestampSecs()) / 2, + frontPose.poseMeters().interpolate(rearPose.poseMeters(), 0.5), + allTagIDs, + allTagPoses, + true, + true)); + } else if (useFrontPose) return Optional.of(frontPose); + else if (useRearPose) return Optional.of(rearPose); + else return Optional.empty(); + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java new file mode 100644 index 00000000..ab63560c --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java @@ -0,0 +1,24 @@ +package org.team1540.robot2024.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import org.littletonrobotics.junction.AutoLog; + +public interface AprilTagVisionIO { + @AutoLog + class AprilTagVisionIOInputs { + public Pose2d estimatedPoseMeters = new Pose2d(); + public int[] seenTagIDs = {}; + public Pose2d[] tagPosesMeters = {}; + public double lastMeasurementTimestampSecs = 0.0; + } + + default void updateInputs(AprilTagVisionIOInputs inputs) {} + + /** Sets the robot-space pose of the camera */ + default void setPoseOffset(Pose3d newPose) {} + + default String getName() { + return ""; + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java new file mode 100644 index 00000000..0c6b01c3 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java @@ -0,0 +1,61 @@ +package org.team1540.robot2024.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj.Timer; +import org.team1540.robot2024.util.vision.LimelightHelpers; + +public class AprilTagVisionIOLimelight implements AprilTagVisionIO { + private final String name; + + public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { + this.name = name; + setPoseOffset(cameraOffsetMeters); + } + + @Override + public void updateInputs(AprilTagVisionIOInputs inputs) { + LimelightHelpers.Results results = LimelightHelpers.getLatestResults(name).targetingResults; + LimelightHelpers.LimelightTarget_Fiducial[] fiducialTargets = results.targets_Fiducials; + + inputs.estimatedPoseMeters = new Pose3d( + results.botpose_wpiblue[0], + results.botpose_wpiblue[1], + results.botpose_wpiblue[2], + new Rotation3d( + Math.toRadians(results.botpose_wpiblue[3]), + Math.toRadians(results.botpose_wpiblue[4]), + Math.toRadians(results.botpose_wpiblue[5]) + ) + ).toPose2d(); + + inputs.seenTagIDs = new int[fiducialTargets.length]; + for (int i = 0; i < fiducialTargets.length; i++) + inputs.seenTagIDs[i] = (int) fiducialTargets[i].fiducialID; // fiducial tag ids had better be ints + + inputs.tagPosesMeters = new Pose2d[fiducialTargets.length]; + for (int i = 0; i < fiducialTargets.length; i++) + inputs.tagPosesMeters[i] = fiducialTargets[i].getTargetPose_RobotSpace2D(); + + inputs.lastMeasurementTimestampSecs = + Timer.getFPGATimestamp() - (results.latency_capture + results.latency_pipeline) / 1000.0; + } + + @Override + public void setPoseOffset(Pose3d poseOffsetMeters) { + LimelightHelpers.setCameraPose_RobotSpace( + name, + poseOffsetMeters.getX(), + poseOffsetMeters.getY(), + poseOffsetMeters.getZ(), + Math.toDegrees(poseOffsetMeters.getRotation().getX()), + Math.toDegrees(poseOffsetMeters.getRotation().getY()), + Math.toDegrees(poseOffsetMeters.getRotation().getZ())); + } + + @Override + public String getName() { + return name; + } +} diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java new file mode 100644 index 00000000..03758d9e --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java @@ -0,0 +1,98 @@ +package org.team1540.robot2024.subsystems.vision; + +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.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; + +import java.io.IOException; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import static org.team1540.robot2024.Constants.Vision.*; + +public class AprilTagVisionIOSim implements AprilTagVisionIO { + private final VisionSystemSim visionSystemSim; + private final PhotonCameraSim cameraSim; + private final PhotonCamera camera; + private final PhotonPoseEstimator photonEstimator; + + private final Supplier poseSupplier; + + private Transform3d cameraTransform; + private Pose2d lastEstimatedPose; + + public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier poseSupplier) { + this.visionSystemSim = new VisionSystemSim(name); + this.poseSupplier = poseSupplier; + + AprilTagFieldLayout aprilTagLayout; + try { + aprilTagLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + } catch (IOException e) { + throw new RuntimeException(e); + } + visionSystemSim.addAprilTags(aprilTagLayout); + + SimCameraProperties cameraProps = new SimCameraProperties(); + cameraProps.setCalibration(SIM_RES_WIDTH, SIM_RES_HEIGHT, SIM_DIAGONAL_FOV); + cameraProps.setFPS(SIM_FPS); + cameraProps.setAvgLatencyMs(SIM_AVG_LATENCY_MS); + camera = new PhotonCamera(name); + cameraSim = new PhotonCameraSim(camera, cameraProps); + + cameraTransform = new Transform3d(cameraOffsetMeters.getTranslation(), cameraOffsetMeters.getRotation()); + visionSystemSim.addCamera(cameraSim, cameraTransform); + + photonEstimator = new PhotonPoseEstimator( + aprilTagLayout, + PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + cameraTransform); + photonEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_LAST_POSE); + lastEstimatedPose = poseSupplier.get(); + } + + @Override + public void updateInputs(AprilTagVisionIOInputs inputs) { + photonEstimator.setReferencePose(lastEstimatedPose); + visionSystemSim.update(poseSupplier.get()); + + Optional estimatedPose = photonEstimator.update(); + List trackedTargets = camera.getLatestResult().getTargets(); + + if (estimatedPose.isPresent()) { + lastEstimatedPose = estimatedPose.get().estimatedPose.toPose2d(); + inputs.estimatedPoseMeters = lastEstimatedPose; + inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; + } + inputs.seenTagIDs = new int[trackedTargets.size()]; + inputs.tagPosesMeters = new Pose2d[trackedTargets.size()]; + for (int i = 0; i < trackedTargets.size(); i++) { + PhotonTrackedTarget target = trackedTargets.get(i); + inputs.seenTagIDs[i] = target.getFiducialId(); + inputs.tagPosesMeters[i] = + new Pose3d().plus(target.getBestCameraToTarget().plus(cameraTransform.inverse())).toPose2d(); + } + } + + @Override + public void setPoseOffset(Pose3d newPose) { + cameraTransform = new Transform3d(newPose.getTranslation(), newPose.getRotation()); + visionSystemSim.adjustCamera(cameraSim, cameraTransform); + } + + @Override + public String getName() { + return camera.getName(); + } +} diff --git a/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java new file mode 100644 index 00000000..9021f037 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java @@ -0,0 +1,769 @@ +package org.team1540.robot2024.util.vision; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +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.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +// NOTE: This file is available at +// https://github.com/LimelightVision/limelightlib-wpijava/blob/main/LimelightHelpers.java + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public LimelightResults() { + targetingResults = new Results(); + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static double getNeuralClassID(String limelightName) { + return getLimelightNTDouble(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} diff --git a/src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java b/src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java new file mode 100644 index 00000000..de1da6a8 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java @@ -0,0 +1,44 @@ +package org.team1540.robot2024.util.vision; + +import edu.wpi.first.math.geometry.Pose2d; + +import java.util.Arrays; +import java.util.Objects; + +/** + * @param timestampSecs RIO FPGA timestamp of this vision pose + * @param poseMeters Measured, blue alliance-origin pose from vision + * @param seenTagIDs Array of all tags used to compute this measurement + * @param tagPosesMeters Pose in robot-space of all tags used to compute this measurement + * @param hasFrontCamera Whether this pose contains measurements from the front camera + * @param hasRearCamera Whether this pose contains measurements from the rear camera + */ +public record TimestampedVisionPose( + double timestampSecs, + Pose2d poseMeters, + int[] seenTagIDs, + Pose2d[] tagPosesMeters, + boolean hasFrontCamera, + boolean hasRearCamera) { + + public int getNumTagsSeen() { + return seenTagIDs.length; + } + + public double getAverageTagDistance() { + double sumDistances = 0.0; + for (Pose2d pose : tagPosesMeters) sumDistances += pose.getTranslation().getNorm(); + return sumDistances / getNumTagsSeen(); + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + TimestampedVisionPose that = (TimestampedVisionPose) o; + return Double.compare(timestampSecs, that.timestampSecs) == 0 + && Objects.equals(poseMeters, that.poseMeters) + && Arrays.equals(seenTagIDs, that.seenTagIDs) + && Arrays.equals(tagPosesMeters, that.tagPosesMeters); + } +} diff --git a/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java new file mode 100644 index 00000000..ca9daea4 --- /dev/null +++ b/src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java @@ -0,0 +1,41 @@ +package org.team1540.robot2024.util.vision; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; + +import java.util.function.Supplier; + +import static org.team1540.robot2024.Constants.Vision.*; + +public class VisionPoseAcceptor { + private final Supplier robotVelocitySupplier; + private final Supplier elevatorVelocitySupplier; + + public VisionPoseAcceptor( + Supplier robotVelocitySupplier, + Supplier elevatorVelocitySupplier) { + this.robotVelocitySupplier = robotVelocitySupplier; + this.elevatorVelocitySupplier = elevatorVelocitySupplier; + } + + public boolean shouldAcceptVision(TimestampedVisionPose visionPose) { + ChassisSpeeds robotVelocity = robotVelocitySupplier.get(); + double elevatorVelocity = elevatorVelocitySupplier.get(); + // Do not accept poses that have too much delay + if (Timer.getFPGATimestamp() - visionPose.timestampSecs() >= MAX_VISION_DELAY_SECS) return false; + + // Do not accept poses that see too little tags + if (visionPose.getNumTagsSeen() < MIN_ACCEPTED_NUM_TAGS) return false; + + // Do not accept poses that have an average tag distance that is too far away + if (visionPose.getAverageTagDistance() > MAX_ACCEPTED_AVG_TAG_DIST_METERS) return false; + + // Do not accept poses taken when the robot has too much rotational or translational velocity + boolean rotatingTooFast = Math.abs(robotVelocity.omegaRadiansPerSecond) > MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC; + boolean translatingTooFast = + Math.hypot(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond) + > MAX_ACCEPTED_LINEAR_SPEED_MPS; + boolean elevatorTooFast = Math.abs(elevatorVelocity) > MAX_ACCEPTED_ELEVATOR_SPEED_MPS; + return !rotatingTooFast && !translatingTooFast && !elevatorTooFast; + } +} diff --git a/vendordeps/photonlib-json-1.0.json b/vendordeps/photonlib-json-1.0.json new file mode 100644 index 00000000..8b19d38a --- /dev/null +++ b/vendordeps/photonlib-json-1.0.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.2.2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.2.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.2" + } + ] +}