195 lines
9.7 KiB
Java
195 lines
9.7 KiB
Java
/* Copyright (c) 2017 FIRST. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without modification,
|
|
* are permitted (subject to the limitations in the disclaimer below) provided that
|
|
* the following conditions are met:
|
|
*
|
|
* Redistributions of source code must retain the above copyright notice, this list
|
|
* of conditions and the following disclaimer.
|
|
*
|
|
* Redistributions in binary form must reproduce the above copyright notice, this
|
|
* list of conditions and the following disclaimer in the documentation and/or
|
|
* other materials provided with the distribution.
|
|
*
|
|
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
|
|
* promote products derived from this software without specific prior written permission.
|
|
*
|
|
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
|
|
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
|
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
|
|
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
|
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
package org.firstinspires.ftc.robotcontroller.external.samples;
|
|
|
|
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
|
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
|
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
|
|
|
import org.firstinspires.ftc.robotcore.external.ClassFactory;
|
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
|
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
|
|
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuMarkInstanceId;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
|
|
|
|
/**
|
|
* This OpMode illustrates the basics of using the Vuforia engine to determine
|
|
* the identity of Vuforia VuMarks encountered on the field. The code is structured as
|
|
* a LinearOpMode. It shares much structure with {@link ConceptVuforiaFieldNavigationWebcam}; we do not here
|
|
* duplicate the core Vuforia documentation found there, but rather instead focus on the
|
|
* differences between the use of Vuforia for navigation vs VuMark identification.
|
|
*
|
|
* @see ConceptVuforiaFieldNavigationWebcam
|
|
* @see VuforiaLocalizer
|
|
* @see VuforiaTrackableDefaultListener
|
|
* see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf
|
|
*
|
|
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
|
|
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
|
|
*
|
|
* IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
|
|
* is explained below
|
|
*/
|
|
|
|
@TeleOp(name="Concept: VuMark Id Webcam", group ="Concept")
|
|
@Disabled
|
|
public class ConceptVuMarkIdentificationWebcam extends LinearOpMode {
|
|
|
|
public static final String TAG = "Vuforia VuMark Sample";
|
|
|
|
OpenGLMatrix lastLocation = null;
|
|
|
|
/**
|
|
* {@link #vuforia} is the variable we will use to store our instance of the Vuforia
|
|
* localization engine.
|
|
*/
|
|
VuforiaLocalizer vuforia;
|
|
|
|
/**
|
|
* This is the webcam we are to use. As with other hardware devices such as motors and
|
|
* servos, this device is identified using the robot configuration tool in the FTC application.
|
|
*/
|
|
WebcamName webcamName;
|
|
|
|
@Override public void runOpMode() {
|
|
|
|
/*
|
|
* Retrieve the camera we are to use.
|
|
*/
|
|
webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
|
|
|
|
/*
|
|
* To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
|
|
* If no camera monitor is desired, use the parameterless constructor instead (commented out below).
|
|
*/
|
|
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
|
|
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
|
|
|
|
// OR... Do Not Activate the Camera Monitor View, to save power
|
|
// VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
|
|
|
|
/*
|
|
* IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
|
|
* 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
|
|
* A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
|
|
* web site at https://developer.vuforia.com/license-manager.
|
|
*
|
|
* Vuforia license keys are always 380 characters long, and look as if they contain mostly
|
|
* random data. As an example, here is a example of a fragment of a valid key:
|
|
* ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
|
|
* Once you've obtained a license key, copy the string from the Vuforia web site
|
|
* and paste it in to your code on the next line, between the double quotes.
|
|
*/
|
|
parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
|
|
|
|
|
|
/**
|
|
* We also indicate which camera on the RC we wish to use. For pedagogical purposes,
|
|
* we use the same logic as in {@link ConceptVuforiaNavigationWebcam}.
|
|
*/
|
|
parameters.cameraName = webcamName;
|
|
this.vuforia = ClassFactory.getInstance().createVuforia(parameters);
|
|
|
|
/**
|
|
* Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
|
|
* in this data set: all three of the VuMarks in the game were created from this one template,
|
|
* but differ in their instance id information.
|
|
* @see VuMarkInstanceId
|
|
*/
|
|
VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
|
|
VuforiaTrackable relicTemplate = relicTrackables.get(0);
|
|
relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary
|
|
|
|
telemetry.addData(">", "Press Play to start");
|
|
telemetry.update();
|
|
waitForStart();
|
|
|
|
relicTrackables.activate();
|
|
|
|
while (opModeIsActive()) {
|
|
|
|
/**
|
|
* See if any of the instances of {@link relicTemplate} are currently visible.
|
|
* {@link RelicRecoveryVuMark} is an enum which can have the following values:
|
|
* UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
|
|
* UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
|
|
*/
|
|
RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
|
|
if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
|
|
|
|
/* Found an instance of the template. In the actual game, you will probably
|
|
* loop until this condition occurs, then move on to act accordingly depending
|
|
* on which VuMark was visible. */
|
|
telemetry.addData("VuMark", "%s visible", vuMark);
|
|
|
|
/* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
|
|
* it is perhaps unlikely that you will actually need to act on this pose information, but
|
|
* we illustrate it nevertheless, for completeness. */
|
|
OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicTemplate.getListener()).getFtcCameraFromTarget();
|
|
telemetry.addData("Pose", format(pose));
|
|
|
|
/* We further illustrate how to decompose the pose into useful rotational and
|
|
* translational components */
|
|
if (pose != null) {
|
|
VectorF trans = pose.getTranslation();
|
|
Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
|
|
|
|
// Extract the X, Y, and Z components of the offset of the target relative to the robot
|
|
double tX = trans.get(0);
|
|
double tY = trans.get(1);
|
|
double tZ = trans.get(2);
|
|
|
|
// Extract the rotational components of the target relative to the robot
|
|
double rX = rot.firstAngle;
|
|
double rY = rot.secondAngle;
|
|
double rZ = rot.thirdAngle;
|
|
}
|
|
}
|
|
else {
|
|
telemetry.addData("VuMark", "not visible");
|
|
}
|
|
|
|
telemetry.update();
|
|
}
|
|
}
|
|
|
|
String format(OpenGLMatrix transformationMatrix) {
|
|
return (transformationMatrix != null) ? transformationMatrix.formatAsTransform() : "null";
|
|
}
|
|
}
|