diff --git a/src/main/java/frc/robot/constants/BuildConstants.java b/src/main/java/frc/robot/constants/BuildConstants.java index 64f33a7..0c23d6a 100644 --- a/src/main/java/frc/robot/constants/BuildConstants.java +++ b/src/main/java/frc/robot/constants/BuildConstants.java @@ -12,12 +12,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "SwerveDrive2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 57; - public static final String GIT_SHA = "a67860d83d8bc2818d16cb35f1bdd918d658be94"; - public static final String GIT_DATE = "2025-02-03 22:56:27 EST"; + public static final int GIT_REVISION = 58; + public static final String GIT_SHA = "94e96c490b34506e76c51f49284254fcf1bae681"; + public static final String GIT_DATE = "2025-02-04 14:31:05 EST"; public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2025-02-04 14:29:24 EST"; - public static final long BUILD_UNIX_TIME = 1738697364551L; + public static final String BUILD_DATE = "2025-02-04 15:21:56 EST"; + public static final long BUILD_UNIX_TIME = 1738700516440L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/vision/IO_VisionBase.java b/src/main/java/frc/robot/vision/IO_VisionBase.java index 19ef0e4..072f4c2 100644 --- a/src/main/java/frc/robot/vision/IO_VisionBase.java +++ b/src/main/java/frc/robot/vision/IO_VisionBase.java @@ -30,7 +30,10 @@ public static class VisionInputs { public double rightBestTargetID = -1.0; public double backLeftBestTargetID = -1.0; - public Pose3d[] visibleTagPoses = new Pose3d[0]; + public Pose3d[] leftVisibleTagPoses = new Pose3d[0]; + public Pose3d[] rightVisibleTagPoses = new Pose3d[0]; + public Pose3d[] backLeftVisibleTagPoses = new Pose3d[0]; + public Pose3d lastEstimatedPose = new Pose3d(); } diff --git a/src/main/java/frc/robot/vision/IO_VisionReal.java b/src/main/java/frc/robot/vision/IO_VisionReal.java index c862822..425854c 100644 --- a/src/main/java/frc/robot/vision/IO_VisionReal.java +++ b/src/main/java/frc/robot/vision/IO_VisionReal.java @@ -46,6 +46,8 @@ public class IO_VisionReal implements IO_VisionBase { private final Map currentResults = new HashMap<>(); private final Map> currentStdDevs = new HashMap<>(); + private Pose3d lastCurrentPose = new Pose3d(); + /** Constructor initializes all cameras and their pose estimators */ public IO_VisionReal() { fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); @@ -68,7 +70,9 @@ public IO_VisionReal() { /** Updates vision inputs with the latest target information from each camera */ @Override public void updateInputs(VisionInputs inputs) { - List visibleTagPoses = new ArrayList<>(); + List leftTagPoses = new ArrayList<>(); + List rightTagPoses = new ArrayList<>(); + List backLeftTagPoses = new ArrayList<>(); // Update all camera results first for (Map.Entry entry : cameras.entrySet()) { @@ -87,19 +91,22 @@ public void updateInputs(VisionInputs inputs) { for (Map.Entry entry : cameras.entrySet()) { CameraConstants.Camera cam = entry.getKey(); PhotonPipelineResult result = currentResults.get(cam); - if (result.hasTargets()) { PhotonTrackedTarget bestTarget = result.getBestTarget(); - // Collect poses of all visible tags + // Calculate camera position based on robot pose + Pose3d cameraPose = + lastCurrentPose.transformBy(new Transform3d(cam.translation, cam.rotation)); + + // Process targets for each camera + List currentCameraPoses = new ArrayList<>(); for (PhotonTrackedTarget target : result.getTargets()) { Optional tagPose = fieldLayout.getTagPose(target.getFiducialId()); - tagPose.ifPresent( - pose -> { - if (!visibleTagPoses.contains(pose)) { - visibleTagPoses.add(pose); - } - }); + if (tagPose.isPresent()) { + // Add both AprilTag pose and camera position pose + currentCameraPoses.add(tagPose.get()); + currentCameraPoses.add(cameraPose); + } } switch (cam) { @@ -120,13 +127,16 @@ public void updateInputs(VisionInputs inputs) { } // Update inputs with visible tag poses - inputs.visibleTagPoses = visibleTagPoses.toArray(new Pose3d[0]); + inputs.leftVisibleTagPoses = leftTagPoses.toArray(new Pose3d[0]); + inputs.rightVisibleTagPoses = rightTagPoses.toArray(new Pose3d[0]); + inputs.backLeftVisibleTagPoses = backLeftTagPoses.toArray(new Pose3d[0]); inputs.lastEstimatedPose = lastEstimatedPose.isPresent() ? lastEstimatedPose.get().estimatedPose : null; } /** Updates robot pose estimation using data from all cameras */ public void updatePoseEstimation(Pose2d currentPose) { + lastCurrentPose = new Pose3d(currentPose); Map cameraEstimates = new HashMap<>(); for (Map.Entry entry : poseEstimators.entrySet()) { diff --git a/src/main/java/frc/robot/vision/IO_VisionSim.java b/src/main/java/frc/robot/vision/IO_VisionSim.java index 2ed0bd3..1d85ef3 100644 --- a/src/main/java/frc/robot/vision/IO_VisionSim.java +++ b/src/main/java/frc/robot/vision/IO_VisionSim.java @@ -14,14 +14,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.constants.CameraConstants; import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; import java.util.Optional; -import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.simulation.PhotonCameraSim; @@ -37,6 +35,8 @@ public class IO_VisionSim implements IO_VisionBase { private Optional lastEstimatedPose = Optional.empty(); private final AprilTagFieldLayout fieldLayout; + private Pose3d lastCurrentPose = new Pose3d(); + public IO_VisionSim() { fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); visionSim = new VisionSystemSim("Vision"); @@ -67,7 +67,9 @@ public IO_VisionSim() { @Override public void updateInputs(VisionInputs inputs) { - List visibleTagPoses = new ArrayList<>(); + List leftTagPoses = new ArrayList<>(); + List rightTagPoses = new ArrayList<>(); + List backLeftTagPoses = new ArrayList<>(); // Update all camera results first for (Map.Entry entry : cameraSims.entrySet()) { @@ -87,48 +89,52 @@ public void updateInputs(VisionInputs inputs) { CameraConstants.Camera cam = entry.getKey(); PhotonPipelineResult result = currentResults.get(cam); - // Collect visible tag poses if (result.hasTargets()) { + PhotonTrackedTarget bestTarget = result.getBestTarget(); + + // Calculate camera position based on robot pose + Pose3d cameraPose = + lastCurrentPose.transformBy(new Transform3d(cam.translation, cam.rotation)); + + // Get poses for the current camera for (PhotonTrackedTarget target : result.getTargets()) { Optional tagPose = fieldLayout.getTagPose(target.getFiducialId()); - tagPose.ifPresent( - pose -> { - if (!visibleTagPoses.contains(pose)) { - visibleTagPoses.add(pose); - } - }); - } - - switch (cam) { - case LEFT_CAM: - inputs.hasLeftTarget = true; - inputs.leftBestTargetID = result.getBestTarget().getFiducialId(); - break; - case RIGHT_CAM: - inputs.hasRightTarget = true; - inputs.rightBestTargetID = result.getBestTarget().getFiducialId(); - break; - case BACK_LEFT_CAM: - inputs.hasBackLeftTarget = true; - inputs.backLeftBestTargetID = result.getBestTarget().getFiducialId(); - break; + if (tagPose.isPresent()) { + switch (cam) { + case LEFT_CAM: + leftTagPoses.add(tagPose.get()); + leftTagPoses.add(cameraPose); + inputs.hasLeftTarget = true; + inputs.leftBestTargetID = bestTarget.getFiducialId(); + break; + case RIGHT_CAM: + rightTagPoses.add(tagPose.get()); + rightTagPoses.add(cameraPose); + inputs.hasRightTarget = true; + inputs.rightBestTargetID = bestTarget.getFiducialId(); + break; + case BACK_LEFT_CAM: + backLeftTagPoses.add(tagPose.get()); + backLeftTagPoses.add(cameraPose); + inputs.hasBackLeftTarget = true; + inputs.backLeftBestTargetID = bestTarget.getFiducialId(); + break; + } + } } } - - inputs.lastEstimatedPose = - lastEstimatedPose.isPresent() ? lastEstimatedPose.get().estimatedPose : null; } - // Update inputs with visible tag poses - inputs.visibleTagPoses = visibleTagPoses.toArray(new Pose3d[0]); - Logger.recordOutput("Vision/TagPoses", inputs.visibleTagPoses); - - // Record estimated pose output - SmartDashboard.putData("Vision/CurrentEstimatedPose", visionSim.getDebugField()); + inputs.leftVisibleTagPoses = leftTagPoses.toArray(new Pose3d[0]); + inputs.rightVisibleTagPoses = rightTagPoses.toArray(new Pose3d[0]); + inputs.backLeftVisibleTagPoses = backLeftTagPoses.toArray(new Pose3d[0]); + inputs.lastEstimatedPose = + lastEstimatedPose.isPresent() ? lastEstimatedPose.get().estimatedPose : null; } @Override public void updatePoseEstimation(Pose2d currentPose) { + lastCurrentPose = new Pose3d(currentPose); visionSim.update(currentPose); }