diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..e0f28e6e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -122,6 +122,8 @@ public void robotPeriodic() { SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if(!Constants.TESTBED){ Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); + Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom()); + Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose()); // Puts data on the elastic dashboard SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 484ec2cb..68d5e122 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,11 +12,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.AddTunableApriltagReading; -import frc.robot.commands.AddApriltagReading; +import frc.robot.commands.*; import frc.robot.commands.AddGarbageReading; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.drive.DriveCircle; import frc.robot.commands.hopper.SpinHopper; import frc.robot.commands.drive.DriveDirectionTime; import frc.robot.commands.feeder.SpinFeeder; @@ -61,10 +61,7 @@ import java.io.File; import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import choreo.auto.AutoFactory; import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; @@ -312,11 +309,13 @@ public void putShuffleboardCommands() { + SmartDashboard.putData("DriveInCircle", new DriveCircle(drivebase, 1,10,15000)); // TODO: These commands do not REQUIRE the subsystem therefore cannot be used in// production SmartDashboard.putData( "Intake/Spin Forward", new InstantCommand(() -> intakeSubsystem.setSpeed(1.0))); + SmartDashboard.putData( "Intake/Spin Backward", new InstantCommand(() -> intakeSubsystem.setSpeed(-1.0))); diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java index 70314611..86d152c5 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import java.io.DataInputStream; import java.io.IOException; +import frc.robot.constants.Constants; public class TCPApriltagServer extends TCPServer { @@ -20,7 +21,7 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc double posY = -1; double poseYaw = -1; double distanceToTag = -1; - double timestamp = -1; + double latency = -1; int apriltagNumber = -1; double now = 0; while (posX == -1 @@ -28,16 +29,16 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc && poseYaw == -1 && distanceToTag == -1 && apriltagNumber == -1 - && timestamp == -1) { + && latency == -1) { posX = stream.readDouble(); posY = stream.readDouble(); poseYaw = stream.readDouble(); distanceToTag = stream.readDouble(); - timestamp = stream.readDouble(); + latency = stream.readDouble(); apriltagNumber = stream.readInt(); - now = Timer.getFPGATimestamp() * 1000; + now = Timer.getFPGATimestamp() * 1000-Constants.AVERAGE_PIR_LATENCY-latency; } - return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, timestamp, now); + return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, latency, now); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drive/DriveCircle.java b/src/main/java/frc/robot/commands/drive/DriveCircle.java new file mode 100644 index 00000000..76366ba8 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveCircle.java @@ -0,0 +1,44 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DriveCircle extends LoggableCommand { + private final SwerveSubsystem drivebase; + private final double radiusMeters; + private final double angularVelocityRadPerSec; + private final double time; + private final Timer timer; + public DriveCircle(SwerveSubsystem drivebase, double radiusMeters, double angularVelocityRadPerSec, double time) { + this.drivebase = drivebase; + this.radiusMeters = radiusMeters; + this.angularVelocityRadPerSec = angularVelocityRadPerSec; + this.time = time; + this.timer = new Timer(); + addRequirements(drivebase); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + double tangentialSpeed = radiusMeters * angularVelocityRadPerSec; + double yVelocity = -tangentialSpeed; + drivebase.drive(new Translation2d(0, yVelocity), angularVelocityRadPerSec, false); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(time); + } + + @Override + public void end(boolean interrupted) { + drivebase.drive(new Translation2d(), 0, false); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index fab3f92d..f7a9e708 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,10 +1,19 @@ package frc.robot.constants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; 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.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.utils.logging.LoggedTunableNumber; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; +import static edu.wpi.first.units.Units.Radians; + public class GameConstants { public enum Mode { @@ -145,7 +154,21 @@ public enum Mode { public static final int SHIFT_4_START = 55; public static final int ENDGAME_START = 30; - public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed) + public static final LoggedTunableNumber VISION_SMOOTHER = new LoggedTunableNumber("VISION_SMOOTHER", 1.0); + public static final LoggedTunableNumber VISION_CONSISTENCY_THRESHOLD = new LoggedTunableNumber("VISION_CONSISTENCY_THRESHOLD", 100); //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed) + public static final LoggedTunableNumber VISION_STD_THRESHOLD = new LoggedTunableNumber("VISION_STD_THRESHOLD", 0.25); + public static final LoggedTunableNumber MAX_VISION_DISTANCE = new LoggedTunableNumber("MAX_VISION_DISTANCE", 6); public static final boolean ENABLE_VISION = true; public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back) + + public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later + public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later + public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change Later + public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later + public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later + public static final double AVERAGE_PIR_LATENCY = 20; //ms + public static final Vector STATE_STD_DEVS = VecBuilder.fill(0.1,0.1,0.1); // TODO: Change Later + public static final Vector INITIAL_VISION_STD_DEVS = VecBuilder.fill(0,0,0); // TODO: Change later + public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later + } diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index adb2bfc9..dcd41142 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -2,11 +2,28 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +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.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.*; +import frc.robot.constants.Constants; +import frc.robot.constants.GameConstants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; +import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.utils.Apriltag; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; import frc.robot.utils.logging.io.BaseIoImpl; +import org.littletonrobotics.junction.Logger; + +import java.util.function.Supplier; +import java.util.Random; public class ApriltagSubsystem extends SubsystemBase { @@ -14,15 +31,18 @@ public class ApriltagSubsystem extends SubsystemBase { private final ApriltagIO io; private final PoseEstimator estimator; private final SwerveSubsystem drivebase; + private final Supplier robotPoseSupplier; + private final Random random = new Random(); public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { this.drivebase = drivebase; this.io = io; estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); + robotPoseSupplier = drivebase::getSimulationPose; } public static ApriltagIO createRealIo() { - + return new TCPApriltagIo(LOGGING_NAME, new ApriltagInputs()); } @@ -40,11 +60,40 @@ public void addSimReading(ApriltagReading reading) { public ApriltagIO getIO(){ return io; } + public Pose2d getSimPose() { + return drivebase.getSimulationPose(); + } @Override public void periodic() { - estimator.updateVision(); estimator.updatePosition(drivebase.getOdom()); + estimator.updateVision(); io.periodic(); + if (Constants.currentMode == Constants.Mode.SIM) { + simReadings(); + } + } + public void simReadings() { + for (Apriltag tag: Apriltag.values()) { + Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(Constants.ROBOT_TO_CAMERA); + if (tag.canSee(cameraPos,Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { + Pose3d adjPose = tag.getPose().relativeTo(cameraPos); + double cosIncidenceAngle = (-adjPose.getX()*Math.cos(adjPose.getRotation().getZ())-adjPose.getY()*Math.sin(adjPose.getRotation().getZ()))/(adjPose.getTranslation().getNorm()); + double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); + if (distance/cosIncidenceAngle < Constants.MAX_VISION_DISTANCE.get()) { + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(),distance,0); + Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement, cameraPos); + double readingX = robotPoseSupplier.get().getX()+ random.nextGaussian()*stdDevs.get(0); + double readingY = robotPoseSupplier.get().getY()+ random.nextGaussian()*stdDevs.get(1); + double readingYaw = robotPoseSupplier.get().getRotation().getDegrees()+ random.nextGaussian()*stdDevs.get(2); + Pose2d readingPos = new Pose2d(readingX,readingY,Rotation2d.fromDegrees(readingYaw)); + distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); + if(BasicVisionFilter.inBounds(readingPos)) { + io.addReading(new ApriltagReading(readingX, readingY, readingYaw, + distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0)); + } + } + } + } } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 570876f4..e3b251a8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -9,20 +9,26 @@ import static edu.wpi.first.units.Units.Meter; import edu.wpi.first.math.controller.SimpleMotorFeedforward; 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.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.constants.Constants; import frc.robot.utils.logging.io.gyro.ThreadedGyro; +import org.littletonrobotics.junction.Logger; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -37,17 +43,21 @@ import java.io.File; import java.util.Arrays; +import java.util.concurrent.ConcurrentLinkedDeque; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import static edu.wpi.first.units.Units.Meter; - public class SwerveSubsystem extends SubsystemBase { /** * Swerve drive object. */ private final SwerveDrive swerveDrive; - private Vector variance = VecBuilder.fill(0.1,0.1,0.1); + private Vector variance = Constants.INITIAL_VISION_STD_DEVS; + private final Field2d rawOdomField = new Field2d(); + private SwerveDriveOdometry rawOdometry; + private final ConcurrentLinkedDeque poseError = new ConcurrentLinkedDeque<>(); + private record PoseErrorRecord(double timestamp, double error) { + } /** * Initialize {@link SwerveDrive} with the directory provided. * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null, @@ -86,7 +96,12 @@ public SwerveSubsystem(File directory, SwerveIMU swerveIMU) { swerveDrive.setModuleEncoderAutoSynchronize(Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE, Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible - + rawOdometry = new SwerveDriveOdometry( + swerveDrive.kinematics, + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions(), + startingPose + ); } /** @@ -108,6 +123,17 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig public void periodic() { //add vision pose here //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d())); + rawOdometry.update( + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions() + ); + + rawOdomField.setRobotPose(rawOdometry.getPoseMeters()); + double currentTime = Logger.getTimestamp()/1000000.0; + double oneSecondAgo = currentTime - 1.0; + poseError.removeIf(record -> record.timestamp < oneSecondAgo); + poseError.add(new PoseErrorRecord(currentTime, getError())); + Logger.recordOutput("AveragePoseError", getAverageError()); } @Override @@ -285,8 +311,14 @@ public SwerveDriveKinematics getKinematics() { * * @param initialHolonomicPose The pose to set the odometry to */ + // Might be broken public void resetOdometry(Pose2d initialHolonomicPose) { swerveDrive.resetOdometry(initialHolonomicPose); + SwerveModulePosition[] modules = new SwerveModulePosition[4]; + for (int i=0; i<4; i++) { + modules[i] = new SwerveModulePosition(); + } + rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose); } /** @@ -297,9 +329,22 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public double getError() { + return getPose().getTranslation().getDistance((getSimulationPose().getTranslation())); + } + public double getAverageError(){ + return poseError.stream().mapToDouble(record -> record.error).average().orElse(0); + } + public Pose3d getCameraPose() { + return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA); + } + + public Pose2d getSimulationPose() { + return swerveDrive.getSimulationDriveTrainPose().orElse(new Pose2d()); + } // Todo: fix to only get odomtry public Pose2d getOdom() { - return swerveDrive.getPose(); + return rawOdometry.getPoseMeters(); } /** @@ -470,7 +515,7 @@ public SwerveDrive getSwerveDrive() { return swerveDrive; } public void setVariance(Vector variance){ - this.variance = variance; + this.variance = variance.times(Constants.VISION_SMOOTHER.get()); } public void addVisionMeasurement(Pose2d pose, double visionTimestamp){ swerveDrive.addVisionMeasurement(pose, visionTimestamp, variance); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/VisionLog.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/VisionLog.java new file mode 100644 index 00000000..712e4290 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/VisionLog.java @@ -0,0 +1,8 @@ +package frc.robot.subsystems.swervedrive.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.subsystems.swervedrive.vision.truster.FilterResult; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.utils.Apriltag; + +public record VisionLog(VisionMeasurement measurement, FilterResult result) {} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 0d04e405..8a668baf 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -2,20 +2,22 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.VisionLog; import frc.robot.subsystems.swervedrive.vision.truster.FilterResult; import frc.robot.subsystems.swervedrive.vision.truster.PoseDeviation; import frc.robot.subsystems.swervedrive.vision.truster.VisionFilter; import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; -import java.util.ArrayList; -import java.util.LinkedHashMap; -import java.util.List; -import java.util.Map; +import java.util.*; +import java.util.concurrent.ConcurrentLinkedDeque; + +import frc.robot.utils.Apriltag; import org.littletonrobotics.junction.Logger; /** @@ -23,7 +25,10 @@ * filter. */ public class FilterablePoseManager extends PoseManager { + private record MeasurementRecord(Apriltag tag, double timestamp, FilterResult result) { } + private final VisionFilter filter; + private final ConcurrentLinkedDeque lastSecondMeasurements = new ConcurrentLinkedDeque<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, @@ -53,29 +58,46 @@ public FilterablePoseManager( @Override public void processQueue() { - LinkedHashMap filteredData = - filter.filter(visionMeasurementQueue); - visionMeasurementQueue.clear(); - List validMeasurements = new ArrayList<>(); - List invalidMeasurements = new ArrayList<>(); - for (Map.Entry entry : filteredData.entrySet()) { - VisionMeasurement v = entry.getKey(); - FilterResult r = entry.getValue(); - switch (r) { - case ACCEPTED -> { - setVisionSTD(visionTruster.calculateTrust(v)); - validMeasurements.add(v.measurement()); - addVisionMeasurement(v); - } - case NOT_PROCESSED -> visionMeasurementQueue.add(v); - case REJECTED -> { - invalidMeasurements.add(v.measurement()); + double currentTime = Logger.getTimestamp()/1000000.0; + double oneSecondAgo = currentTime - 1.0; + lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo); + List log = new ArrayList<>(); + List validMeasurementsPose = new ArrayList<>(); + List invalidMeasurementsPose = new ArrayList<>();; + List acceptedTagsPose = new ArrayList<>(); + for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { + int tagId = queueEntry.getKey(); + Queue queue = queueEntry.getValue(); + LinkedHashMap filteredData = + filter.filter(queue, drivebase.getCameraPose()); + queue.clear(); + for (Map.Entry filterEntry : filteredData.entrySet()) { + VisionMeasurement v = filterEntry.getKey(); + FilterResult r = filterEntry.getValue(); + log.add(new VisionLog(v, r)); + lastSecondMeasurements.add(new MeasurementRecord(Apriltag.of(tagId), v.timeOfMeasurement(),r)); + switch (r) { + case ACCEPTED -> { + setVisionSTD(visionTruster.calculateTrust(v, drivebase.getCameraPose())); + validMeasurementsPose.add(v.measurement()); + addVisionMeasurement(v); + acceptedTagsPose.add(Apriltag.of(tagId).getPose()); + + } + case NOT_PROCESSED -> queue.add(v); + case REJECTED -> { + invalidMeasurementsPose.add(v.measurement()); + } } } } - Logger.recordOutput("Apriltag/acceptedMeasurements", validMeasurements.toArray(Pose2d[]::new)); - Logger.recordOutput( - "Apriltag/rejectedMeasurements", invalidMeasurements.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/numberAcceptedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).count()); + Logger.recordOutput("Apriltag/numberNotProcessedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.NOT_PROCESSED).count()); + Logger.recordOutput("Apriltag/numberRejectedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.REJECTED).count()); + Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new)); + Logger.recordOutput("Apriltag/Log", log.toArray(VisionLog[]::new)); } public VisionTruster getVisionTruster() { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index e5ef49ae..069afe12 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -16,9 +16,7 @@ import frc.robot.constants.Constants; import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; -import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; -import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.*; import frc.robot.utils.Apriltag; import frc.robot.utils.math.ArrayUtils; @@ -78,20 +76,20 @@ public PoseEstimator( initGyroValueDeg);*/ TimeInterpolatableBuffer m1Buffer = TimeInterpolatableBuffer.createBuffer(Constants.POSE_BUFFER_STORAGE_TIME); + VisionTruster truster = new SquareVisionTruster(Constants.INITIAL_VISION_STD_DEVS, Constants.VISION_STD_DEV_CONST); this.poseManager = new FilterablePoseManager( - visionMeasurementStdDevs2, + Constants.INITIAL_VISION_STD_DEVS, kinematics, drivebase, m1Buffer, - new BasicVisionFilter(m1Buffer) { + new BasicVisionFilter(m1Buffer, truster) { @Override public Pose2d getVisionPose(VisionMeasurement measurement) { return measurement.measurement(); } - }, - new ConstantVisionTruster(visionMeasurementStdDevs2)); - SmartDashboard.putData(field); + }, truster); + } @@ -128,7 +126,7 @@ private void updateVision(int... invalidApriltagNumbers) { && !ArrayUtils.contains( invalidApriltagNumbers, apriltagSystem.getIO().getInputs().apriltagNumber[i])) { VisionMeasurement measurement = getVisionMeasurement(pos, i); - poseManager.registerVisionMeasurement(measurement); + poseManager.registerVisionMeasurement(measurement, apriltagSystem.getIO().getInputs().apriltagNumber[i]); } else { invalidCounter++; Logger.recordOutput("Apriltag/ValidationFailureCount", invalidCounter); @@ -144,9 +142,9 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { double serverTime = apriltagSystem.getIO().getInputs().serverTime[index]; //double timestamp = 0; // latency is not right we are assuming zero double timestamp = apriltagSystem.getIO().getInputs().timestamp[index]; - Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); + Pose2d visionPose = new Pose2d(pos[0], pos[1], Rotation2d.fromDegrees(pos[2])); double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; - return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); + return new VisionMeasurement(visionPose, Apriltag.of(apriltagSystem.getIO().getInputs().apriltagNumber[index]).getTagInfo(),distanceFromTag, timestamp/1000); } /** @@ -154,7 +152,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { * are sent to the {@link PoseManager} for further processing */ public void updateVision() { - updateVision(15, 4, 14, 5, 16, 3); + updateVision(0); } public void updateVision(Apriltag focusedTag) { @@ -200,6 +198,10 @@ public FilterablePoseManager getPoseManager() { public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( - new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6)); + new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(),0, Logger.getTimestamp() / 1e6),1); + } + + public VisionTruster getVisionTruster() { + return poseManager.getVisionTruster(); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index 3f979f9f..1d59828f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -12,6 +12,7 @@ import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import org.littletonrobotics.junction.Logger; +import java.util.LinkedHashMap; import java.util.LinkedList; import java.util.Optional; import java.util.Queue; @@ -23,8 +24,8 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; - protected final Queue visionMeasurementQueue = new LinkedList<>(); - private final SwerveSubsystem drivebase; + protected final LinkedHashMap> visionMeasurementQueueMap = new LinkedHashMap<>(); + protected final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; public PoseManager( @@ -62,23 +63,22 @@ public void addOdomMeasurement(Pose2d pose, long timestamp) { estimatedPoseBuffer.addSample(timestamp, pose); } - public void registerVisionMeasurement(VisionMeasurement measurement) { + public void registerVisionMeasurement(VisionMeasurement measurement, int tagId) { if (measurement == null) { return; } - while (visionMeasurementQueue.size() >= 3) { - visionMeasurementQueue.poll(); - } - visionMeasurementQueue.add(measurement); + visionMeasurementQueueMap.computeIfAbsent(tagId, k -> new LinkedList<>()).add(measurement); } // override for filtering public void processQueue() { - VisionMeasurement m = visionMeasurementQueue.poll(); - while (m != null) { - setVisionSTD(visionTruster.calculateTrust(m)); - addVisionMeasurement(m); - m = visionMeasurementQueue.poll(); + for (Queue queue : visionMeasurementQueueMap.values()) { + VisionMeasurement m = queue.poll(); + while (m != null) { + setVisionSTD(visionTruster.calculateTrust(m, drivebase.getCameraPose())); + addVisionMeasurement(m); + m = queue.poll(); + } } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java index 5d160e28..8d4014fb 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.numbers.N3; import frc.robot.constants.Constants; import java.util.LinkedHashMap; import java.util.Optional; @@ -17,14 +20,16 @@ public abstract class BasicVisionFilter implements VisionFilter, VisionTransformer { private final TimeInterpolatableBuffer poseBuffer; + private final VisionTruster truster; - public BasicVisionFilter(TimeInterpolatableBuffer poseBuffer) { + public BasicVisionFilter(TimeInterpolatableBuffer poseBuffer, VisionTruster truster) { this.poseBuffer = poseBuffer; + this.truster = truster; } @Override public LinkedHashMap filter( - Queue measurements) { + Queue measurements, Pose3d cameraPose) { LinkedHashMap resultMap = new LinkedHashMap<>(); VisionMeasurement m1 = measurements.poll(); VisionMeasurement m2 = measurements.peek(); @@ -54,7 +59,7 @@ public LinkedHashMap filter( Pose2d vision1Pose = getVisionPose(m1); Pose2d vision2Pose = getVisionPose(m2); boolean valid1 = - filterVision(vision1Pose, vision2Pose, m1.timeOfMeasurement(), m2.timeOfMeasurement()); + filterVision(m1, m2, cameraPose); resultMap.put(m1, valid1 ? FilterResult.ACCEPTED : FilterResult.REJECTED); m1 = measurements.poll(); m2 = measurements.peek(); @@ -63,23 +68,27 @@ public LinkedHashMap filter( return resultMap; } - private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double m2Time) { - Optional odomPoseAtVis1 = poseBuffer.getSample(m1Time); - Optional odomPoseAtVis2 = poseBuffer.getSample(m2Time); + private boolean filterVision(VisionMeasurement m1, VisionMeasurement m2, Pose3d cameraPose) { + Optional odomPoseAtVis1 = poseBuffer.getSample(m1.timeOfMeasurement()); + Optional odomPoseAtVis2 = poseBuffer.getSample(m2.timeOfMeasurement()); if (odomPoseAtVis1.isEmpty() || odomPoseAtVis2.isEmpty()) { return false; } - if (!inBounds(m1Pose) || !inBounds(m2Pose)) { + if (!inBounds(m1.measurement()) || !inBounds(m2.measurement())) { return false; } double odomDiff1To2 = odomPoseAtVis1.get().getTranslation().getDistance(odomPoseAtVis2.get().getTranslation()); - double visionDiff1To2 = m1Pose.getTranslation().getDistance(m2Pose.getTranslation()); + double visionDiff1To2 = m1.measurement().getTranslation().getDistance(m2.measurement().getTranslation()); double diff = Math.abs(odomDiff1To2 - visionDiff1To2); - return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD; + Vector std = truster.calculateTrust(m1, cameraPose); + if (std.get(0) > Constants.VISION_STD_THRESHOLD.get()) { + return false; + } + return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD.get(); } - private boolean inBounds(Pose2d pose2d) { + public static boolean inBounds(Pose2d pose2d) { return pose2d.getX() > 0 && pose2d.getX() < 20 && pose2d.getY() > 0 && pose2d.getY() < 20; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java index 3017ccb1..fe133bbe 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; @@ -11,7 +12,7 @@ public ConstantVisionTruster(Vector initialSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { return initialSTD; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java index e7f75ea6..78e6f094 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public class LinearVisionTruster extends DistanceVisionTruster { @@ -14,7 +15,7 @@ public LinearVisionTruster(Vector initialSTD, double slopeSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { double std = measurement.distanceFromTag() * slopeSTD; return initialSTD.plus(VecBuilder.fill(std, std, std)); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java index 7a925fc8..944fbffa 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java @@ -2,7 +2,9 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; +import frc.robot.utils.Apriltag; public class SquareVisionTruster extends DistanceVisionTruster { @@ -15,8 +17,10 @@ public SquareVisionTruster(Vector initialSTD, double constant) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { - double std = Math.pow(measurement.distanceFromTag() - 0.4572, 2) * constant; - return initialSTD.plus(VecBuilder.fill(std, std, std)); + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { + Pose3d adjPose = measurement.tag().tag().getPose().relativeTo(cameraPose); + double cosIncidenceAngle = (-adjPose.getX()*Math.cos(adjPose.getRotation().getZ())-adjPose.getY()*Math.sin(adjPose.getRotation().getZ()))/(adjPose.getTranslation().getNorm()); + double std = Math.pow(measurement.distanceFromTag(), 2) * constant / cosIncidenceAngle; //TODO: No magic numbers + return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java index e1b94831..6f156bad 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.geometry.Pose3d; + import java.util.LinkedHashMap; import java.util.Queue; public interface VisionFilter { - LinkedHashMap filter(Queue measurements); + LinkedHashMap filter(Queue measurements, Pose3d cameraPose); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java index edd9af38..82b54ead 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.utils.Apriltag; /** * @param measurement estimated robot position (meters) calculated from apriltag tag what tag produced the @@ -9,4 +10,4 @@ * @param timeOfMeasurement time when the pose was measured (seconds) */ public record VisionMeasurement( - Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} + Pose2d measurement, Apriltag.TagPose tag, double distanceFromTag, double timeOfMeasurement) {} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java index 7ce7c3d4..41776101 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public interface VisionTruster { - Vector calculateTrust(VisionMeasurement measurement); + Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPos); } diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java index 66449f22..7edb0330 100644 --- a/src/main/java/frc/robot/utils/Apriltag.java +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -1,40 +1,49 @@ package frc.robot.utils; -import edu.wpi.first.math.geometry.Translation3d; - -public enum Apriltag { //Andymark field: - ONE(467.08,291.79, 35.00), //Trench, Red z rotation:180 - TWO(468.56,182.08, 44.25), //Hub, Red z rotation:90 - THREE( 444.80,172.32, 44.25), //Hub, Red z rotation:180 - FOUR(444.80,158.32, 44.25), //Hub, Red z rotation:180 - FIVE(468.56,134.56, 44.25), //Hub, Red z rotation:270 - SIX(467.08,24.85, 35.00), //Trench, Red z rotation:180 - SEVEN(470.03,24.85, 35.00), //Trench, Red z rotation:0 - EIGHT(482.56,134.56, 44.25), //Hub, Red z rotation:270 - NINE(492.33,144.32, 44.25), //Hub, Red z rotation:0 - TEN(492.33,158.32, 44.25), //Hub, Red z rotation:0 - ELEVEN(482.56,182.08, 44.25), //Hub, Red z rotation:90 - TWELVE(470.03,291.79, 35.00), //Trench, Red z rotation:0 - THIRTEEN(649.58,291.02, 21.75), //Outpost, Red z rotation:180 - FOURTEEN(649.58,274.02, 21.75), //Outpost, Red z rotation:180 - FIFTEEN(649.57,169.78, 21.75), //Tower, Red z rotation:180 - SIXTEEN(649.57,152.78, 21.75), //Tower, Red z rotation:180 - SEVENTEEN(183.03,24.85, 35.00), //Trench, Blue z rotation:0 - EIGHTEEN(181.56,134.56, 44.25), //Hub, Blue z rotation:270 - NINETEEN(205.32,144.32, 44.25), //Hub, Blue z rotation:0 - TWENTY(205.32,158.32, 44.52), //Hub, Blue z rotation:0 - TWENTY_ONE(181.56,182.08, 44.25), //Hub, Blue z rotation:90 - TWENTY_TWO(183.03, 291.79, 35.00), //Trench, Blue z rotation:0 - TWENTY_THREE(180.08, 291.79, 35.00),//Trench, Blue z rotation:180 - TWENTY_FOUR(167.56, 182.08, 44.25),//Hub, Blue z rotation:90 - TWENTY_FIVE(157.79, 172.32, 44.25),//Hub, Blue z rotation:180 - TWENTY_SIX(157.79, 158.32, 44.25),//Hub, Blue z rotation:180 - TWENTY_SEVEN(167.58, 134.56, 44.25),//Hub, Blue z rotation:270 - TWENTY_EIGHT(180.08, 24.85, 35.00),//Trench, Blue z rotation:180 - TWENTY_NINE(0.54, 25.62, 21.75),//Outpost, Blue z rotation:0 - THIRTY(0.54, 42.62, 21.75),//Outpost, Blue z rotation:0 - THIRTY_ONE(0.55, 146.86, 21.75),//Tower, Blue z rotation:0 - THIRTY_TWO(0.55, 163.86, 21.75);//Tower, Blue z rotation:0 +import edu.wpi.first.math.geometry.*; + +import java.lang.Math.*; + +import edu.wpi.first.math.util.Units; + +import static edu.wpi.first.units.Units.Radians; +import static java.lang.Math.PI; +import static java.lang.Math.abs; + +public enum Apriltag { + //Andymark field: + ONE(467.08,291.79, 35.00, PI), //Trench, Red z rotation:180 + TWO(468.56,182.08, 44.25, PI/2), //Hub, Red z rotation:90 + THREE( 444.80,172.32, 44.25, PI), //Hub, Red z rotation:180 + FOUR(444.80,158.32, 44.25, PI), //Hub, Red z rotation:180 + FIVE(468.56,134.56, 44.25, 3*PI/2), //Hub, Red z rotation:270 + SIX(467.08,24.85, 35.00,PI), //Trench, Red z rotation:180 + SEVEN(470.03,24.85, 35.00, 0), //Trench, Red z rotation:0 + EIGHT(482.56,134.56, 44.25, 3*PI/2), //Hub, Red z rotation:270 + NINE(492.33,144.32, 44.25, 0), //Hub, Red z rotation:0 + TEN(492.33,158.32, 44.25, 0), //Hub, Red z rotation:0 + ELEVEN(482.56,182.08, 44.25, PI/2), //Hub, Red z rotation:90 + TWELVE(470.03,291.79, 35.00, 0), //Trench, Red z rotation:0 + THIRTEEN(649.58,291.02, 21.75, PI), //Outpost, Red z rotation:180 + FOURTEEN(649.58,274.02, 21.75, PI), //Outpost, Red z rotation:180 + FIFTEEN(649.57,169.78, 21.75, PI), //Tower, Red z rotation:180 + SIXTEEN(649.57,152.78, 21.75, PI), //Tower, Red z rotation:180 + SEVENTEEN(183.03,24.85, 35.00, 0), //Trench, Blue z rotation:0 + EIGHTEEN(181.56,134.56, 44.25, 3*PI/2), //Hub, Blue z rotation:270 + NINETEEN(205.32,144.32, 44.25, 0), //Hub, Blue z rotation:0 + TWENTY(205.32,158.32, 44.52, 0), //Hub, Blue z rotation:0 + TWENTY_ONE(181.56,182.08, 44.25, PI/2), //Hub, Blue z rotation:90 + TWENTY_TWO(183.03, 291.79, 35.00, 0), //Trench, Blue z rotation:0 + TWENTY_THREE(180.08, 291.79, 35.00, PI),//Trench, Blue z rotation:180 + TWENTY_FOUR(167.56, 182.08, 44.25, PI/2),//Hub, Blue z rotation:90 + TWENTY_FIVE(157.79, 172.32, 44.25, PI),//Hub, Blue z rotation:180 + TWENTY_SIX(157.79, 158.32, 44.25, PI),//Hub, Blue z rotation:180 + TWENTY_SEVEN(167.58, 134.56, 44.25, 3*PI/2),//Hub, Blue z rotation:270 + TWENTY_EIGHT(180.08, 24.85, 35.00, PI),//Trench, Blue z rotation:180 + TWENTY_NINE(0.54, 25.62, 21.75, 0),//Outpost, Blue z rotation:0 + THIRTY(0.54, 42.62, 21.75, 0),//Outpost, Blue z rotation:0 + THIRTY_ONE(0.55, 146.86, 21.75, 0),//Tower, Blue z rotation:0 + THIRTY_TWO(0.55, 163.86, 21.75, 0);//Tower, Blue z rotation:0 /* Welded Field: @@ -73,15 +82,21 @@ public enum Apriltag { THIRTY_ONE(0.32, 147.47, 21.75),//Tower, Blue z rotation:0 THIRTY_TWO(0.32, 164.47, 21.75);//Tower, Blue z rotation:0 */ + public record TagPose(Apriltag tag, Pose3d pose) {} + private final double xMeters; + private final double yMeters; + private final double zMeters; + private final Pose3d pose; + private final Translation3d translation; + private final double rotation; - private final double x; - private final double y; - private final double z; - - Apriltag(double x, double y, double z) { - this.x = x; - this.y = y; - this.z = z; + Apriltag(double xInches, double yInches, double zInches, double rotation) { + this.xMeters = Units.inchesToMeters(xInches); + this.yMeters = Units.inchesToMeters(yInches); + this.zMeters = Units.inchesToMeters(zInches); + this.translation = new Translation3d(xMeters,yMeters,zMeters); + this.rotation = rotation; + this.pose = new Pose3d(translation,new Rotation3d(0,0,rotation)); } public static Apriltag of(int number) { @@ -92,22 +107,48 @@ public static Apriltag of(int number) { } public double getX() { - return x; + return xMeters; } public double getY() { - return y; + return yMeters; } public double getZ() { - return z; + return zMeters; + } + + public double getRotation() { + return rotation; } public Translation3d getTranslation() { - return new Translation3d(x, y, z); + return translation; + } + + public Pose3d getPose() { + return pose; } public int number() { - return ordinal(); + return ordinal()+1; + } + public TagPose getTagInfo() { + return new TagPose(this, pose); + } + public boolean canSee(Pose3d cameraPose, double HorizontalFOV, double VerticalFOV) { + Pose3d adjPose = getPose().relativeTo(cameraPose); + double horizontalAngle = Math.atan2(adjPose.getY(), adjPose.getX()); + double verticalAngle = Math.asin(adjPose.getZ()/adjPose.getTranslation().getNorm()); + double tagAngle = adjPose.getRotation().getZ()+PI; + double diffAngle = abs(horizontalAngle-tagAngle); + if (diffAngle>=2*PI) { + diffAngle-=2*PI; + } + if (diffAngle >= PI/2 && diffAngle <=3*PI/2) { + return false; + } + return abs(horizontalAngle) < HorizontalFOV/2 && abs(verticalAngle) < VerticalFOV/2;// TODO: Maybe change Later By Implementing Math Calculations (Linear Algebra) } } +