package org.firstinspires.ftc.teamcode;
import android.graphics.Bitmap;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.HardwareMap;
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;
import java.util.concurrent.BlockingQueue;
/**
* This OpMode illustrates the basics of using the Vuforia engine to determine
* the identity of Vuforia VuMarks encountered on the field. The VuForia function
* is packaged as utility class within the main opMmode class (inner class). The findVuMark class
* is generic usable for any single VuMark. It could be moved out of this example to a separate
* class or a library class.
*
* IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
* is explained in FIRST sample code: ConceptVuforiaNavigation.
*
* VuMark is like a bar code. It is an image that contains encoded variable information. For the
* Relic Recovery game, the VuMark is the image of a temple. Encoded on that image in hexagonal
* dots is a code indicating left, center and right. Vuforia is used to locate the image in the
* camera field of view and extract the code returning that to your program. FIRST included a
* custom enum class to display the code (also called an instance id) as text.
*
* VuMarks are defined by two data files created by the Vuforia Target Manager. In our case, those
* files are provided by FIRST. The files are embedded in the robot controller program by putting
* them in the assets directory of FtcRobotController section of the project.
*
* You can capture VuMarks with the robot controller phone camera or with USB webcam attached to
* a Control Hub.
*
* This version also streams images from the camera device over the network connection for viewing
* in a browser. See VuforiaCapture.java and MJpegServer.hava for more informmation.
*
* 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. The license key needs
* to support external cameras to use webcams.
*/
@Autonomous(name="VuMark Id - Streaming", group ="Exercises")
//@Disabled
public class VuMarkIdentificationStreaming extends LinearOpMode
{
VuMarkFinder vmf;
RelicRecoveryVuMark vuMark;
VuforiaCapture vuforiaCapture;
MJpegServer server;
@Override
public void runOpMode() throws InterruptedException
{
Bitmap cameraImage;
// Create an instance of VuMarkFinder using RC phone camera.
// Here we chose the back (HiRes) camera (for greater range), but
// for a competition robot, the front camera might be more convenient.
//vmf = new VuMarkFinder(hardwareMap, "RelicVuMark", VuforiaLocalizer.CameraDirection.BACK, true);
// Create an instance of VuMarkFinder using USB webcam.
vmf = new VuMarkFinder(hardwareMap, "RelicVuMark", "Webcam 1", true);
// Create instances of the Vuforia camera capture and image streaming server.
vuforiaCapture = new VuforiaCapture(vmf.getLocalizer());
server = new MJpegServer();
telemetry.addData("Mode", "Press Play to start");
telemetry.update();
// Start VuForia background process looking for vumarks in camera field of view. Activate
// before waitForStart() allows you to see camera stream on DS at INIT wait. See DS menu.
vmf.activate();
waitForStart();
vuforiaCapture.startCapture();
while (opModeIsActive())
{
// See if a VuMark is currently visible.
if (vmf.findVuMark())
{
// Convert vumark instance id to game specific id.
vuMark = RelicRecoveryVuMark.from(vmf.instanceId);
telemetry.addData("VuMark", "%s visible", vuMark);
//telemetry.addData("Pose", vmf.formatPose(vmf.pose));
telemetry.addData("X Y Z", "X=%f Y=%f Z=%f", vmf.tX, vmf.tY, vmf.tZ);
}
else
telemetry.addData("VuMark", "not visible");
telemetry.update();
// Get a camera image fromm Vuforia and post it to the streaming server.
cameraImage = vuforiaCapture.getImage();
server.setImage(cameraImage);
idle();
}
server.stop();
}
/**
* VuForia VuMark finder class.
*/
public class VuMarkFinder
{
private VuforiaLocalizer vuforia;
private VuforiaTrackables trackables;
private VuforiaTrackable template;
private VuforiaLocalizer.Parameters parameters;
private BlockingQueue<VuforiaLocalizer.CloseableFrame> frameQueue;
public VuMarkInstanceId instanceId;
public OpenGLMatrix pose;
public double tX, tY, tZ, rX, rY, rZ;
/** Constructor for using RC phone camera.
* Create an instance of the class.
* @param hMap HardwareMap object.
* @param assetName Name of the asset files containing the VuMark definition.
* @param includeViewer True to display camera viewer on RC phone.
* @param camera Front or Back RC phone camera choice.
*/
public VuMarkFinder(HardwareMap hMap,
String assetName,
VuforiaLocalizer.CameraDirection camera,
boolean includeViewer)
{
/*
* 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 .
*/
if (includeViewer)
{
int cameraMonitorViewId = hMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hMap.appContext.getPackageName());
parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
}
else
// OR... Do Not Activate the Camera Monitor View, to save power
parameters = new VuforiaLocalizer.Parameters();
/*
* We also indicate which camera on the RC that we wish to use.
*/
parameters.cameraDirection = camera;
parameters.useExtendedTracking = false;
initializeVuforia(assetName);
}
/** Constructor for using webcam on Control Hub.
* Create an instance of the class.
* @param hMap HardwareMap object.
* @param assetName Name of the asset files containing the VuMark definition.
* @param cameraName Name of webcam as configured on Control Hub.
* @param includeViewer True to display camera viewer on DS phone.
*/
public VuMarkFinder(HardwareMap hMap,
String assetName,
String cameraName,
boolean includeViewer)
{
/*
* 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 .
*/
if (includeViewer)
{
int cameraMonitorViewId = hMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hMap.appContext.getPackageName());
parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
}
else
// OR... Do Not Activate the Camera Monitor View, to save power
parameters = new VuforiaLocalizer.Parameters();
/*
* Retrieve and set the USB web camera we are to use.
*/
parameters.cameraName = hMap.get(WebcamName.class, cameraName);
parameters.useExtendedTracking = false;
initializeVuforia(assetName);
}
private void initializeVuforia(String assetName)
{
parameters.vuforiaLicenseKey = "AaDAvEH/////AAABmT7XsefufE2DuYYnYCxrBsQQ9FrK/39uullPQn7b/XVUOAU9eFLRcRYYm1JY0ChQpml/x1CPv5kyBtc5rwVrTM0I2/VcBKiulYWzGM8kZDYAIwIwpncnYbyxCHgN80KAZplNqMiL0lWP1SKFE1jXojLSu33a+gcyDvRQCJtHteF976mcXTsadxZCJFhUGx198hOmuK5HNTwjvNoxcEUmF5BOS9hLDBCrZnfTHYbYyKoKMX17a3K7FR+T8C8s+zOGvKXc9vtjNTJDUZ0D1gvyDSlzu52fHXAVTb7HRJN9rapGZ6wyqn2UGay5dxjCOFsxsWZFSzPRN3zZri//WalFQysr6MWUEykjDJhGKZucvBHr";
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.
*/
trackables = vuforia.loadTrackablesFromAsset(assetName);
template = trackables.get(0);
template.setName(assetName); // can help in debugging; otherwise not necessary
}
/**
* Activate VuForia image processing. Call after waitForStart().
*/
public void activate()
{
trackables.activate();
}
/**
* Get the VuforiaLocalizer used by this class.
* @return Localizer.
*/
public VuforiaLocalizer getLocalizer()
{
return vuforia;
}
/**
* Call to find out if VuMark is visible to the phone camera.
* @return True if VuMark found, false if not.
*/
public boolean findVuMark()
{
// See if any instances of the template are currently visible.
instanceId = ((VuforiaTrackableDefaultListener) template.getListener()).getVuMarkInstanceId();
if (instanceId != null)
{
// Get and display pose information, that is, vumark location relative to camera
// center of view.
if (vuforia.getCameraName().isWebcam())
pose = ((VuforiaTrackableDefaultListener) template.getListener()).getFtcCameraFromTarget();
else
pose = ((VuforiaTrackableDefaultListener) template.getListener()).getPose();
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
tX = trans.get(0);
tY = trans.get(1);
tZ = trans.get(2);
// Extract the rotational components of the target relative to the robot
rX = rot.firstAngle;
rY = rot.secondAngle;
rZ = rot.thirdAngle;
}
return true;
}
else
{
pose = null;
return false;
}
}
/**
* Format pose object for human viewing.
* @param pose Pose object returned when VuMark is found.
* @return Pose description.
*/
protected String formatPose(OpenGLMatrix pose)
{
return (pose != null) ? pose.formatAsTransform() : "null";
}
}
}