Removed an unnecessary method from IAprilTagProvider, and modified some stuff that used it so that things make more sense
This commit is contained in:
parent
477f066585
commit
dd39baf182
17
.OutlineViewer/outlineviewer.json
Normal file
17
.OutlineViewer/outlineviewer.json
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
{
|
||||||
|
"Connections": {
|
||||||
|
"open": true
|
||||||
|
},
|
||||||
|
"NetworkTables Settings": {
|
||||||
|
"mode": "Server"
|
||||||
|
},
|
||||||
|
"Server": {
|
||||||
|
"Publishers": {
|
||||||
|
"open": true
|
||||||
|
},
|
||||||
|
"Subscribers": {
|
||||||
|
"open": true
|
||||||
|
},
|
||||||
|
"open": true
|
||||||
|
}
|
||||||
|
}
|
@ -7,20 +7,6 @@ import java.util.OptionalDouble;
|
|||||||
* information from various sources in a consistent way.
|
* information from various sources in a consistent way.
|
||||||
*/
|
*/
|
||||||
public interface IAprilTagProvider {
|
public interface IAprilTagProvider {
|
||||||
/**
|
|
||||||
* A method to get the AprilTag offset from the center of the image. This is intended to be used with
|
|
||||||
* a PIDController, the PIDController keeps the robot rotated such that the AprilTag stays in the horizontal
|
|
||||||
* center of the image.
|
|
||||||
*
|
|
||||||
* TODO Is there some other way of ensuring an AprilTag is in the center of the image frame without knowing the horizontal resolution?
|
|
||||||
* TODO Certain providers don't give resolution information natively. Which makes me think there's some other way to do this.
|
|
||||||
*
|
|
||||||
* @param id The ID of the AprilTag to give an offset from
|
|
||||||
* @param cameraHalfXRes Half of the horizontal resolution of the camera sensor
|
|
||||||
* @return A double with the number of pixels offset from center the tag is, or OptionalDouble.empty() if the tag is not present in the camera's view
|
|
||||||
*/
|
|
||||||
public OptionalDouble getTagOffsetFromCameraCenterByID(int id, double cameraHalfXRes);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A method to get the distance from <i>the camera</i> to the AprilTag specified
|
* A method to get the distance from <i>the camera</i> to the AprilTag specified
|
||||||
*
|
*
|
||||||
@ -31,18 +17,18 @@ public interface IAprilTagProvider {
|
|||||||
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
|
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A method to get the pitch of a particular AprilTag
|
* A method to get the pitch from the center of the image of a particular AprilTag
|
||||||
*
|
*
|
||||||
* @param id The ID of the AprilTag to get the pitch of
|
* @param id The ID of the AprilTag to get the pitch of
|
||||||
* @return The pitch, in radians, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
|
* @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
|
||||||
*/
|
*/
|
||||||
public OptionalDouble getTagPitchByID(int id);
|
public OptionalDouble getTagPitchByID(int id);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A method to get the yaw of a particular AprilTag
|
* A method to get the yaw from the center of the image of a particular AprilTag
|
||||||
*
|
*
|
||||||
* @param id The ID of the AprilTag to get the yaw of
|
* @param id The ID of the AprilTag to get the yaw of
|
||||||
* @return The yaw, in radians, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
|
* @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
|
||||||
*/
|
*/
|
||||||
public OptionalDouble getTagYawByID(int id);
|
public OptionalDouble getTagYawByID(int id);
|
||||||
}
|
}
|
||||||
|
@ -330,13 +330,13 @@ public class Drivetrain extends SubsystemBase {
|
|||||||
return new PIDCommand(
|
return new PIDCommand(
|
||||||
controller,
|
controller,
|
||||||
() -> {
|
() -> {
|
||||||
OptionalDouble tagOffset = m_aprilTagProvider.getTagOffsetFromCameraCenterByID(tagID, cameraHorizontalResolution);
|
OptionalDouble tagYaw = m_aprilTagProvider.getTagYawByID(tagID);
|
||||||
|
|
||||||
if (tagOffset.isEmpty()) {
|
if (tagYaw.isEmpty()) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
return tagOffset.getAsDouble();
|
return tagYaw.getAsDouble();
|
||||||
},
|
},
|
||||||
() -> { return 0; },
|
() -> { return 0; },
|
||||||
(output) -> {
|
(output) -> {
|
||||||
|
@ -78,27 +78,6 @@ public class PhotonVision implements ICamera,IAprilTagProvider,IVisualPoseProvid
|
|||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
|
||||||
public OptionalDouble getTagOffsetFromCameraCenterByID(int id, double cameraHalfXRes) {
|
|
||||||
PhotonPipelineResult result = camera.getLatestResult();
|
|
||||||
|
|
||||||
if (!result.hasTargets()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
|
|
||||||
|
|
||||||
if (desiredTarget.isEmpty()) {
|
|
||||||
return OptionalDouble.empty();
|
|
||||||
}
|
|
||||||
|
|
||||||
List<TargetCorner> corners = desiredTarget.get().getDetectedCorners();
|
|
||||||
|
|
||||||
return OptionalDouble.of(
|
|
||||||
corners.get(2).x - corners.get(3).x - cameraHalfXRes
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
|
public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
|
||||||
PhotonPipelineResult result = camera.getLatestResult();
|
PhotonPipelineResult result = camera.getLatestResult();
|
||||||
|
Loading…
Reference in New Issue
Block a user