From f1497712b603f0ae71a9b00533de68dc8daa888f Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Fri, 5 Apr 2024 11:01:49 -0400 Subject: [PATCH] Revert "Added logging of Vision Camera Pose2d" This reverts commit 9f9c24ae480559e267b041eb60d6c6d8a7777de8. --- .../frc/robot/config/RobotConfigPhoenix.java | 22 ------ .../subsystems/vision/VisionSubsystem.java | 72 +++++++++---------- 2 files changed, 36 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/robot/config/RobotConfigPhoenix.java b/src/main/java/frc/robot/config/RobotConfigPhoenix.java index a311501f..794ea58f 100644 --- a/src/main/java/frc/robot/config/RobotConfigPhoenix.java +++ b/src/main/java/frc/robot/config/RobotConfigPhoenix.java @@ -40,28 +40,6 @@ public RobotConfigPhoenix() { new Translation3d(-Units.inchesToMeters(10.75), 0, Units.inchesToMeters(8)), new Rotation3d(0, Units.degreesToRadians(-33), Units.degreesToRadians(180))))); - cameras.add( - new VisionCamera( - "right", - "1196", - new Transform3d( - new Translation3d( - -Units.inchesToMeters(5.25), - -Units.inchesToMeters(11.25), - Units.inchesToMeters(7)), - new Rotation3d(0, Units.degreesToRadians(-32), Units.degreesToRadians(-90))))); - - cameras.add( - new VisionCamera( - "left", - "1190", - new Transform3d( - new Translation3d( - -Units.inchesToMeters(5.25), - Units.inchesToMeters(11.25), - Units.inchesToMeters(7)), - new Rotation3d(0, Units.degreesToRadians(-32), Units.degreesToRadians(90))))); - VisionConstants.visionDistanceOffsetInMeters = -0.2; vision = new VisionSubsystem(cameras, AprilTagFields.k2024Crescendo.loadAprilTagLayoutField()); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index eca6b071..abec06ce 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -3,6 +3,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.VecBuilder; 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.Transform3d; import edu.wpi.first.math.util.Units; @@ -15,7 +16,6 @@ import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -34,14 +34,11 @@ private class VisionCameraImpl { private final Transform3d robotToCamera; private final PhotonPoseEstimator poseEstimator; private final int index; - private final String name; - private PhotonCameraSim simCamera; private PhotonPipelineResult result; private Optional estimatedRobotPose; private VisionCameraImpl(VisionCamera camera, AprilTagFieldLayout fieldLayout) { - this.name = camera.name; this.camera = new PhotonCamera(camera.name); this.robotToCamera = camera.robotToCamera; this.poseEstimator = @@ -57,11 +54,6 @@ private VisionCameraImpl(VisionCamera camera, AprilTagFieldLayout fieldLayout) { private void update() { result = camera.getLatestResult(); estimatedRobotPose = poseEstimator.update(); - if (estimatedRobotPose.isPresent()) { - Logger.recordOutput( - "VisionSubsystem/Camera/" + name + "/Pose2d", - estimatedRobotPose.get().estimatedPose.toPose2d()); - } } private double getLatestTimestamp() { @@ -102,11 +94,16 @@ private Transform3d getRobotToCamera() { private final AprilTagFieldLayout fieldLayout; /* Debug Info */ - @AutoLogOutput private int numTargetsVisible; - @AutoLogOutput private int selectedTargetId = DevilBotState.getActiveTargetId(); - @AutoLogOutput private String selectedTargetName = DevilBotState.getTargetName(selectedTargetId); - @AutoLogOutput private double selectedTargetDistance = 0; - @AutoLogOutput private double selectedTargetYaw = 0; + @AutoLogOutput private int debugTargetsVisible; + @AutoLogOutput private int debugCurrentTargetId = DevilBotState.getActiveTargetId(); + + @AutoLogOutput + private String debugCurrentTargetName = DevilBotState.getTargetName(debugCurrentTargetId); + + @AutoLogOutput private double debugTargetDistance = 0; + @AutoLogOutput private double debugTargetYaw = 0; + @AutoLogOutput private Pose2d debugEstimatedPose; + @AutoLogOutput private Pose2d debugEstimatedPose2d; /* Simulation Support*/ private boolean simEnabled = false; @@ -153,19 +150,18 @@ public void periodic() { for (VisionCameraImpl camera : cameras) { camera.update(); if (camera == primaryCamera) { - numTargetsVisible = camera.result.getTargets().size(); - - // if (numTargetsVisible > 0) { - // PhotonTrackedTarget target = camera.result.getBestTarget(); - // Optional fieldToTarget = fieldLayout.getTagPose(target.getFiducialId()); - // if (fieldToTarget.isPresent()) { - // debugEstimatedPose2d = - // PhotonUtils.estimateFieldToRobotAprilTag( - // target.getBestCameraToTarget(), fieldToTarget.get(), - // camera.robotToCamera) - // .toPose2d(); - // } - // } + debugTargetsVisible = camera.result.getTargets().size(); + + if (debugTargetsVisible > 0) { + PhotonTrackedTarget target = camera.result.getBestTarget(); + Optional fieldToTarget = fieldLayout.getTagPose(target.getFiducialId()); + if (fieldToTarget.isPresent()) { + debugEstimatedPose2d = + PhotonUtils.estimateFieldToRobotAprilTag( + target.getBestCameraToTarget(), fieldToTarget.get(), camera.robotToCamera) + .toPose2d(); + } + } } Optional currentEstimatedRobotPose = camera.getEstimatedRobotPose(); @@ -182,24 +178,28 @@ public void periodic() { currentEstimatedRobotPose.get().timestampSeconds, VecBuilder.fill(distance / 2, distance / 2, distance / 2)); } + + if (camera == primaryCamera) { + debugEstimatedPose = currentEstimatedRobotPose.get().estimatedPose.toPose2d(); + } } } Optional distance; Optional yaw; - if (selectedTargetId != DevilBotState.getActiveTargetId()) { - selectedTargetId = DevilBotState.getActiveTargetId(); - selectedTargetDistance = 0; - selectedTargetYaw = 0; - selectedTargetName = DevilBotState.getTargetName(selectedTargetId); + if (debugCurrentTargetId != DevilBotState.getActiveTargetId()) { + debugCurrentTargetId = DevilBotState.getActiveTargetId(); + debugTargetDistance = 0; + debugTargetYaw = 0; + debugCurrentTargetName = DevilBotState.getTargetName(debugCurrentTargetId); } - distance = getDistanceToAprilTag(selectedTargetId); - yaw = getYawToAprilTag(selectedTargetId); + distance = getDistanceToAprilTag(debugCurrentTargetId); + yaw = getYawToAprilTag(debugCurrentTargetId); if (distance.isPresent()) { - selectedTargetDistance = distance.get(); + debugTargetDistance = distance.get(); } if (yaw.isPresent()) { - selectedTargetYaw = yaw.get(); + debugTargetYaw = yaw.get(); } }