|
| 1 | +/* Copyright (c) 2023 FIRST. All rights reserved. |
| 2 | + * |
| 3 | + * Redistribution and use in source and binary forms, with or without modification, |
| 4 | + * are permitted (subject to the limitations in the disclaimer below) provided that |
| 5 | + * the following conditions are met: |
| 6 | + * |
| 7 | + * Redistributions of source code must retain the above copyright notice, this list |
| 8 | + * of conditions and the following disclaimer. |
| 9 | + * |
| 10 | + * Redistributions in binary form must reproduce the above copyright notice, this |
| 11 | + * list of conditions and the following disclaimer in the documentation and/or |
| 12 | + * other materials provided with the distribution. |
| 13 | + * |
| 14 | + * Neither the name of FIRST nor the names of its contributors may be used to endorse or |
| 15 | + * promote products derived from this software without specific prior written permission. |
| 16 | + * |
| 17 | + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS |
| 18 | + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 19 | + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, |
| 20 | + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 21 | + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE |
| 22 | + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
| 23 | + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
| 24 | + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 25 | + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
| 26 | + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
| 27 | + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| 28 | + */ |
| 29 | + |
| 30 | +package org.firstinspires.ftc.robotcontroller.external.samples; |
| 31 | + |
| 32 | +import com.qualcomm.robotcore.eventloop.opmode.Disabled; |
| 33 | +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; |
| 34 | +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; |
| 35 | +import java.util.List; |
| 36 | +import android.util.Size; |
| 37 | +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; |
| 38 | +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; |
| 39 | +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; |
| 40 | +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; |
| 41 | +import org.firstinspires.ftc.vision.VisionPortal; |
| 42 | +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; |
| 43 | +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; |
| 44 | +import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; |
| 45 | + |
| 46 | +/** |
| 47 | + * This 2023-2024 OpMode illustrates the basics of AprilTag recognition and pose estimation, |
| 48 | + * including Java Builder structures for specifying Vision parameters. |
| 49 | + * |
| 50 | + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. |
| 51 | + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. |
| 52 | + */ |
| 53 | +@TeleOp(name = "Concept: AprilTag", group = "Concept") |
| 54 | +@Disabled |
| 55 | +public class ConceptAprilTag extends LinearOpMode { |
| 56 | + |
| 57 | + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera |
| 58 | + |
| 59 | + /** |
| 60 | + * {@link #aprilTag} is the variable to store our instance of the AprilTag processor. |
| 61 | + */ |
| 62 | + private AprilTagProcessor aprilTag; |
| 63 | + |
| 64 | + /** |
| 65 | + * {@link #visionPortal} is the variable to store our instance of the vision portal. |
| 66 | + */ |
| 67 | + private VisionPortal visionPortal; |
| 68 | + |
| 69 | + @Override |
| 70 | + public void runOpMode() { |
| 71 | + |
| 72 | + initAprilTag(); |
| 73 | + |
| 74 | + // Wait for the DS start button to be touched. |
| 75 | + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); |
| 76 | + telemetry.addData(">", "Touch Play to start OpMode"); |
| 77 | + telemetry.update(); |
| 78 | + waitForStart(); |
| 79 | + |
| 80 | + if (opModeIsActive()) { |
| 81 | + while (opModeIsActive()) { |
| 82 | + |
| 83 | + telemetryAprilTag(); |
| 84 | + |
| 85 | + // Push telemetry to the Driver Station. |
| 86 | + telemetry.update(); |
| 87 | + |
| 88 | + // Save CPU resources; can resume streaming when needed. |
| 89 | + if (gamepad1.dpad_down) { |
| 90 | + visionPortal.stopStreaming(); |
| 91 | + } else if (gamepad1.dpad_up) { |
| 92 | + visionPortal.resumeStreaming(); |
| 93 | + } |
| 94 | + |
| 95 | + // Share the CPU. |
| 96 | + sleep(20); |
| 97 | + } |
| 98 | + } |
| 99 | + |
| 100 | + // Save more CPU resources when camera is no longer needed. |
| 101 | + visionPortal.close(); |
| 102 | + |
| 103 | + } // end method runOpMode() |
| 104 | + |
| 105 | + /** |
| 106 | + * Initialize the AprilTag processor. |
| 107 | + */ |
| 108 | + private void initAprilTag() { |
| 109 | + |
| 110 | + // Create the AprilTag processor. |
| 111 | + aprilTag = new AprilTagProcessor.Builder() |
| 112 | + //.setDrawAxes(false) |
| 113 | + //.setDrawCubeProjection(false) |
| 114 | + //.setDrawTagOutline(true) |
| 115 | + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) |
| 116 | + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) |
| 117 | + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) |
| 118 | + |
| 119 | + // == CAMERA CALIBRATION == |
| 120 | + // If you do not manually specify calibration parameters, the SDK will attempt |
| 121 | + // to load a predefined calibration for your camera. |
| 122 | + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) |
| 123 | + |
| 124 | + // ... these parameters are fx, fy, cx, cy. |
| 125 | + |
| 126 | + .build(); |
| 127 | + |
| 128 | + // Create the vision portal by using a builder. |
| 129 | + VisionPortal.Builder builder = new VisionPortal.Builder(); |
| 130 | + |
| 131 | + // Set the camera (webcam vs. built-in RC phone camera). |
| 132 | + if (USE_WEBCAM) { |
| 133 | + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); |
| 134 | + } else { |
| 135 | + builder.setCamera(BuiltinCameraDirection.BACK); |
| 136 | + } |
| 137 | + |
| 138 | + // Choose a camera resolution. Not all cameras support all resolutions. |
| 139 | + //builder.setCameraResolution(new Size(640, 480)); |
| 140 | + |
| 141 | + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. |
| 142 | + //builder.enableCameraMonitoring(true); |
| 143 | + |
| 144 | + // Set the stream format; MJPEG uses less bandwidth than default YUY2. |
| 145 | + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); |
| 146 | + |
| 147 | + // Choose whether or not LiveView stops if no processors are enabled. |
| 148 | + // If set "true", monitor shows solid orange screen if no processors enabled. |
| 149 | + // If set "false", monitor shows camera view without annotations. |
| 150 | + //builder.setAutoStopLiveView(false); |
| 151 | + |
| 152 | + // Set and enable the processor. |
| 153 | + builder.addProcessor(aprilTag); |
| 154 | + |
| 155 | + // Build the Vision Portal, using the above settings. |
| 156 | + visionPortal = builder.build(); |
| 157 | + |
| 158 | + // Disable or re-enable the aprilTag processor at any time. |
| 159 | + //visionPortal.setProcessorEnabled(aprilTag, true); |
| 160 | + |
| 161 | + } // end method initAprilTag() |
| 162 | + |
| 163 | + |
| 164 | + /** |
| 165 | + * Function to add telemetry about AprilTag detections. |
| 166 | + */ |
| 167 | + private void telemetryAprilTag() { |
| 168 | + |
| 169 | + List<AprilTagDetection> currentDetections = aprilTag.getDetections(); |
| 170 | + telemetry.addData("# AprilTags Detected", currentDetections.size()); |
| 171 | + |
| 172 | + // Step through the list of detections and display info for each one. |
| 173 | + for (AprilTagDetection detection : currentDetections) { |
| 174 | + if (detection.metadata != null) { |
| 175 | + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); |
| 176 | + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); |
| 177 | + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); |
| 178 | + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); |
| 179 | + } else { |
| 180 | + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); |
| 181 | + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); |
| 182 | + } |
| 183 | + } // end for() loop |
| 184 | + |
| 185 | + // Add "key" information to telemetry |
| 186 | + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); |
| 187 | + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); |
| 188 | + telemetry.addLine("RBE = Range, Bearing & Elevation"); |
| 189 | + |
| 190 | + } // end method telemetryAprilTag() |
| 191 | + |
| 192 | +} // end class |
0 commit comments