Added Tensorflow and Vuforia to Robot class, added testing engines

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java

@@ -1,5 +1,9 @@
 package org.timecrafters.FreightFrenzy.Competition.Common;
 
+import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
+import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
+
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -7,14 +11,59 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+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.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.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+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.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
 import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
+import java.util.ArrayList;
+import java.util.List;
+
 public class Robot {
+    private static final float mmPerInch        = 25.4f;
+    private static final float mmTargetHeight   = 6 * mmPerInch;  // the height of the center of the target image above the floor
+    private static final float halfField        = 72 * mmPerInch;
+    private static final float halfTile         = 12 * mmPerInch;
+    private static final float oneAndHalfTile   = 36 * mmPerInch;
+
+    private static final String TENSORFLOW_MODEL_ASSET = "FreightFrenzy_BCDM.tflite";
+    private static final String[] TENSORFLOW_MODEL_LABELS = {
+            "Ball",
+            "Cube",
+            "Duck",
+            "Marker"
+    };
+    private static final String VUFORIA_KEY = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
+    private static final float VUFORIA_CAMERA_FORWARD_DISPLACEMENT  = 0.0f * mmPerInch; // Camera lens forward distance from robot center
+    private static final float VUFORIA_CAMERA_VERTICAL_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens height above ground
+    private static final float VUFORIA_CAMERA_LEFT_DISPLACEMENT     = 0.0f * mmPerInch; // Camera lens left distance from robot center
+
     private final CyberarmEngine engine;
     public TimeCraftersConfiguration configuration;
 
     // Shiny
     public RevBlinkinLedDriver leds;
+    public TFObjectDetector tensorflow;
+    private List<Recognition> tensorflowRecognitions = new ArrayList<>();
+    public VuforiaLocalizer vuforia;
+    private VuforiaTrackables vuforiaTargets;
+    private List<VuforiaTrackable> vuforiaTrackables;
+    private OpenGLMatrix vuforiaCameraLocationOnRobot = OpenGLMatrix
+            .translation(VUFORIA_CAMERA_FORWARD_DISPLACEMENT, VUFORIA_CAMERA_LEFT_DISPLACEMENT, VUFORIA_CAMERA_VERTICAL_DISPLACEMENT)
+            .multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XZY, AngleUnit.DEGREES, 90, 90, 0));
+    private RobotLocation vuforiaLastLocation;
 
     // Sensors
     public BNO055IMU imu;
@@ -43,20 +92,72 @@ public class Robot {
         initCollector();
         initDepositor();
         initCarousel();
+        initVuforia();
+        initTensorflow();
     }
 
