[Vision] Init

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpRobotController.java

@@ -8,9 +8,14 @@
 import org.firstinspires.ftc.teamcode.subsystems.driveTrain.commands.mecanumDrive.MecanumArcadeDriveCommand;
 import org.firstinspires.ftc.teamcode.util.TeamColor;
 import org.firstinspires.ftc.teamcode.util.opModes.SympleCommandOpMode;
+import org.firstinspires.ftc.teamcode.vision.VisionConstants;
+import org.firstinspires.ftc.teamcode.vision.VisionManger;
+import org.firstinspires.ftc.teamcode.vision.VisionPipeline;
 
 public class TeleOpRobotController extends RobotControllerBase {
+    private final TeamColor teamColor;
     private final MecanumDriveSubsystem mecanumDriveSubsystem;
+    private final VisionManger visionManger;
 
     private TeleOpRobotController(HardwareMap hMap, Telemetry telemetry, Gamepad driverController, Gamepad actionController, TeamColor teamColor, String logFilePrefix, boolean logData) {
         super(hMap, telemetry, driverController, actionController, logFilePrefix, logData);
@@ -21,7 +26,10 @@ private TeleOpRobotController(HardwareMap hMap, Telemetry telemetry, Gamepad dri
             throw exception;
         }
 
+        this.teamColor = teamColor;
+
         this.mecanumDriveSubsystem = new MecanumDriveSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger());
+        this.visionManger = new VisionManger(this.getHardwareMap(), this.getDataLogger());
     }
 
     @Override
