diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4f80a33c..001ff2d8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -537,6 +537,9 @@ public static class VisionConstants { public static final int NUM_LAST_EST_POSES = 3; public static final double STD_DEV_SCALAR = 30; + public static final double MAX_ROBOT_VISION_VELOCITY = 2.0; // untuned 3/29 + public static final double AMBIGUITY_MATRIX_SCALAR = 4.0; // untuned 3/29 + // --- vision subsystem --- // (camera setup) // DEFAULT CONSTANTS (robot specific constants are below) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70e5adfb..5d4b00aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -95,7 +95,7 @@ public boolean isMagnitudeGreaterThan(double threshold) { public final CommandSwerveDrivetrain m_drivetrain = TunerConstants.createDrivetrain(); public final RollerSubsystem m_roller = new RollerSubsystem(); public final ShooterSubsystem m_shooter = new ShooterSubsystem(); - public final VisionSubsystem m_vision = new VisionSubsystem(() -> m_drivetrain.getState().Pose.getRotation()); + public final VisionSubsystem m_vision = new VisionSubsystem(() -> m_drivetrain.getState()); public final IndexerSubsystem m_indexer = new IndexerSubsystem(); public final PivotSubsystem m_pivot = new PivotSubsystem(); public final ColumnSubsystem m_column = new ColumnSubsystem(); diff --git a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java index 949445ec..39a55cb4 100644 --- a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,6 +26,8 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonTrackedTarget; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + import frc.robot.Constants.VisionConstants; public class LocalizationCamera { @@ -32,11 +35,12 @@ public class LocalizationCamera { private final PhotonCamera m_camera; private final String m_cameraName; private final String m_logString; + private final boolean m_isFront; // boolean that stores camera location (front or back) private AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); private final Field2d m_estPoseField = new Field2d(); // field pose estimator private PhotonPoseEstimator poseEstimator; - private final Supplier m_robotRotationSupplier; + private final Supplier m_robotSwerveStateSupplier; private LocalVisionFilterPipeline m_filterPipeline = new LocalVisionFilterPipeline(); // filtering pipeline for each camera, initalizes as empty pipeline @@ -50,11 +54,19 @@ public class LocalizationCamera { // every camera periodically creates a new CameraReading containing robot pose, std dev, timestamp, and number of targets seen. public static record CameraReading(String cameraName, EstimatedRobotPose robotPose, Matrix stdDevs, double timestampSeconds, int numTargets) {} - public LocalizationCamera(String cameraName, Transform3d robotToCam, Supplier robotHeadingSupplier) { + public LocalizationCamera(String cameraName, Transform3d robotToCam, Supplier robotSwerveStateSupplier) { m_cameraName = cameraName; m_camera = new PhotonCamera(m_cameraName); m_logString = "vision/" + m_cameraName; - m_robotRotationSupplier = robotHeadingSupplier; + m_robotSwerveStateSupplier = robotSwerveStateSupplier; + + // boolean that stores camera location + if (m_cameraName.indexOf("front") == -1) { + m_isFront = false; + } + else { + m_isFront = true; + } poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); @@ -229,11 +241,18 @@ private Optional resolveHighAmbiguityPose( var bestPose = estimateSingleTagRobotPose(candidatePoseEstimator, target, target.getBestCameraToTarget()); var alternatePose = estimateSingleTagRobotPose(candidatePoseEstimator, target, target.getAlternateCameraToTarget()); + SwerveDriveState robotState = m_robotSwerveStateSupplier.get(); + + // null check for robot swerve state. If it's null, can't resolve heading. + if (robotState == null) { + return Optional.empty(); + } + if (bestPose.isEmpty() || alternatePose.isEmpty()) { return Optional.empty(); } - Rotation2d currentHeading = m_robotRotationSupplier.get(); + Rotation2d currentHeading = robotState.Pose.getRotation(); Pose3d chosenPose = bestPose.get(); // Prefer the candidate whose field-relative heading is closer to the drivetrain heading. if (headingDistance(alternatePose.get().toPose2d().getRotation(), currentHeading) @@ -275,6 +294,8 @@ private Matrix calculateEstimationStdDevs(EstimatedRobotPose estimatedPo // Pose present. Start running Heuristic int numTags = 0; double avgDist = 0; + double matrixScalar = 1.0; + // Precalculation - see how many tags we found, and calculate an // average-distance metric @@ -290,10 +311,23 @@ private Matrix calculateEstimationStdDevs(EstimatedRobotPose estimatedPo .getDistance(estimatedPose.estimatedPose.toPose2d().getTranslation()); } + // if robot is moving, want to trust front camera data less. + // NOTE: if state is null, matrixScalar stays at 1.0 + SwerveDriveState robotState = m_robotSwerveStateSupplier.get(); + + if (robotState != null && robotState.Speeds != null) { // necessary null check + // if camera is in front and robot is above max velocity, update matrixScalar + double linearRobotSpeed = Math.hypot(robotState.Speeds.vxMetersPerSecond, + robotState.Speeds.vyMetersPerSecond); + if (m_isFront && linearRobotSpeed > VisionConstants.MAX_ROBOT_VISION_VELOCITY) { + matrixScalar = VisionConstants.AMBIGUITY_MATRIX_SCALAR; + } + } + if (numTags == 0) { // No tags visible. Default to single-tag std devs - SmartDashboard.putString(m_logString + "/standardDeviationState", "no tags visible"); - return VisionConstants.kSingleTagStdDevs; + SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviationState", "no tags visible"); + return VisionConstants.kSingleTagStdDevs.times(matrixScalar); } else if (numTags == 1 && avgDist > VisionConstants.VISION_DISTANCE_DISCARD) { SmartDashboard.putString(m_logString + "/standardDeviationState", "target too far"); return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); @@ -301,8 +335,8 @@ private Matrix calculateEstimationStdDevs(EstimatedRobotPose estimatedPo var unscaledStdDevs = numTags > 1 ? VisionConstants.kMultiTagStdDevs : VisionConstants.kSingleTagStdDevs; avgDist /= numTags; // increase std devs based on (average) distance - SmartDashboard.putString(m_logString + "/standardDeviationState", "good :)"); - return unscaledStdDevs.times(1 + (avgDist * avgDist / VisionConstants.STD_DEV_SCALAR)); + SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviationState", "good :)"); + return unscaledStdDevs.times(1 + (avgDist * avgDist / VisionConstants.STD_DEV_SCALAR)).times(matrixScalar); } } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 97e76026..b82e87f2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -43,6 +44,7 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import frc.robot.Constants; import frc.robot.Constants.VisionConstants; @@ -65,11 +67,11 @@ public class VisionSubsystem extends SubsystemBase { private boolean m_rawVideoModeEnabled = false; // true = raw video feed, false = normal AprilTag processing /** Creates a new Vision Subsystem. */ - public VisionSubsystem(Supplier robotHeadingSupplier) { - cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D, robotHeadingSupplier)); - cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D, robotHeadingSupplier)); - cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D, robotHeadingSupplier)); - cameras.add(new LocalizationCamera(VisionConstants.CAMERA4_NAME, VisionConstants.ROBOT_TO_CAM4_3D, robotHeadingSupplier)); + public VisionSubsystem(Supplier robotSwerveStateSupplier) { + cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D, robotSwerveStateSupplier)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D, robotSwerveStateSupplier)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA3_NAME, VisionConstants.ROBOT_TO_CAM3_3D, robotSwerveStateSupplier)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA4_NAME, VisionConstants.ROBOT_TO_CAM4_3D, robotSwerveStateSupplier)); } public List getValidCameraReadings(){ diff --git a/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java b/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java index 843c4c20..2a37d0ec 100644 --- a/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java +++ b/src/test/java/frc/robot/subsystems/vision/LocalizationCameraTest.java @@ -10,6 +10,9 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import org.junit.jupiter.api.AfterEach; @@ -17,6 +20,8 @@ import org.junit.jupiter.api.Test; import org.photonvision.EstimatedRobotPose; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.vision.LocalizationCamera; import frc.robot.subsystems.vision.LocalizationCamera.CameraReading; @@ -26,6 +31,7 @@ import java.lang.reflect.Field; import java.util.LinkedList; import java.util.List; +import java.util.function.Supplier; class LocalizationCameraTest { static final double DELTA = 1e-5; // acceptable deviation range @@ -33,19 +39,43 @@ class LocalizationCameraTest { LocalizationCamera m_camera; Transform3d m_robotToCam; + SwerveDriveState m_mockSwerveDriveState; + + /** + * Creates a mock SwerveDriveState for testing purposes. + * Returns a state with zero pose and zero velocity. + */ + private Supplier createMockSwerveDriveStateSupplier() { + m_mockSwerveDriveState = new SwerveDriveState(); + m_mockSwerveDriveState.Pose = new Pose2d(); + m_mockSwerveDriveState.Speeds = new ChassisSpeeds(); + m_mockSwerveDriveState.ModuleStates = new SwerveModuleState[] { + new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState() + }; + m_mockSwerveDriveState.ModuleTargets = new SwerveModuleState[] { + new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState() + }; + m_mockSwerveDriveState.ModulePositions = new SwerveModulePosition[] { + new SwerveModulePosition(), new SwerveModulePosition(), + new SwerveModulePosition(), new SwerveModulePosition() + }; + return () -> m_mockSwerveDriveState; + } @BeforeEach void setup() { // Initialize the HAL for simulation assert HAL.initialize(500, 0); - + // Create a test camera transform m_robotToCam = new Transform3d( new Translation3d(0.2, 0.0, 0.5), new Rotation3d(0, Math.toRadians(-15), 0) ); - - m_camera = new LocalizationCamera("test_camera", m_robotToCam, Rotation2d::new); + + m_camera = new LocalizationCamera("test_camera", m_robotToCam, createMockSwerveDriveStateSupplier()); } @AfterEach