-    public double rotation() {
+    public double heading() {
         return imu.getAngularOrientation().firstAngle;
     }
 
-    public double pitch() {
+    public double roll() {
         return imu.getAngularOrientation().secondAngle;
     }
 
-    public double roll() {
+    public double pitch() {
         return imu.getAngularOrientation().thirdAngle;
     }
 
+    public void activateTensorflow() {
+        tensorflow.activate();
+    }
+
+    public List<Recognition> tensorflowDetections() {
+        List<Recognition> updateRecognitions = tensorflow.getUpdatedRecognitions();
+
+        if (updateRecognitions != null) {
+            tensorflowRecognitions = updateRecognitions;
+        }
+
+        return tensorflowRecognitions;
+    }
+
+    public void deactivateTensorflow() {
+        tensorflow.deactivate();
+    }
+
+    public void activateVuforia() {
+        vuforiaTargets.activate();
+    }
+
+    public RobotLocation vuforiaLocation() {
+        for (VuforiaTrackable trackable : vuforiaTrackables) {
+            if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
+                OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
+                if (robotLocationTransform != null) {
+                    vuforiaLastLocation = new RobotLocation(robotLocationTransform);
+                }
+
+                // Recycle old position since there is no new data
+                return vuforiaLastLocation;
+            }
+        }
+
+        // Return null if there is no visible trackable
+        return null;
+    }
+
+    private void vuforiaIdentifyTarget(int targetIndex, String targetName, float dx, float dy, float dz, float rx, float ry, float rz) {
+        VuforiaTrackable aTarget = vuforiaTargets.get(targetIndex);
+        aTarget.setName(targetName);
+        aTarget.setLocation(OpenGLMatrix.translation(dx, dy, dz)
+                .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, rx, ry, rz)));
+    }
+
+    public void deactivateVuforia() {
+        vuforiaTargets.deactivate();
+    }
+
     public double ticksToInches(int ticks) {
         return ticks * (WHEEL_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
     }
@@ -83,6 +184,8 @@ public class Robot {
 
         this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
         imu.initialize(parameters);
+
+        imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
     }
 
     private void initDrivetrain() {
@@ -106,4 +209,44 @@ public class Robot {
     private void initCarousel() {
 
     }
+
+    private void initVuforia() {
+        int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
+
+        parameters.vuforiaLicenseKey = VUFORIA_KEY;
+        parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
+        parameters.useExtendedTracking = false;
+
+        vuforia = ClassFactory.getInstance().createVuforia(parameters);
+
+        vuforiaTargets = vuforia.loadTrackablesFromAsset("FreightFrenzy");
+
+        vuforiaTrackables = new ArrayList<>(vuforiaTargets);
+
+        // Name and locate each trackable object
+        vuforiaIdentifyTarget(0, "Blue Storage",       -halfField,  oneAndHalfTile, mmTargetHeight, 90, 0, 90);
+        vuforiaIdentifyTarget(1, "Blue Alliance Wall",  halfTile,   halfField,      mmTargetHeight, 90, 0, 0);
+        vuforiaIdentifyTarget(2, "Red Storage",        -halfField, -oneAndHalfTile, mmTargetHeight, 90, 0, 90);
+        vuforiaIdentifyTarget(3, "Red Alliance Wall",   halfTile,  -halfField,      mmTargetHeight, 90, 0, 180);
+
+        for (VuforiaTrackable trackable : vuforiaTrackables) {
+            ((VuforiaTrackableDefaultListener) trackable.getListener())
+                    .setCameraLocationOnRobot(parameters.cameraName, vuforiaCameraLocationOnRobot);
+        }
+    }
+
+    private void initTensorflow() {
+        int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
+                "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
+
+        TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
+        parameters.minResultConfidence = 0.8f;
+        parameters.isModelTensorFlow2 = true;
+        parameters.inputSize = 320;
+
+        tensorflow = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
+        tensorflow.loadModelFromAsset(TENSORFLOW_MODEL_ASSET, TENSORFLOW_MODEL_LABELS);
+    }
 }

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/RobotLocation.java

@@ -0,0 +1,25 @@
+package org.timecrafters.FreightFrenzy.Competition.Common;
+
+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;
+
+public class RobotLocation {
+    public double x, y, z, roll, pitch, heading;
+    public RobotLocation(OpenGLMatrix matrix) {
+        VectorF translation = matrix.getTranslation();
+        Orientation rotation = Orientation.getOrientation(
+                matrix, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
+
+        x = translation.get(0);
+        y = translation.get(1);
+        z = translation.get(2);
+
+        roll = rotation.firstAngle;
+        pitch = rotation.secondAngle;
+        heading = rotation.thirdAngle;
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/TensorFlowTestEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.HardwareTesting.States.TensorFlowTestState;
+
+@TeleOp(name = "TensorFlow Test", group = "testing")
+public class TensorFlowTestEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        Robot robot = new Robot(this);
+
+        addState(new TensorFlowTestState(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/VuforiaTestEngine.java

@@ -0,0 +1,17 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.Engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.HardwareTesting.States.VuforiaTestState;
+
+@TeleOp(name = "Vuforia Test", group = "testing")
+public class VuforiaTestEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        Robot robot = new Robot(this);
+
+        addState(new VuforiaTestState(robot));
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/TensorFlowTestState.java

@@ -0,0 +1,38 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+
+public class TensorFlowTestState extends CyberarmState {
+    private Robot robot;
+
+    public TensorFlowTestState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        robot.activateTensorflow();
+    }
+
+    @Override
+    public void exec() {
+
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateTensorflow();
+    }
+
+    @Override
+    public void telemetry() {
+        for (Recognition recognition : robot.tensorflowDetections()) {
+            engine.telemetry.addData("Label", recognition.getLabel());
+            engine.telemetry.addData("Left", recognition.getLeft());
+            engine.telemetry.addData("Top", recognition.getTop());
+            engine.telemetry.addData("Confidence", recognition.getConfidence());
+        }
+    }
+}

TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/VuforiaTestState.java

@@ -0,0 +1,42 @@
+package org.timecrafters.FreightFrenzy.HardwareTesting.States;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.FreightFrenzy.Competition.Common.Robot;
+import org.timecrafters.FreightFrenzy.Competition.Common.RobotLocation;
+
+public class VuforiaTestState extends CyberarmState {
+    private Robot robot;
+
+    public VuforiaTestState(Robot robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void init() {
+        robot.activateVuforia();
+    }
+
+    @Override
+    public void exec() {
+
+    }
+
+    @Override
+    public void telemetry() {
+        RobotLocation robotLocation = robot.vuforiaLocation();
+
+        engine.telemetry.addLine("Robot Vuforia Location");
+
+        if (robotLocation != null) {
+            engine.telemetry.addData("Orientation", "heading: %.2f, roll: %.2f, pitch: %.2f", robotLocation.heading, robotLocation.roll, robotLocation.pitch);
+            engine.telemetry.addData("Location", "X: %.2f, Y: %.2f, Z: %.2f", robotLocation.x, robotLocation.y, robotLocation.z);
+        } else {
+            engine.telemetry.addLine("No Data");
+        }
+    }
+
+    @Override
+    public void stop() {
+        robot.deactivateVuforia();
+    }
+}