@@ -32,6 +40,7 @@ public void createKeyBindings() {
     @Override
     public void initialize() {
         this.mecanumDriveSubsystem.setDefaultCommand(new MecanumArcadeDriveCommand(this.mecanumDriveSubsystem, this.driverController));
+        this.visionManger.setPipeline(new VisionPipeline(VisionConstants.GamePieceColor.fromTeamColor(this.teamColor)));
     }
 
     @Override

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/maps/SensorMap.java

@@ -12,7 +12,8 @@ public enum SensorMap {
     // the ids of the dead wheels are from the motors
     DEAD_WHEEL_RIGHT(MotorMap.LEG_FRONT_RIGHT.getId()),
     DEAD_WHEEL_LEFT(MotorMap.LEG_FRONT_LEFT.getId()),
-    DEAD_WHEEL_BACK(MotorMap.LEG_BACK_LEFT.getId());
+    DEAD_WHEEL_BACK(MotorMap.LEG_BACK_LEFT.getId()),
+    WEBCAM("Webcam 1");
 
 
     private final String id;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstants.java

@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import org.firstinspires.ftc.teamcode.util.TeamColor;
+import org.opencv.core.Scalar;
+
+public class VisionConstants {
+    public static final Scalar RED_LOWER1 = new Scalar(0, 120, 70);
+    public static final Scalar RED_UPPER1 = new Scalar(10, 255, 255);
+    public static final Scalar RED_LOWER2 = new Scalar(170, 120, 70);
+    public static final Scalar RED_UPPER2 = new Scalar(180, 255, 255);
+
+    public static final Scalar BLUE_LOWER = new Scalar(90, 120, 70);
+    public static final Scalar BLUE_UPPER = new Scalar(130, 255, 255);
+
+    public static final Scalar YELLOW_LOWER = new Scalar(15, 120, 70);
+    public static final Scalar YELLOW_UPPER = new Scalar(35, 255, 255);
+
+    public static final double MIN_AREA = 150;
+
+    public static final double FOV_HORIZONTAL = 65;
+
+//    public static final double KNOWN_DISTANCE_CM = 60;
+//    public static final double KNOWN_WIDTH_CM = 15;
+
+    public enum GamePieceColor {
+        RED(255, 0, 0),
+        BLUE(0, 0, 255),
+        YELLOW(255, 255, 0);
+
+        private final Scalar color;
+
+        GamePieceColor(int r, int g, int b) {
+            this.color = new Scalar(r, g, b);
+        }
+
+        public Scalar getColor() {
+            return color;
+        }
+
+        public static GamePieceColor fromTeamColor(TeamColor teamColor) {
+            switch (teamColor) {
+                case BLUE:
+                    return VisionConstants.GamePieceColor.BLUE;
+
+                case RED:
+                    return VisionConstants.GamePieceColor.RED;
+
+                default:
+                    return VisionConstants.GamePieceColor.YELLOW;
+            }
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionManger.java

@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.maps.SensorMap;
+import org.firstinspires.ftc.teamcode.util.DataLogger;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+public class VisionManger {
+    private final OpenCvCamera camera;
+    private final DataLogger dataLogger;
+
+    public VisionManger(HardwareMap hardwareMap, DataLogger dataLogger) {
+        this.dataLogger = dataLogger;
+
+        this.dataLogger.addData(DataLogger.DataType.INFO, "[Vision] Trying to get camera.");
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
+        this.camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, SensorMap.WEBCAM.getId()), cameraMonitorViewId);
+        this.dataLogger.addData(DataLogger.DataType.INFO, "[Vision] Camera found!");
+
+        this.camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            @Override
+            public void onOpened() {
+                VisionManger.this.camera.startStreaming(640, 480, OpenCvCameraRotation.SENSOR_NATIVE);
+                dataLogger.addData(DataLogger.DataType.INFO, "[Vision] Starting to stream camera.");
+            }
+
+            @Override
+            public void onError(int errorCode) {
+                dataLogger.addData(DataLogger.DataType.ERROR, "[Vision] Error while trying to open the camera, Error code: " + errorCode);
+            }
+        });
+
+        this.dataLogger.addData(DataLogger.DataType.INFO, "[Vision] Starting to stream to ftc dashboard.");
+        FtcDashboard.getInstance().startCameraStream(this.camera, 30);
+    }
+
+    public void setPipeline(OpenCvPipeline pipeline) {
+        this.dataLogger.addData(DataLogger.DataType.INFO, String.format("[Vision] Setting vision pipeline to '%s.'", pipeline.getClass().getSimpleName()));
+        this.camera.setPipeline(pipeline);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionPipeline.java

@@ -0,0 +1,171 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import android.util.Pair;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+import org.openftc.easyopencv.OpenCvPipeline;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+
+public class VisionPipeline extends OpenCvPipeline {
+    private final VisionConstants.GamePieceColor color;
+
+    public VisionPipeline(VisionConstants.GamePieceColor color) {
+        this.color = color;
+    }
+
+    @Override
+    public Mat processFrame(Mat frame) {
+        Mat mask = this.maskGamePiece(frame);
+        Pair<Mat, List<RectLocation>> gamePiece = detectGamePiece(mask, VisionConstants.MIN_AREA);
+        Mat filledMask = gamePiece.first;
+        List<RectLocation> locations = gamePiece.second;
+
+        if(!locations.isEmpty()) {
+            RectLocation target = locations.get(0);
+
+            this.displayGamePiece(frame, target.getRect());
+
+            Pair<Double, Direction> centerDistance = this.calculateCenterDistance(frame, target.getCenter().x);
+            double yaw = this.getYaw(frame, target.getCenter().x);
+
+            String text = "Distance X: " + centerDistance.first + "px (" + centerDistance.second.name() + ")\nYaw: " + yaw;
+            Imgproc.putText(
+                    frame,
+                    text,
+                    new Point(10, 30),
+                    Imgproc.FONT_HERSHEY_SIMPLEX,
+                    0.7,
+                    this.color.getColor(),
+                    2
+            );
+        }
+
+        return frame;
+    }
+
+    private Mat maskGamePiece(Mat frame) {
+        Mat blur = new Mat();
+        Imgproc.GaussianBlur(frame, blur, new Size(5, 5), 0);
+
+        Mat hsv = new Mat();
+        Imgproc.cvtColor(blur, hsv, Imgproc.COLOR_BGR2HSV);
+
+        Mat mask = new Mat();
+
+        switch (color) {
+            case RED:
+                Mat mask1 = new Mat();
+                Core.inRange(hsv, VisionConstants.RED_LOWER1, VisionConstants.RED_UPPER1, mask1);
+
+                Mat mask2 = new Mat();
+                Core.inRange(hsv, VisionConstants.RED_LOWER2, VisionConstants.RED_UPPER2, mask2);
+                Core.bitwise_or(mask1, mask2, mask);
+                break;
+
+            case BLUE:
+                Core.inRange(hsv, VisionConstants.BLUE_LOWER, VisionConstants.BLUE_UPPER, mask);
+                break;
+
+            case YELLOW:
+                Core.inRange(hsv, VisionConstants.YELLOW_LOWER, VisionConstants.YELLOW_UPPER, mask);
+                break;
+
+            default:
+                throw new RuntimeException("Color not found");
+        }
+
+        Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
+        Imgproc.morphologyEx(mask, mask, Imgproc.MORPH_CLOSE, kernel);
+        Imgproc.morphologyEx(mask, mask, Imgproc.MORPH_OPEN, kernel);
+
+        return mask;
+    }
+
+    private Pair<Mat, List<RectLocation>> detectGamePiece(Mat mask, double minArea) {
+        List<MatOfPoint> contours = new ArrayList<>();
+        Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);
+
+        List<RectLocation> locations = new ArrayList<>();
+        Mat filledMask = new Mat();
+
+        for(MatOfPoint contour : contours) {
+            double area = Imgproc.contourArea(contour);
+            if (area > minArea) {
+                Imgproc.drawContours(filledMask, List.of(contour), -1, this.color.getColor());
+
+                Rect rect = Imgproc.boundingRect(contour);
+
+                locations.add(new RectLocation(
+                        new Point(
+                                rect.x + (rect.width / 2),
+                                rect.y + (rect.height / 2)
+                        ),
+                        rect,
+                        area
+                ));
+            }
+        }
+        locations.sort(Comparator.comparingDouble(location -> location.area));
+        return Pair.create(filledMask, locations);
+    }
+
+    private double getYaw(Mat frame, double targetCenterX) {
+        int frameWidth = frame.width();
+        int frameCenterX = frameWidth / 2;
+
+        double offsetX = targetCenterX - frameCenterX;
+        return (offsetX / (frameWidth / 2f)) * (VisionConstants.FOV_HORIZONTAL / 2f);
+    }
+
+    private Pair<Double, Direction> calculateCenterDistance(Mat frame, double targetCenterX) {
+        int frameWidth = frame.width();
+
+        int frameCenterX = frameWidth / 2;
+
+        double distanceX = targetCenterX - frameCenterX;
+
+        return Pair.create(distanceX, distanceX > 0 ? Direction.RIGHT : Direction.LEFT);
+    }
+
+    private void displayGamePiece(Mat frame, Rect rect) {
+        Imgproc.rectangle(frame, rect, this.color.getColor(), 4);
+    }
+
+    private final class RectLocation {
+        private final Point center;
+        private final Rect rect;
+        private final double area;
+
+        public RectLocation(Point center, Rect rect, double area) {
+            this.center = center;
+            this.rect = rect;
+            this.area = area;
+        }
+
+        public Rect getRect() {
+            return rect;
+        }
+
+        public double getArea() {
+            return area;
+        }
+
+        public Point getCenter() {
+            return center;
+        }
+    }
+
+    public enum Direction {
+        LEFT,
+        RIGHT
+    }
+}