From d90cc082712f31793f01461d272234b6e0529690 Mon Sep 17 00:00:00 2001 From: ImBonana Date: Mon, 16 Dec 2024 22:15:24 +0200 Subject: [PATCH] Add VisionManager class, Add the vision pipeline for the game piece --- .../ftc/teamcode/TeleOpRobotController.java | 9 + .../ftc/teamcode/maps/SensorMap.java | 3 +- .../ftc/teamcode/vision/VisionConstants.java | 53 ++++++ .../ftc/teamcode/vision/VisionManger.java | 47 +++++ .../ftc/teamcode/vision/VisionPipeline.java | 171 ++++++++++++++++++ 5 files changed, 282 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionManger.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionPipeline.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpRobotController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpRobotController.java index c1de0cc..2b1f56c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpRobotController.java +++ b/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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/maps/SensorMap.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/maps/SensorMap.java index 360d0a6..4118ffc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/maps/SensorMap.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstants.java new file mode 100644 index 0000000..d9047b5 --- /dev/null +++ b/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; + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionManger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionManger.java new file mode 100644 index 0000000..89bfb31 --- /dev/null +++ b/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); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionPipeline.java new file mode 100644 index 0000000..2feeb1d --- /dev/null +++ b/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> gamePiece = detectGamePiece(mask, VisionConstants.MIN_AREA); + Mat filledMask = gamePiece.first; + List locations = gamePiece.second; + + if(!locations.isEmpty()) { + RectLocation target = locations.get(0); + + this.displayGamePiece(frame, target.getRect()); + + Pair 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> detectGamePiece(Mat mask, double minArea) { + List contours = new ArrayList<>(); + Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); + + List 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 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 + } +}