This repository has been archived by the owner on Feb 18, 2024. It is now read-only.
-
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.
- Loading branch information
Showing
7 changed files
with
218 additions
and
3 deletions.
There are no files selected for viewing
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
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
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
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,33 @@ | ||
package frc.robot.subsystems.vision; | ||
|
||
import edu.wpi.first.apriltag.AprilTagFieldLayout; | ||
import edu.wpi.first.math.geometry.Transform3d; | ||
import java.util.Optional; | ||
import org.littletonrobotics.junction.AutoLog; | ||
import org.photonvision.EstimatedRobotPose; | ||
import org.photonvision.PhotonPoseEstimator.PoseStrategy; | ||
import org.photonvision.targeting.PhotonPipelineResult; | ||
|
||
public interface VisionIO { | ||
@AutoLog | ||
public static class VisionIOInputs { | ||
public String cameraName; | ||
public boolean hasTarget; | ||
public boolean isTargetAprilTag; | ||
public double targetX; | ||
public double targetY; | ||
public double targetArea; | ||
public double targetSkew; | ||
public double targetLatency; | ||
public double FId; | ||
} | ||
|
||
public default void updateInputs(VisionIOInputs inputs) {} | ||
|
||
public void initPhotonPoseEstimator( | ||
Transform3d cameraToRobot, AprilTagFieldLayout tagLayout, PoseStrategy poseStrategy); | ||
|
||
public PhotonPipelineResult getPipelineResult(); | ||
|
||
public Optional<EstimatedRobotPose> getEstimatedRobotPose(); | ||
} |
99 changes: 99 additions & 0 deletions
99
src/main/java/frc/robot/subsystems/vision/VisionIOCamera.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,99 @@ | ||
package frc.robot.subsystems.vision; | ||
|
||
import edu.wpi.first.apriltag.AprilTagFieldLayout; | ||
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 frc.robot.constants.Constants.Vision; | ||
|
||
import java.util.Optional; | ||
|
||
import javax.xml.crypto.dsig.Transform; | ||
|
||
import org.photonvision.EstimatedRobotPose; | ||
import org.photonvision.PhotonCamera; | ||
import org.photonvision.PhotonPoseEstimator; | ||
import org.photonvision.PhotonUtils; | ||
import org.photonvision.PhotonPoseEstimator.PoseStrategy; | ||
import org.photonvision.targeting.PhotonPipelineResult; | ||
import org.photonvision.targeting.PhotonTrackedTarget; | ||
|
||
public class VisionIOCamera implements VisionIO { | ||
public String m_cameraName; | ||
public PhotonCamera m_camera; | ||
public Transform3d m_cameraToRobot; | ||
private PhotonPoseEstimator m_poseEstimator; | ||
public AprilTagFieldLayout m_tagLayout; | ||
public PoseStrategy m_poseStrategy; | ||
private Optional<Pose2d> curPose; | ||
private PhotonPipelineResult m_result; | ||
|
||
public VisionIOCamera(String cameraName) { | ||
m_cameraName = cameraName; | ||
m_camera = new PhotonCamera(m_cameraName); | ||
} | ||
|
||
@Override | ||
public void initPhotonPoseEstimator( | ||
Transform3d cameraToRobot, AprilTagFieldLayout tagLayout, PoseStrategy poseStrategy) { | ||
m_tagLayout = tagLayout; | ||
m_poseStrategy = poseStrategy; | ||
m_poseEstimator = new PhotonPoseEstimator(tagLayout, poseStrategy, m_camera, cameraToRobot); | ||
} | ||
|
||
public void initPoseEstimator( | ||
Transform3d cameraToRobot, AprilTagFieldLayout tagLayout, PoseStrategy poseStrategy) { | ||
m_tagLayout = tagLayout; | ||
m_poseStrategy = poseStrategy; | ||
m_cameraToRobot = cameraToRobot; | ||
|
||
} | ||
|
||
@Override | ||
public PhotonPipelineResult getPipelineResult() { | ||
|
||
return m_result; | ||
} | ||
|
||
// @Override | ||
// public Optional<EstimatedRobotPose> getEstimatedRobotPose() { | ||
// if (m_poseEstimator != null) { | ||
// curPose = m_poseEstimator.update(getPipelineResult()); | ||
// return curPose; | ||
// } else { | ||
// return Optional.empty(); | ||
// } | ||
// } | ||
|
||
@Override | ||
public Optional<Pose2d> getEstimatedRobotPose() { | ||
if (m_result.hasTargets()) { | ||
PhotonTrackedTarget target = m_result.getBestTarget(); | ||
Pose3d robotPose = PhotonUtils.estimateFieldToRobotAprilTag( | ||
target.getBestCameraToTarget(), | ||
m_tagLayout.getTagPose(target.getFiducialId()).get(), | ||
m_cameraToRobot | ||
); | ||
curPose = Optional.of(robotPose.toPose2d()); | ||
return curPose; | ||
} | ||
return Optional.empty(); | ||
} | ||
|
||
@Override | ||
public void updateInputs(VisionIOInputs inputs) { | ||
inputs.cameraName = m_cameraName; | ||
m_result = m_camera.getLatestResult(); | ||
inputs.hasTarget = m_result.hasTargets(); | ||
inputs.isTargetAprilTag = m_result.hasTargets() && true; | ||
if (m_result.hasTargets()) { | ||
PhotonTrackedTarget target = m_result.getBestTarget(); | ||
inputs.targetX = target.getPitch(); | ||
inputs.targetY = target.getYaw(); | ||
inputs.targetArea = target.getArea(); | ||
inputs.targetSkew = target.getSkew(); | ||
inputs.FId = target.getFiducialId(); | ||
} | ||
} | ||
} |
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
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,57 @@ | ||
{ | ||
"fileName": "photonlib.json", | ||
"name": "photonlib", | ||
"version": "v2024.1.2", | ||
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", | ||
"frcYear": "2024", | ||
"mavenUrls": [ | ||
"https://maven.photonvision.org/repository/internal", | ||
"https://maven.photonvision.org/repository/snapshots" | ||
], | ||
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", | ||
"jniDependencies": [], | ||
"cppDependencies": [ | ||
{ | ||
"groupId": "org.photonvision", | ||
"artifactId": "photonlib-cpp", | ||
"version": "v2024.1.2", | ||
"libName": "photonlib", | ||
"headerClassifier": "headers", | ||
"sharedLibrary": true, | ||
"skipInvalidPlatforms": true, | ||
"binaryPlatforms": [ | ||
"windowsx86-64", | ||
"linuxathena", | ||
"linuxx86-64", | ||
"osxuniversal" | ||
] | ||
}, | ||
{ | ||
"groupId": "org.photonvision", | ||
"artifactId": "photontargeting-cpp", | ||
"version": "v2024.1.2", | ||
"libName": "photontargeting", | ||
"headerClassifier": "headers", | ||
"sharedLibrary": true, | ||
"skipInvalidPlatforms": true, | ||
"binaryPlatforms": [ | ||
"windowsx86-64", | ||
"linuxathena", | ||
"linuxx86-64", | ||
"osxuniversal" | ||
] | ||
} | ||
], | ||
"javaDependencies": [ | ||
{ | ||
"groupId": "org.photonvision", | ||
"artifactId": "photonlib-java", | ||
"version": "v2024.1.2" | ||
}, | ||
{ | ||
"groupId": "org.photonvision", | ||
"artifactId": "photontargeting-java", | ||
"version": "v2024.1.2" | ||
} | ||
] | ||
} |