Skip to content

Commit

Permalink
Merge pull request #44 from svhsrobotics/apriltag
Browse files Browse the repository at this point in the history 
AprilTag Robot Pose calculation and Camera Switching
  • Loading branch information
@ExultantExalted
ExultantExalted authored Nov 6, 2023
2 parents d211f53 + 373c120 commit d4fefa5
Show file tree
Hide file tree
Showing 2 changed files with 143 additions and 86 deletions.
199 changes: 128 additions & 71 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ApriltagTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,9 @@
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.vision.VisionPortal;
Expand All @@ -18,102 +20,157 @@

@Autonomous(name = "AprilTag Test")
public class ApriltagTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
private VisionPortal visionPortal;
private AprilTagProcessor aprilTag;

AprilTagProcessor aprilTag = new AprilTagProcessor.Builder().build();
VisionPortal.Builder builder = new VisionPortal.Builder();
builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"));
builder.addProcessor(aprilTag);
VisionPortal visionPortal = builder.build();
/**
* The array of webcams that will be used in the pipeline
*/
private WebcamName[] webcams;


/**
* Transform the angle into the first quadrant
*/
private double quadrantTransform(double angle) {
if (angle < 0 || angle > 360) {
throw new IllegalArgumentException("Angle must be between 0 and 360");
}

waitForStart();

while (opModeIsActive()) {
//drive.setPoseEstimate(new Pose2d(0, 0, 0));
//drive.update();
List<AprilTagDetection> currentDetections = aprilTag.getDetections();
telemetry.addData("# AprilTags Detected", currentDetections.size());

for (AprilTagDetection detection : currentDetections) {
if (detection.metadata != null) {
//double theta_need = detection.ftcPose.yaw + detection.ftcPose.bearing;
//double theta_need_rad = Math.toRadians(theta_need);
telemetry.addData("Range", detection.ftcPose.range);
//telemetry.addData("Theta Need", theta_need);
telemetry.addData("Yaw", detection.ftcPose.yaw);
telemetry.addData("Bearing", detection.ftcPose.bearing);
telemetry.addData("X, Y", detection.ftcPose.x + ", " + detection.ftcPose.y);
double thetaNeed = detection.ftcPose.yaw - detection.ftcPose.bearing;
telemetry.addData("Theta Need", thetaNeed);
double a = detection.ftcPose.range * Math.cos(Math.toRadians(thetaNeed));
double b = detection.ftcPose.range * Math.sin(Math.toRadians(thetaNeed));
telemetry.addData("a", a);
telemetry.addData("b", b);
//double absolutex = detection.metadata.fieldPosition.get(0) + a; // + for lower side
//double absolutey = detection.metadata.fieldPosition.get(1) - b; // - for lower side
if (angle < 90) {
return angle;
} else if (angle < 180) {
return 180 - angle;
} else if (angle < 270) {
return angle - 180;
} else {
return 360 - angle;
}
}

//Vector2d absVector = new Vector2d(detection.metadata.fieldPosition.get(0) + a, detection.metadata.fieldPosition.get(1) - b);
//absVector.rotated(detection.metadata.fieldOrientation.toOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.RADIANS).thirdAngle);
/**
* Switch to the camera at the given index in `webcams`
* @param cameraIndex the index of the camera to switch to
*/
private void switchCamera(int cameraIndex) {
// Wait for the visionPortal status to be STREAMING before we attempt to switch cameras
android.util.Log.i("AprilTag Test", "Waiting for camera to start STREAMING before switching cameras");
while (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
sleep(10);
}
android.util.Log.i("AprilTag Test", "Camera is STREAMING, switching cameras...");
// Get the current camera
CameraName currentCamera = visionPortal.getActiveCamera();
// Find the index of the current camera in the array
int currentCameraIndex = -1;
for (int i = 0; i < webcams.length; i++) {
if (webcams[i] == currentCamera) {
currentCameraIndex = i;
break;
}
}
// If the current camera is not the one we want to switch to, switch to it
if (currentCameraIndex != cameraIndex) {
visionPortal.setActiveCamera(webcams[cameraIndex]);
// Flush any detections from the previous camera
aprilTag.getFreshDetections();
}
}

//int[] NEED_TO_ROTATE = {1,2,3,4,5,6};
List<Integer> NEED_TO_ROTATE = Arrays.asList(1,2,3,4,5,6);
private Pose2d estimateCameraPoseFromAprilTags(int camera) {
// Switch to the camera
switchCamera(camera);
// Get the detections
List<AprilTagDetection> detections = aprilTag.getFreshDetections();
// Wait for detections to not be null
// TODO: Timeout
while (detections == null) {
detections = aprilTag.getFreshDetections();
}
// If there are no detections, return null
// TODO: Multiple tries?
if (detections.size() == 0) {
return null;
}
// Get the first detection
// TODO: Pick the best detection?
AprilTagDetection detection = detections.get(0);

double thetaNeed = detection.ftcPose.yaw - detection.ftcPose.bearing;
double a = detection.ftcPose.range * Math.cos(Math.toRadians(thetaNeed));
double b = detection.ftcPose.range * Math.sin(Math.toRadians(thetaNeed));

List<Integer> NEED_TO_ROTATE = Arrays.asList(1, 2, 3, 4, 5, 6);

double absX, absY, absRot;
if (NEED_TO_ROTATE.contains(detection.id)) {
absX = detection.metadata.fieldPosition.get(0) - a; // + for lower side
absY = detection.metadata.fieldPosition.get(1) + b; // - for lower side
absRot = 180 - detection.ftcPose.yaw + 180;
} else {
absX = detection.metadata.fieldPosition.get(0) + a; // + for lower side
absY = detection.metadata.fieldPosition.get(1) - b; // - for lower side
absRot = 180 - detection.ftcPose.yaw;
}

double absX, absY, absRot;
if (NEED_TO_ROTATE.contains(detection.id)) {
absX = detection.metadata.fieldPosition.get(0) - a; // + for lower side
absY = detection.metadata.fieldPosition.get(1) + b; // - for lower side
absRot = 180-detection.ftcPose.yaw + 180;
absRot = absRot % 360;

} else {
absX = detection.metadata.fieldPosition.get(0) + a; // + for lower side
absY = detection.metadata.fieldPosition.get(1) - b; // - for lower side
absRot = 180-detection.ftcPose.yaw;
}
return new Pose2d(absX, absY, Math.toRadians(absRot));
}
@Override
public void runOpMode() throws InterruptedException {
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);

aprilTag = new AprilTagProcessor.Builder().build();

// Add the cameras to the array
webcams = new WebcamName[3];
webcams[0] = hardwareMap.get(WebcamName.class, "Webcam 1");
webcams[1] = hardwareMap.get(WebcamName.class, "Webcam 2");
webcams[2] = hardwareMap.get(WebcamName.class, "Webcam 3");

//telemetry.addData("First Angle", detection.metadata.fieldOrientation.toOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle);
//telemetry.addData("Second Angle", detection.metadata.fieldOrientation.toOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).secondAngle );
//telemetry.addData("Third Angle", detection.metadata.fieldOrientation.toOrientation(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle);
CameraName switchableCamera = ClassFactory.getInstance()
.getCameraManager().nameForSwitchableCamera(webcams);

// Create the vision portal by using a builder.
visionPortal = new VisionPortal.Builder()
.setCamera(switchableCamera)
.addProcessor(aprilTag)
.build();

android.util.Log.i("AprilTag Test", "Waiting for start...");
switchCamera(1);
waitForStart();

//telemetry.addData("position x, position y", absolutex + "," + absolutey);
while(opModeIsActive()) {
Pose2d cameraPose = estimateCameraPoseFromAprilTags(1);

if (cameraPose != null) {
android.util.Log.i("AprilTag Test", "Camera pose: " + cameraPose);

telemetry.addData("Field Centric Deg", 180 - detection.ftcPose.yaw);
double CAMERA_OFFSET = 10; // Center camera
double CAMERA_ANGLE = Math.toRadians(50);

Pose2d absPose = new Pose2d(absX, absY, Math.toRadians(absRot));
double offsetX = CAMERA_OFFSET * Math.cos(cameraPose.getHeading());
double offsetY = CAMERA_OFFSET * Math.sin(cameraPose.getHeading());

//Vector2d vec = new Vector2d(absolutex, absolutey).rotated(180);
//Pose2d poseFlipped = new Pose2d(vec.getX(),vec.getY(), Math.toRadians(-detection.ftcPose.yaw));
android.util.Log.i("AprilTag Test", "Camera offset: " + offsetX + ", " + offsetY);

drive.setPoseEstimate(absPose);
//drive.update();
double robotX = cameraPose.getX() - offsetX;
double robotY = cameraPose.getY() - offsetY;
double robotHeading = cameraPose.getHeading() - CAMERA_ANGLE;

Pose2d robotPose = new Pose2d(robotX, robotY, robotHeading);

//telemetry.addData("Relative X", detection.ftcPose.range * Math.cos(Math.toRadians(detection.ftcPose.yaw)));
//telemetry.addData("Relative Y", detection.ftcPose.range * Math.sin(Math.toRadians(detection.ftcPose.yaw)));
android.util.Log.i("AprilTag Test", "Robot pose: " + robotPose);

//Orientation.getOrientation()
//telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
//telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
drive.setPoseEstimate(robotPose);

//telemetry.addLine(String.format("Field XYZ %6.1f %6.1f %6.1f (inch)", detection.metadata.fieldPosition.get(0), detection.metadata.fieldPosition.get(1), detection.metadata.fieldPosition.get(2)));
}
//telemetry.update();
drive.update();
}

drive.update();

}

visionPortal.close();

}
}
}
30 changes: 15 additions & 15 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RedLeftAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,30 +21,30 @@ public void runOpMode() throws InterruptedException {
TrajectorySequence autoDrive = drive.trajectorySequenceBuilder(startPose)

//relativistic driving that also includes turning to simulate april tags recognition
.forward(24)
.waitSeconds(1)
.turn(Math.toRadians(90))
.waitSeconds(3)
.turn(Math.toRadians(-180))
.forward(80)
.build();
// .forward(24)
// .waitSeconds(1)
// .turn(Math.toRadians(90))
// .waitSeconds(3)
// .turn(Math.toRadians(-180))
// .forward(80)
// .build();





/*
//coordinate based driving
.lineTo(new Vector2d(-34, -36))
.strafeTo(new Vector2d(48, -36))
.build();
*/
/*


// also coordinate but spline and goes to corner instead of right in front of board
.splineTo(new Vector2d(-34, -30), Math.toRadians(90))
.splineTo(new Vector2d(34, -30), Math.toRadians(-90))
.splineTo(new Vector2d(56, -56),Math.toRadians(0) )
.build();
*/
// .splineTo(new Vector2d(-34, -30), Math.toRadians(90))
// .splineTo(new Vector2d(34, -30), Math.toRadians(-90))
// .splineTo(new Vector2d(56, -56),Math.toRadians(0) )
// .build();
//

waitForStart();

Expand Down

0 comments on commit d4fefa5

Please sign in to comment.