|
| 1 | +/* Copyright (c) 2024 Dryw Wade. 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 | + |
| 36 | +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; |
| 37 | +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; |
| 38 | +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; |
| 39 | +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; |
| 40 | +import org.firstinspires.ftc.robotcore.external.navigation.Position; |
| 41 | +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; |
| 42 | +import org.firstinspires.ftc.vision.VisionPortal; |
| 43 | +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; |
| 44 | +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; |
| 45 | + |
| 46 | +import java.util.List; |
| 47 | + |
| 48 | +/* |
| 49 | + * This OpMode illustrates the basics of AprilTag based localization. |
| 50 | + * |
| 51 | + * For an introduction to AprilTags, see the FTC-DOCS link below: |
| 52 | + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html |
| 53 | + * |
| 54 | + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default |
| 55 | + * "TagLibrary" will be used to compute the robot's location and orientation. This default TagLibrary contains |
| 56 | + * the current Season's AprilTags and a small set of "test Tags" in the high number range. |
| 57 | + * |
| 58 | + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the robot, relative to the field origin. |
| 59 | + * This information is provided in the "robotPose" member of the returned "detection". |
| 60 | + * |
| 61 | + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. |
| 62 | + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. |
| 63 | + */ |
| 64 | +@TeleOp(name = "Concept: AprilTag Localization", group = "Concept") |
| 65 | +@Disabled |
| 66 | +public class ConceptAprilTagLocalization extends LinearOpMode { |
| 67 | + |
| 68 | + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera |
| 69 | + |
| 70 | + /** |
| 71 | + * Variables to store the position and orientation of the camera on the robot. Setting these |
| 72 | + * values requires a definition of the axes of the camera and robot: |
| 73 | + * |
| 74 | + * Camera axes: |
| 75 | + * Origin location: Center of the lens |
| 76 | + * Axes orientation: +x right, +y down, +z forward (from camera's perspective) |
| 77 | + * |
| 78 | + * Robot axes (this is typical, but you can define this however you want): |
| 79 | + * Origin location: Center of the robot at field height |
| 80 | + * Axes orientation: +x right, +y forward, +z upward |
| 81 | + * |
| 82 | + * Position: |
| 83 | + * If all values are zero (no translation), that implies the camera is at the center of the |
| 84 | + * robot. Suppose your camera is positioned 5 inches to the left, 7 inches forward, and 12 |
| 85 | + * inches above the ground - you would need to set the position to (-5, 7, 12). |
| 86 | + * |
| 87 | + * Orientation: |
| 88 | + * If all values are zero (no rotation), that implies the camera is pointing straight up. In |
| 89 | + * most cases, you'll need to set the pitch to -90 degrees (rotation about the x-axis), meaning |
| 90 | + * the camera is horizontal. Use a yaw of 0 if the camera is pointing forwards, +90 degrees if |
| 91 | + * it's pointing straight left, -90 degrees for straight right, etc. You can also set the roll |
| 92 | + * to +/-90 degrees if it's vertical, or 180 degrees if it's upside-down. |
| 93 | + */ |
| 94 | + private Position cameraPosition = new Position(DistanceUnit.INCH, |
| 95 | + 0, 0, 0, 0); |
| 96 | + private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, |
| 97 | + 0, -90, 0, 0); |
| 98 | + |
| 99 | + /** |
| 100 | + * The variable to store our instance of the AprilTag processor. |
| 101 | + */ |
| 102 | + private AprilTagProcessor aprilTag; |
| 103 | + |
| 104 | + /** |
| 105 | + * The variable to store our instance of the vision portal. |
| 106 | + */ |
| 107 | + private VisionPortal visionPortal; |
| 108 | + |
| 109 | + @Override |
| 110 | + public void runOpMode() { |
| 111 | + |
| 112 | + initAprilTag(); |
| 113 | + |
| 114 | + // Wait for the DS start button to be touched. |
| 115 | + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); |
| 116 | + telemetry.addData(">", "Touch START to start OpMode"); |
| 117 | + telemetry.update(); |
| 118 | + waitForStart(); |
| 119 | + |
| 120 | + while (opModeIsActive()) { |
| 121 | + |
| 122 | + telemetryAprilTag(); |
| 123 | + |
| 124 | + // Push telemetry to the Driver Station. |
| 125 | + telemetry.update(); |
| 126 | + |
| 127 | + // Save CPU resources; can resume streaming when needed. |
| 128 | + if (gamepad1.dpad_down) { |
| 129 | + visionPortal.stopStreaming(); |
| 130 | + } else if (gamepad1.dpad_up) { |
| 131 | + visionPortal.resumeStreaming(); |
| 132 | + } |
| 133 | + |
| 134 | + // Share the CPU. |
| 135 | + sleep(20); |
| 136 | + } |
| 137 | + |
| 138 | + // Save more CPU resources when camera is no longer needed. |
| 139 | + visionPortal.close(); |
| 140 | + |
| 141 | + } // end method runOpMode() |
| 142 | + |
| 143 | + /** |
| 144 | + * Initialize the AprilTag processor. |
| 145 | + */ |
| 146 | + private void initAprilTag() { |
| 147 | + |
| 148 | + // Create the AprilTag processor. |
| 149 | + aprilTag = new AprilTagProcessor.Builder() |
| 150 | + |
| 151 | + // The following default settings are available to un-comment and edit as needed. |
| 152 | + //.setDrawAxes(false) |
| 153 | + //.setDrawCubeProjection(false) |
| 154 | + //.setDrawTagOutline(true) |
| 155 | + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) |
| 156 | + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) |
| 157 | + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) |
| 158 | + .setCameraPose(cameraPosition, cameraOrientation) |
| 159 | + |
| 160 | + // == CAMERA CALIBRATION == |
| 161 | + // If you do not manually specify calibration parameters, the SDK will attempt |
| 162 | + // to load a predefined calibration for your camera. |
| 163 | + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) |
| 164 | + // ... these parameters are fx, fy, cx, cy. |
| 165 | + |
| 166 | + .build(); |
| 167 | + |
| 168 | + // Adjust Image Decimation to trade-off detection-range for detection-rate. |
| 169 | + // eg: Some typical detection data using a Logitech C920 WebCam |
| 170 | + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second |
| 171 | + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second |
| 172 | + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) |
| 173 | + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) |
| 174 | + // Note: Decimation can be changed on-the-fly to adapt during a match. |
| 175 | + //aprilTag.setDecimation(3); |
| 176 | + |
| 177 | + // Create the vision portal by using a builder. |
| 178 | + VisionPortal.Builder builder = new VisionPortal.Builder(); |
| 179 | + |
| 180 | + // Set the camera (webcam vs. built-in RC phone camera). |
| 181 | + if (USE_WEBCAM) { |
| 182 | + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); |
| 183 | + } else { |
| 184 | + builder.setCamera(BuiltinCameraDirection.BACK); |
| 185 | + } |
| 186 | + |
| 187 | + // Choose a camera resolution. Not all cameras support all resolutions. |
| 188 | + //builder.setCameraResolution(new Size(640, 480)); |
| 189 | + |
| 190 | + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. |
| 191 | + //builder.enableLiveView(true); |
| 192 | + |
| 193 | + // Set the stream format; MJPEG uses less bandwidth than default YUY2. |
| 194 | + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); |
| 195 | + |
| 196 | + // Choose whether or not LiveView stops if no processors are enabled. |
| 197 | + // If set "true", monitor shows solid orange screen if no processors enabled. |
| 198 | + // If set "false", monitor shows camera view without annotations. |
| 199 | + //builder.setAutoStopLiveView(false); |
| 200 | + |
| 201 | + // Set and enable the processor. |
| 202 | + builder.addProcessor(aprilTag); |
| 203 | + |
| 204 | + // Build the Vision Portal, using the above settings. |
| 205 | + visionPortal = builder.build(); |
| 206 | + |
| 207 | + // Disable or re-enable the aprilTag processor at any time. |
| 208 | + //visionPortal.setProcessorEnabled(aprilTag, true); |
| 209 | + |
| 210 | + } // end method initAprilTag() |
| 211 | + |
| 212 | + /** |
| 213 | + * Add telemetry about AprilTag detections. |
| 214 | + */ |
| 215 | + private void telemetryAprilTag() { |
| 216 | + |
| 217 | + List<AprilTagDetection> currentDetections = aprilTag.getDetections(); |
| 218 | + telemetry.addData("# AprilTags Detected", currentDetections.size()); |
| 219 | + |
| 220 | + // Step through the list of detections and display info for each one. |
| 221 | + for (AprilTagDetection detection : currentDetections) { |
| 222 | + if (detection.metadata != null) { |
| 223 | + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); |
| 224 | + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", |
| 225 | + detection.robotPose.getPosition().x, |
| 226 | + detection.robotPose.getPosition().y, |
| 227 | + detection.robotPose.getPosition().z)); |
| 228 | + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", |
| 229 | + detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), |
| 230 | + detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), |
| 231 | + detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); |
| 232 | + } else { |
| 233 | + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); |
| 234 | + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); |
| 235 | + } |
| 236 | + } // end for() loop |
| 237 | + |
| 238 | + // Add "key" information to telemetry |
| 239 | + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); |
| 240 | + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); |
| 241 | + |
| 242 | + } // end method telemetryAprilTag() |
| 243 | + |
| 244 | +} // end class |
0 commit comments