diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionPortalStreamingOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionPortalStreamingOpMode.java new file mode 100644 index 000000000..3163585ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionPortalStreamingOpMode.java @@ -0,0 +1,68 @@ +package org.firstinspires.ftc.teamcode; + +import android.graphics.Bitmap; +import android.graphics.Canvas; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.function.Consumer; +import org.firstinspires.ftc.robotcore.external.function.Continuation; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.stream.CameraStreamSource; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.VisionProcessor; +import org.opencv.android.Utils; +import org.opencv.core.Mat; + +import java.util.concurrent.atomic.AtomicReference; + +@Autonomous +public class VisionPortalStreamingOpMode extends LinearOpMode { + public static class CameraStreamProcessor implements VisionProcessor, CameraStreamSource { + private final AtomicReference lastFrame = new AtomicReference<>(Bitmap.createBitmap(1, 1, Bitmap.Config.RGB_565)); + + @Override + public void init(int width, int height, CameraCalibration calibration) { + lastFrame.set(Bitmap.createBitmap(width, height, Bitmap.Config.RGB_565)); + } + + @Override + public Object processFrame(Mat frame, long captureTimeNanos) { + Bitmap b = Bitmap.createBitmap(frame.width(), frame.height(), Bitmap.Config.RGB_565); + Utils.matToBitmap(frame, b); + lastFrame.set(b); + return null; + } + + @Override + public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + // do nothing + } + + @Override + public void getFrameBitmap(Continuation> continuation) { + continuation.dispatch(bitmapConsumer -> bitmapConsumer.accept(lastFrame.get())); + } + } + + @Override + public void runOpMode() throws InterruptedException { + final CameraStreamProcessor processor = new CameraStreamProcessor(); + + new VisionPortal.Builder() + .addProcessor(processor) + .setCamera(BuiltinCameraDirection.BACK) + .build(); + + FtcDashboard.getInstance().startCameraStream(processor, 0); + + waitForStart(); + + while (opModeIsActive()) { + sleep(100L); + } + } +}