-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: AprilTag vision IO and implementation for Limelight
- Loading branch information
1 parent
2c65005
commit cc0abdf
Showing
3 changed files
with
841 additions
and
0 deletions.
There are no files selected for viewing
23 changes: 23 additions & 0 deletions
23
src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,23 @@ | ||
package org.team1540.robot2024.subsystems.vision; | ||
|
||
import edu.wpi.first.math.geometry.Pose2d; | ||
import org.littletonrobotics.junction.AutoLog; | ||
|
||
public interface AprilTagVisionIO { | ||
@AutoLog | ||
class AprilTagVisionIOInputs { | ||
public Pose2d estimatedPoseMeters = new Pose2d(); | ||
public boolean hasTarget = false; | ||
public int[] seenTargetIDs = {}; | ||
public Pose2d[] targetPosesMeters = {}; | ||
|
||
public double pipelineLatency = 0.0; | ||
public double captureLatency = 0.0; | ||
} | ||
|
||
default void updateInputs(AprilTagVisionIOInputs inputs) {} | ||
|
||
default String getName() { | ||
return ""; | ||
} | ||
} |
52 changes: 52 additions & 0 deletions
52
src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,52 @@ | ||
package org.team1540.robot2024.subsystems.vision; | ||
|
||
import edu.wpi.first.math.geometry.Pose2d; | ||
import edu.wpi.first.math.geometry.Pose3d; | ||
import edu.wpi.first.math.geometry.Rotation3d; | ||
import org.team1540.robot2024.util.vision.LimelightHelpers; | ||
|
||
public class AprilTagVisionIOLimelight implements AprilTagVisionIO { | ||
private final String name; | ||
|
||
public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) { | ||
this.name = name; | ||
LimelightHelpers.setCameraPose_RobotSpace( | ||
name, | ||
cameraOffsetMeters.getX(), | ||
cameraOffsetMeters.getY(), | ||
cameraOffsetMeters.getZ(), | ||
Math.toDegrees(cameraOffsetMeters.getRotation().getX()), | ||
Math.toDegrees(cameraOffsetMeters.getRotation().getY()), | ||
Math.toDegrees(cameraOffsetMeters.getRotation().getZ())); | ||
} | ||
|
||
@Override | ||
public void updateInputs(AprilTagVisionIOInputs inputs) { | ||
LimelightHelpers.Results results = LimelightHelpers.getLatestResults(name).targetingResults; | ||
LimelightHelpers.LimelightTarget_Fiducial[] fiducialTargets = results.targets_Fiducials; | ||
|
||
inputs.estimatedPoseMeters = new Pose3d( | ||
results.botpose_wpiblue[0], | ||
results.botpose_wpiblue[1], | ||
results.botpose_wpiblue[2], | ||
new Rotation3d( | ||
Math.toRadians(results.botpose_wpiblue[3]), | ||
Math.toRadians(results.botpose_wpiblue[4]), | ||
Math.toRadians(results.botpose_wpiblue[5]) | ||
) | ||
).toPose2d(); | ||
inputs.hasTarget = results.valid; | ||
inputs.seenTargetIDs = new int[fiducialTargets.length]; | ||
for (int i = 0; i < fiducialTargets.length; i++) inputs.seenTargetIDs[i] = (int) fiducialTargets[i].fiducialID; // fiducial tag ids had better be ints | ||
inputs.targetPosesMeters = new Pose2d[fiducialTargets.length]; | ||
for (int i = 0; i < fiducialTargets.length; i++) inputs.targetPosesMeters[i] = fiducialTargets[i].getTargetPose_RobotSpace2D(); | ||
|
||
inputs.captureLatency = results.latency_capture; | ||
inputs.pipelineLatency = results.latency_pipeline; | ||
} | ||
|
||
@Override | ||
public String getName() { | ||
return name; | ||
} | ||
} |
Oops, something went wrong.