/* * Copyright (c) 2016 Titan Robotics Club (http://www.titanrobotics.com) * Based on sample code by Robert Atkinson. * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in all * copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. */ package ftclib; import android.graphics.Bitmap; import com.vuforia.CameraDevice; import com.vuforia.HINT; import com.vuforia.Image; import com.vuforia.PIXEL_FORMAT; import com.vuforia.Vuforia; import org.firstinspires.ftc.robotcore.external.ClassFactory; import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; 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.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 org.opencv.android.Utils; import org.opencv.core.Mat; import java.util.ArrayList; import java.util.HashMap; import trclib.TrcVideoSource; /** * This class makes using Vuforia a little easier by minimizing the number of calls to it. It only exposes the * minimum things you need to set for the FTC competition. If you want to do more complex stuff, you may consider * not using this and call Vuforia directly so you can customize other stuff. */ public class FtcVuforia implements TrcVideoSource<Mat> { /** * This class contains information about a trackable target. */ public static class TargetInfo { public final int index; public final String name; public final boolean isObjectTarget; public final OpenGLMatrix locationOnField; public VuforiaTrackables targetList = null; /** * Constructor: Create an instance of the object. * * @param index specifies the target index in the trackables list. * @param name specifies the target name. * @param isObjectTarget specifies true if target is an object, false if target is an image. * @param locationOnField specifies the target location on field. */ public TargetInfo(int index, String name, boolean isObjectTarget, OpenGLMatrix locationOnField) { this.index = index; this.name = name; this.isObjectTarget = isObjectTarget; this.locationOnField = locationOnField; } //TargetInfo private void setTargetList(VuforiaTrackables targetList) { this.targetList = targetList; } //setTargetList } //class TargetInfo private VuforiaLocalizer.CameraDirection cameraDir; private VuforiaLocalizer localizer; private ArrayList<VuforiaTrackables> targetLists = new ArrayList<>(); private HashMap<String, TargetInfo> targetMap = new HashMap<>(); private int numImageTargets = 0; private int numObjectTargets = 0; private int imageWidth = 0; private int imageHeight = 0; /** * Constructor: Create an instance of this object. It initializes Vuforia with the specified target images and * other parameters. * * @param licenseKey specifies the Vuforia license key. * @param cameraViewId specifies the camera view ID on the activity, -1 if none given. * @param cameraDir specifies which camera to use (front or back). * @param cameraMonitorFeedback specifies the feedback image showing the orientation of the target. */ public FtcVuforia( String licenseKey, int cameraViewId, VuforiaLocalizer.CameraDirection cameraDir, VuforiaLocalizer.Parameters.CameraMonitorFeedback cameraMonitorFeedback) { this.cameraDir = cameraDir; // // If no camera view ID, do not activate camera monitor view to save power. // VuforiaLocalizer.Parameters params = cameraViewId == -1? new VuforiaLocalizer.Parameters(): new VuforiaLocalizer.Parameters(cameraViewId); params.vuforiaLicenseKey = licenseKey; params.cameraDirection = cameraDir; params.cameraMonitorFeedback = cameraMonitorFeedback; localizer = ClassFactory.getInstance().createVuforia(params); } //FtcVuforia /** * Constructor: Create an instance of this object. It initializes Vuforia with the specified target images and * other parameters. * * @param licenseKey specifies the Vuforia license key. * @param cameraViewId specifies the camera view ID on the activity, -1 if none given. * @param cameraDir specifies which camera to use (front or back). */ public FtcVuforia(String licenseKey, int cameraViewId, VuforiaLocalizer.CameraDirection cameraDir) { this(licenseKey, cameraViewId, cameraDir, null); } //FtcVuforia /** * This method returns the Vuforia Localizer. * * @return vuforia localizer. */ public VuforiaLocalizer getLocalizer() { return localizer; } //getLocalizer public void addTargetList(String trackablesFile, TargetInfo[] targets, OpenGLMatrix phoneLocationOnRobot) { VuforiaTrackables targetList = localizer.loadTrackablesFromAsset(trackablesFile); targetLists.add(targetList); for (TargetInfo targetInfo: targets) { VuforiaTrackable target = targetList.get(targetInfo.index); target.setName(targetInfo.name); targetInfo.setTargetList(targetList); if (targetInfo.locationOnField != null) { target.setLocation(targetInfo.locationOnField); } if (phoneLocationOnRobot != null) { ((VuforiaTrackableDefaultListener) target.getListener()).setPhoneInformation( phoneLocationOnRobot, cameraDir); } targetMap.put(targetInfo.name, targetInfo); if (targetInfo.isObjectTarget) { numObjectTargets++; } else { numImageTargets++; } } Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, numImageTargets); Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_OBJECT_TARGETS, numObjectTargets); } //addTargetList /** * This method enables/disables target tracking. * * @param enabled specifies true to enable target tracking, false otherwise. */ public void setTrackingEnabled(boolean enabled) { for (VuforiaTrackables targetList: targetLists) { if (enabled) { targetList.activate(); } else { targetList.deactivate(); } } } //setTrackingEnabled /** * This method creates a location matrix that can be used to relocate an object to its final location by rotating * and translating the object from the origin of the field. It is doing the operation in the order of the * parameters. In other words, it will first rotate the object on the X-axis, then rotate on the Y-axis, then * rotate on the Z-axis, then translate on the X-axis, then translate on the Y-axis and finally translate on the * Z-axis. * * @param rotateX specifies rotation on the X-axis. * @param rotateY specifies rotation on the Y-axis. * @param rotateZ specifies rotation on the Z-axis. * @param translateX specifies translation on the X-axis. * @param translateY specifies translation on the Y-axis. * @param translateZ specifies translation on the Z-axis. * @return returns the location matrix. */ public OpenGLMatrix locationMatrix( float rotateX, float rotateY, float rotateZ, float translateX, float translateY, float translateZ) { return OpenGLMatrix.translation(translateX, translateY, translateZ) .multiplied(Orientation.getRotationMatrix( AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES, rotateX, rotateY, rotateZ)); } //locationMatrix /** * This method returns the number of trackable image targets. * * @return number of trackable image targets. */ public int getNumImageTargets() { return numImageTargets; } //getNumImageTargets /** * This method returns the number of trackable object targets. * * @return number of trackable object targets. */ public int getNumObjectTargets() { return numObjectTargets; } //getNumObjectTargets /** * This method returns the target object with the specified index in the target list. * * @param name specifies the target name. * @return target. */ public VuforiaTrackable getTarget(String name) { VuforiaTrackable target = null; TargetInfo targetInfo = targetMap.get(name); if (targetInfo != null) { target = targetInfo.targetList.get(targetInfo.index); } return target; } //getTarget /** * This method determines if the target is visible. * * @param target specifies the target object. * @return true if the target is in view, false otherwise. */ public boolean isTargetVisible(VuforiaTrackable target) { VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener)target.getListener(); return listener.isVisible(); } //isTargetVisible /** * This method returns the position matrix of the specified target. * * @param target specifies the target to get the position matrix. * @return position matrix of the specified target. */ public OpenGLMatrix getTargetPose(VuforiaTrackable target) { VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener)target.getListener(); return listener.getPose(); } //getTargetPose /** * This method determines the robot location by the given target. * * @param target specifies the target to be used to determine robot location. * @return robot location matrix. */ public OpenGLMatrix getRobotLocation(VuforiaTrackable target) { VuforiaTrackableDefaultListener listener = (VuforiaTrackableDefaultListener)target.getListener(); return listener.getRobotLocation(); } //getRobotLocation /** * This method configures Vuforia to capture video frames of the given format. * * @param imageWidth specifies the image width to capture. * @param imageHeight specifies the image height to capture. * @param queueCapacity specifies the frame queue capacity. */ public void configVideoSource(int imageWidth, int imageHeight, int queueCapacity) { this.imageWidth = imageWidth; this.imageHeight = imageHeight; Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true); localizer.setFrameQueueCapacity(queueCapacity); } //configVideoSource /** * This methods turns the phone flash light ON or OFF. * * @param enabled specifies true to turn flashlight on, false to turn off. */ public void setFlashlightEnabled(boolean enabled) { CameraDevice.getInstance().setFlashTorchMode(enabled); } //setFlashlightEnabled // // Implements HalVideoSource interface. // /** * This method gets a frame from the frame queue and returns the image that matches the format specified by the * configVideoSource method. * * @param frame specifies the frame object to hold image. * @return true if success, false otherwise. */ @Override public boolean getFrame(Mat frame) { boolean success = false; try { VuforiaLocalizer.CloseableFrame closeableFrame = localizer.getFrameQueue().take(); for (int i = 0; i < closeableFrame.getNumImages(); i++) { Image image = closeableFrame.getImage(i); if (image.getWidth() == imageWidth && image.getHeight() == imageHeight && image.getFormat() == PIXEL_FORMAT.RGB565) { Bitmap bm = Bitmap.createBitmap(image.getWidth(), image.getHeight(), Bitmap.Config.RGB_565); bm.copyPixelsFromBuffer(image.getPixels()); Utils.bitmapToMat(bm, frame); break; } } closeableFrame.close(); success = true; } catch (InterruptedException e) { e.printStackTrace(); } return success; } //getFrame /** * This method draws the given image frame to the display surface. * * @param frame specifies the image frame to be displayed. */ @Override public void putFrame(Mat frame) { // TODO: figure out how to render frame back to Vuforia. } //putFrame } //class FtcVuforia