Skip to content

Commit

Permalink
Better apriltag preview and logging
Browse files Browse the repository at this point in the history
  • Loading branch information
@WindingMotor
WindingMotor committed Feb 4, 2025
1 parent 94e96c4 commit a889373
Show file tree
Hide file tree
Showing 4 changed files with 69 additions and 50 deletions.
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/constants/BuildConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {}
Expand Down
5 changes: 4 additions & 1 deletion src/main/java/frc/robot/vision/IO_VisionBase.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand Down
30 changes: 20 additions & 10 deletions src/main/java/frc/robot/vision/IO_VisionReal.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ public class IO_VisionReal implements IO_VisionBase {
private final Map<CameraConstants.Camera, PhotonPipelineResult> currentResults = new HashMap<>();
private final Map<CameraConstants.Camera, Matrix<N3, N1>> currentStdDevs = new HashMap<>();

private Pose3d lastCurrentPose = new Pose3d();

/** Constructor initializes all cameras and their pose estimators */
public IO_VisionReal() {
fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
Expand All @@ -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<Pose3d> visibleTagPoses = new ArrayList<>();
List<Pose3d> leftTagPoses = new ArrayList<>();
List<Pose3d> rightTagPoses = new ArrayList<>();
List<Pose3d> backLeftTagPoses = new ArrayList<>();

// Update all camera results first
for (Map.Entry<CameraConstants.Camera, PhotonCamera> entry : cameras.entrySet()) {
Expand All @@ -87,19 +91,22 @@ public void updateInputs(VisionInputs inputs) {
for (Map.Entry<CameraConstants.Camera, PhotonCamera> 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<Pose3d> currentCameraPoses = new ArrayList<>();
for (PhotonTrackedTarget target : result.getTargets()) {
Optional<Pose3d> 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) {
Expand All @@ -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<CameraConstants.Camera, EstimatedRobotPose> cameraEstimates = new HashMap<>();

for (Map.Entry<CameraConstants.Camera, PhotonPoseEstimator> entry : poseEstimators.entrySet()) {
Expand Down
74 changes: 40 additions & 34 deletions src/main/java/frc/robot/vision/IO_VisionSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -37,6 +35,8 @@ public class IO_VisionSim implements IO_VisionBase {
private Optional<EstimatedRobotPose> lastEstimatedPose = Optional.empty();
private final AprilTagFieldLayout fieldLayout;

private Pose3d lastCurrentPose = new Pose3d();

public IO_VisionSim() {
fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
visionSim = new VisionSystemSim("Vision");
Expand Down Expand Up @@ -67,7 +67,9 @@ public IO_VisionSim() {

@Override
public void updateInputs(VisionInputs inputs) {
List<Pose3d> visibleTagPoses = new ArrayList<>();
List<Pose3d> leftTagPoses = new ArrayList<>();
List<Pose3d> rightTagPoses = new ArrayList<>();
List<Pose3d> backLeftTagPoses = new ArrayList<>();

// Update all camera results first
for (Map.Entry<CameraConstants.Camera, PhotonCameraSim> entry : cameraSims.entrySet()) {
Expand All @@ -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<Pose3d> 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);
}

Expand Down

0 comments on commit a889373

Please sign in to comment.