Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adding changes to interfaces AND preliminary PhotonVision interfacing class

Browse Source
This commit is contained in:
Bradley Bickford 2024-01-12 22:19:17 -05:00
parent 49b55e5942
commit 7be7a80aac
5 changed files with 203 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/java/frc/robot/interfaces/IAprilTagProvider.java
View File

@ -3,8 +3,7 @@ package frc.robot.interfaces;
import java.util.OptionalDouble;
public interface IAprilTagProvider {
public OptionalDouble getTagHorizontalOffsetByID(int id);
public OptionalDouble getTagDistanceFromCameraByID(int id);
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
public OptionalDouble getTagPitchByID(int id);
public OptionalDouble getTagYawByID(int id);
}

4
src/main/java/frc/robot/interfaces/IVisualPoseProvider.java
View File

@ -1,9 +1,11 @@
package frc.robot.interfaces;
import java.util.Optional;
import edu.wpi.first.math.geometry.Pose2d;
public interface IVisualPoseProvider {
public record VisualPose(Pose2d visualPose, double timestamp) {}
public VisualPose getVisualPose();
public Optional<VisualPose> getVisualPose();
}

10
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -25,6 +25,7 @@ import frc.robot.constants.DrivetrainConstants;
import frc.robot.utilities.MAXSwerveModule;
import frc.robot.utilities.SwerveUtils;
import frc.robot.interfaces.IVisualPoseProvider;
import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@ -134,10 +135,11 @@ public class Drivetrain extends SubsystemBase {
);
if (m_visualPoseProvider != null) {
m_poseEstimator.addVisionMeasurement(
m_visualPoseProvider.getVisualPose().visualPose(),
m_visualPoseProvider.getVisualPose().timestamp()
);
Optional<VisualPose> vPose = m_visualPoseProvider.getVisualPose();
if (vPose.isPresent()) {
m_poseEstimator.addVisionMeasurement(vPose.get().visualPose(), vPose.get().timestamp());
}
}
}

135
src/main/java/frc/robot/utilities/PhotonVision.java Normal file
View File

@ -0,0 +1,135 @@
package frc.robot.utilities;
import java.io.IOException;
import java.util.List;
import java.util.Optional;
import java.util.OptionalDouble;
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;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import frc.robot.interfaces.IAprilTagProvider;
import frc.robot.interfaces.IVisualPoseProvider;
public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
private final PhotonCamera camera;
private final PhotonPoseEstimator photonPoseEstimator;
private final double cameraHeightMeters;
private final double cameraPitchRadians;
public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
camera = new PhotonCamera(cameraName);
photonPoseEstimator = new PhotonPoseEstimator(
AprilTagFieldLayout.loadFromResource(
AprilTagFields.k2024Crescendo.m_resourceFile
),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
camera,
robotToCam
);
this.cameraHeightMeters = cameraHeightMeters;
this.cameraPitchRadians = cameraPitchRadians;
}
@Override
public Optional<VisualPose> getVisualPose() {
Optional<EstimatedRobotPose> pose = photonPoseEstimator.update();
if (pose.isEmpty()) {
return Optional.empty();
}
return Optional.of(
new VisualPose(
pose.get().estimatedPose.toPose2d(),
pose.get().timestampSeconds
)
);
}
@Override
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
PhotonPipelineResult result = camera.getLatestResult();
if (!result.hasTargets()) {
return OptionalDouble.empty();
}
Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
if (desiredTarget.isEmpty()) {
return OptionalDouble.empty();
}
return OptionalDouble.of(
PhotonUtils.calculateDistanceToTargetMeters(
cameraHeightMeters,
targetHeightMeters,
cameraPitchRadians,
Units.degreesToRadians(desiredTarget.get().getPitch()))
);
}
@Override
public OptionalDouble getTagPitchByID(int id) {
PhotonPipelineResult result = camera.getLatestResult();
if (!result.hasTargets()) {
return OptionalDouble.empty();
}
Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
if (desiredTarget.isEmpty()) {
return OptionalDouble.empty();
}
return OptionalDouble.of(
desiredTarget.get().getPitch()
);
}
@Override
public OptionalDouble getTagYawByID(int id) {
PhotonPipelineResult result = camera.getLatestResult();
if (!result.hasTargets()) {
return OptionalDouble.empty();
}
Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
if (desiredTarget.isEmpty()) {
return OptionalDouble.empty();
}
return OptionalDouble.of(
desiredTarget.get().getYaw()
);
}
private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
for (PhotonTrackedTarget target : targets) {
if (target.getFiducialId() == id) {
return Optional.of(target);
}
}
return Optional.empty();
}
}

57
vendordeps/photonlib.json Normal file
View File

@ -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"
}
]
}