From fc13916f0b5f31d3bc67f7aa86c90bea1247ef05 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 17 Feb 2024 16:20:02 -0600 Subject: [PATCH] Initial commit --- .gitignore | 1 + build.gradle | 80 +++++++++++ proguard-rules.pro | 21 +++ src/main/AndroidManifest.xml | 11 ++ .../com/tearabite/ftctearabits/Alliance.java | 4 + .../localization/AprilTagPoseEstimator.java | 82 +++++++++++ .../BasicColorDetectionVisionProcessor.java | 79 +++++++++++ .../tearabite/ftctearabits/vision/Colors.java | 21 +++ .../ftctearabits/vision/Constants.java | 15 ++ .../ftctearabits/vision/Detection.java | 133 ++++++++++++++++++ .../ftctearabits/vision/OpenCVUtil.java | 102 ++++++++++++++ .../ftctearabits/vision/ScalarRange.java | 13 ++ .../AprilTagPoseEstimatorTest.java | 80 +++++++++++ 13 files changed, 642 insertions(+) create mode 100644 .gitignore create mode 100644 build.gradle create mode 100644 proguard-rules.pro create mode 100644 src/main/AndroidManifest.xml create mode 100644 src/main/java/com/tearabite/ftctearabits/Alliance.java create mode 100644 src/main/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimator.java create mode 100644 src/main/java/com/tearabite/ftctearabits/vision/BasicColorDetectionVisionProcessor.java create mode 100644 src/main/java/com/tearabite/ftctearabits/vision/Colors.java create mode 100644 src/main/java/com/tearabite/ftctearabits/vision/Constants.java create mode 100644 src/main/java/com/tearabite/ftctearabits/vision/Detection.java create mode 100644 src/main/java/com/tearabite/ftctearabits/vision/OpenCVUtil.java create mode 100644 src/main/java/com/tearabite/ftctearabits/vision/ScalarRange.java create mode 100644 src/test/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimatorTest.java diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..42afabf --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/build \ No newline at end of file diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..d1330c0 --- /dev/null +++ b/build.gradle @@ -0,0 +1,80 @@ +import java.text.SimpleDateFormat + +buildscript { + repositories { + mavenCentral() + google() + } + dependencies { + classpath 'com.android.tools.build:gradle:8.2.1' + } +} + +plugins { + id 'com.android.library' + id 'maven-publish' + id 'signing' +} + +java { + sourceCompatibility = JavaVersion.VERSION_1_8 + targetCompatibility = JavaVersion.VERSION_1_8 +} + +group = 'com.tearabite.ftctearabits' +version = '1.0' + +sourceSets { + main { + java { srcDirs = ["src/java"] } + resources { srcDir "src/resources" } + } +} + +android { + namespace = 'com.tearabite.ftctearabits' + + defaultConfig { + minSdkVersion 24 + productFlavors { + biscuit { + aarMetadata { + minCompileSdk 24 + } + } + } + } + + testFixtures { + enable true + } +} + +publishing { + publications { + mavenJava(MavenPublication) { + groupId = 'com.tearabite.ftctearabits' + artifactId = 'ftctearabits' + version = '1.0' + } + } +} + +signing { + sign publishing.publications.mavenJava +} + +repositories { + mavenCentral() + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + implementation 'org.firstinspires.ftc:RobotCore:9.0.1' + implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta6' + implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta6' + implementation 'org.firstinspires.ftc:Vision:9.0.1' + testImplementation 'org.junit.jupiter:junit-jupiter:5.9.1' + compileOnly 'org.projectlombok:lombok:1.18.30' + annotationProcessor 'org.projectlombok:lombok:1.18.30' +} \ No newline at end of file diff --git a/proguard-rules.pro b/proguard-rules.pro new file mode 100644 index 0000000..481bb43 --- /dev/null +++ b/proguard-rules.pro @@ -0,0 +1,21 @@ +# Add project specific ProGuard rules here. +# You can control the set of applied configuration files using the +# proguardFiles setting in build.gradle. +# +# For more details, see +# http://developer.android.com/guide/developing/tools/proguard.html + +# If your project uses WebView with JS, uncomment the following +# and specify the fully qualified class name to the JavaScript interface +# class: +#-keepclassmembers class fqcn.of.javascript.interface.for.webview { +# public *; +#} + +# Uncomment this to preserve the line number information for +# debugging stack traces. +#-keepattributes SourceFile,LineNumberTable + +# If you keep the line number information, uncomment this to +# hide the original source file name. +#-renamesourcefileattribute SourceFile \ No newline at end of file diff --git a/src/main/AndroidManifest.xml b/src/main/AndroidManifest.xml new file mode 100644 index 0000000..3705b31 --- /dev/null +++ b/src/main/AndroidManifest.xml @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/src/main/java/com/tearabite/ftctearabits/Alliance.java b/src/main/java/com/tearabite/ftctearabits/Alliance.java new file mode 100644 index 0000000..26a420f --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/Alliance.java @@ -0,0 +1,4 @@ +package com.tearabite.ftctearabits; + +public enum Alliance { Blue, Red } + diff --git a/src/main/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimator.java b/src/main/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimator.java new file mode 100644 index 0000000..a8fa830 --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimator.java @@ -0,0 +1,82 @@ +package com.tearabite.ftctearabits.localization; + +import static java.lang.Math.PI; +import static java.lang.Math.asin; +import static java.lang.Math.atan2; +import static java.lang.Math.cos; +import static java.lang.Math.pow; +import static java.lang.Math.sin; +import static java.lang.Math.tan; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; + +import java.security.InvalidParameterException; + +public class AprilTagPoseEstimator { + + private final Pose2d robotOffset; + + public AprilTagPoseEstimator() { + this(new Pose2d(new Vector2d(0, 0), 0)); + } + + public AprilTagPoseEstimator(Pose2d robotOffset) { + this.robotOffset = robotOffset; + } + + + public Pose2d estimatePose(AprilTagDetection detection) { + if (detection == null || detection.metadata == null || detection.metadata.fieldPosition == null || detection.ftcPose == null) { + throw new InvalidParameterException(); + } + + AprilTagPoseFtc ftcPose = detection.ftcPose; + VectorF fieldPosition = detection.metadata.fieldPosition; + Quaternion fieldOrientation = detection.metadata.fieldOrientation; + return estimatePose( + Math.toRadians(ftcPose.yaw), + Math.toRadians(ftcPose.bearing), + ftcPose.range, + fieldPosition.get(0), + fieldPosition.get(1), + fieldOrientation.x, + fieldOrientation.y, + fieldOrientation.z, + fieldOrientation.w); + } + + private Pose2d estimatePose(double yaw, double bearing, double range, double Tx, double Ty, double Ta, double Tb, double Tc, double Td) { + double aprilTagHeading = getTYaw(Ta, Tb, Tc, Td); + double cameraRotation = ((PI / 2) - aprilTagHeading + yaw); + double cx = Tx + range * cos(bearing) * cos(cameraRotation) + range * sin(bearing) * sin(cameraRotation); + double cy = Ty + range * cos(bearing) * sin(cameraRotation) - range * sin(bearing) * cos(cameraRotation); + + double aprilTagX = this.robotOffset.position.x; + double aprilTagY = this.robotOffset.position.y; + double cameraRotationOnRobot = this.robotOffset.heading.toDouble(); + double robotRotation = (PI / 2) + aprilTagHeading + yaw - cameraRotationOnRobot; + double rx = cx + aprilTagX * cos(robotRotation) - aprilTagY * sin(robotRotation); + double ry = cy + aprilTagX * sin(robotRotation) + aprilTagY * cos(robotRotation); + double rh = tan(yaw - cameraRotationOnRobot); + + return new Pose2d(rx, ry, rh); + } + + private static double getTPitch(double a, double b, double c, double d) { + return asin(2 * ((a * d) - (b * c))); + } + + private static double getTRoll(double a, double b, double c, double d) { + return atan2(2 * ((a * c) + (b * d)), 1 - 2 * (pow(c, 2) + pow(d, 2))); + } + + private static double getTYaw(double a, double b, double c, double d) { + return atan2(2 * ((a * b) + (c * d)), 1 - 2 * ((pow(b, 2) + pow(c, 2)))); + } +} diff --git a/src/main/java/com/tearabite/ftctearabits/vision/BasicColorDetectionVisionProcessor.java b/src/main/java/com/tearabite/ftctearabits/vision/BasicColorDetectionVisionProcessor.java new file mode 100644 index 0000000..a6b98b9 --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/vision/BasicColorDetectionVisionProcessor.java @@ -0,0 +1,79 @@ +package com.tearabite.ftctearabits.vision; + +import static com.tearabite.ftctearabits.vision.Colors.WHITE; +import static com.tearabite.ftctearabits.vision.OpenCVUtil.getLargestContour; + +import android.graphics.Canvas; + +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.vision.VisionProcessor; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +import java.util.ArrayList; + +import lombok.Getter; + +public class BasicColorDetectionVisionProcessor implements VisionProcessor { + public static final Size BLUR_SIZE = new Size(7, 7); + public static final int ERODE_DILATE_ITERATIONS = 2; + public static final Mat STRUCTURING_ELEMENT = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5)); + public static final Point ANCHOR = new Point((STRUCTURING_ELEMENT.cols() / 2f), STRUCTURING_ELEMENT.rows() / 2f); + + private final Mat blurred = new Mat(); + private final ScalarRange[] colorRanges; + @Getter private Detection detection; + private final Mat hsv = new Mat(); + private final double ignoreSmallerThan; + private final double ignoreLargerThan; + private final Mat mask = new Mat(); + private final Mat tmpMask = new Mat(); + + + public BasicColorDetectionVisionProcessor(double ignoreSmallerThan, double ignoreLargerThan, ScalarRange... colorRanges) { + this.ignoreSmallerThan = ignoreSmallerThan; + this.ignoreLargerThan = ignoreLargerThan; + this.colorRanges = colorRanges; + } + + @Override + public void init(int width, int height, CameraCalibration calibration) { + this.detection = new Detection(new Size(width, height), ignoreSmallerThan, ignoreLargerThan); + } + + @Override + public Object processFrame(Mat input, long captureTimeNanos) { + Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0); + Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); + + mask.release(); + for (ScalarRange colorRange : this.colorRanges) { + Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask); + if (mask.empty() || mask.rows() <= 0) { + Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask); + } + Core.add(mask, tmpMask, mask); + } + + Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + + ArrayList contours = new ArrayList<>(); + Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); + + detection.setContour(getLargestContour(contours)); + + return input; + } + @Override + public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + if (detection != null && detection.isValid()) { + Point center = detection.getCenterPx(); + canvas.drawCircle((float) center.x, (float) center.y, 10, WHITE); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/tearabite/ftctearabits/vision/Colors.java b/src/main/java/com/tearabite/ftctearabits/vision/Colors.java new file mode 100644 index 0000000..f4f5057 --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/vision/Colors.java @@ -0,0 +1,21 @@ +package com.tearabite.ftctearabits.vision; + +import android.graphics.Color; + +import org.opencv.core.Scalar; +public class Colors { + public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80); + public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255); + public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER); + public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2])); + public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80); + public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255); + public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER); + public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40); + public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255); + + public static OpenCVUtil.LinePaint RED = new OpenCVUtil.LinePaint(Color.RED); + public static OpenCVUtil.LinePaint BLUE = new OpenCVUtil.LinePaint(Color.BLUE); + public static OpenCVUtil.LinePaint BLACK = new OpenCVUtil.LinePaint(Color.BLACK); + public static OpenCVUtil.LinePaint WHITE = new OpenCVUtil.LinePaint(Color.WHITE); +} \ No newline at end of file diff --git a/src/main/java/com/tearabite/ftctearabits/vision/Constants.java b/src/main/java/com/tearabite/ftctearabits/vision/Constants.java new file mode 100644 index 0000000..030b7e7 --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/vision/Constants.java @@ -0,0 +1,15 @@ +package com.tearabite.ftctearabits.vision; + +import org.opencv.core.Scalar; + +public class Constants { + public static Scalar RED = new Scalar(255, 0, 0); + public static Scalar GREEN = new Scalar(0, 255, 0); + public static Scalar BLUE = new Scalar(0, 0, 255); + public static Scalar WHITE = new Scalar(255, 255, 255); + public static Scalar GRAY = new Scalar(80, 80, 80); + public static Scalar BLACK = new Scalar(0, 0, 0); + public static Scalar ORANGE = new Scalar(255, 165, 0); + public static Scalar YELLOW = new Scalar(255, 255, 0); + public static Scalar PURPLE = new Scalar(128, 0, 128); +} diff --git a/src/main/java/com/tearabite/ftctearabits/vision/Detection.java b/src/main/java/com/tearabite/ftctearabits/vision/Detection.java new file mode 100644 index 0000000..49d4dfb --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/vision/Detection.java @@ -0,0 +1,133 @@ +package com.tearabite.ftctearabits.vision; + +import static com.tearabite.ftctearabits.vision.Constants.GREEN; +import static com.tearabite.ftctearabits.vision.OpenCVUtil.drawConvexHull; +import static com.tearabite.ftctearabits.vision.OpenCVUtil.drawPoint; +import static com.tearabite.ftctearabits.vision.OpenCVUtil.fillConvexHull; +import static com.tearabite.ftctearabits.vision.OpenCVUtil.getBottomLeftOfContour; +import static com.tearabite.ftctearabits.vision.OpenCVUtil.getBottomRightOfContour; +import static com.tearabite.ftctearabits.vision.OpenCVUtil.getCenterOfContour; + +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +// Class for a Detection +public class Detection { + public static final Point INVALID_POINT = new Point(Double.MIN_VALUE, Double.MIN_VALUE); + public static final double INVALID_AREA = -1; + public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0); + + private double minAreaPx; + private double maxAreaPx; + private final Size maxSizePx; + private double areaPx = INVALID_AREA; + private Point centerPx = INVALID_POINT; + private Point bottomLeftPx = INVALID_POINT; + private Point bottomRightPx = INVALID_POINT; + private MatOfPoint contour; + + // Constructor + public Detection(Size frameSize, double minAreaFactor) { + this.maxSizePx = frameSize; + this.minAreaPx = frameSize.area() * minAreaFactor; + this.maxAreaPx = frameSize.area(); + } + + public Detection(Size frameSize, double minAreaFactor, double maxAreaFactor) { + this.maxSizePx = frameSize; + this.minAreaPx = frameSize.area() * minAreaFactor; + this.maxAreaPx = frameSize.area() * maxAreaFactor; + } + + public void setMinArea(double minAreaFactor) { + this.minAreaPx = maxSizePx.area() * minAreaFactor; + } + + public void setMaxArea(double maxAreaFactor) { + this.minAreaPx = maxSizePx.area() * maxAreaFactor; + } + + // Draw a convex hull around the current detection on the given image + public void draw(Mat img, Scalar color) { + if (isValid()) { + drawConvexHull(img, contour, color); + drawPoint(img, centerPx, GREEN); + } + } + + // Draw a convex hull around the current detection on the given image + public void fill(Mat img, Scalar color) { + if (isValid()) { + fillConvexHull(img, contour, color); + drawPoint(img, centerPx, GREEN); + } + } + + // Check if the current Detection is valid + public boolean isValid() { + return (this.contour != null) && (this.areaPx != INVALID_AREA); + } + + // Get the current contour + public MatOfPoint getContour() { + return contour; + } + + // Set the values of the current contour + public void setContour(MatOfPoint contour) { + this.contour = contour; + + double area; + if (contour != null && (area = Imgproc.contourArea(contour)) > minAreaPx && area < maxAreaPx) { + this.areaPx = area; + this.centerPx = getCenterOfContour(contour); + this.bottomLeftPx = getBottomLeftOfContour(contour); + this.bottomRightPx = getBottomRightOfContour(contour); + } else { + this.areaPx = INVALID_AREA; + this.centerPx = INVALID_POINT; + this.bottomLeftPx = INVALID_POINT; + this.bottomRightPx = INVALID_POINT; + } + } + + // Returns the center of the Detection, normalized so that the width and height of the frame is from [-50,50] + public Point getCenter() { + if (!isValid()) { + return INVALID_POINT; + } + + double normalizedX = ((centerPx.x / maxSizePx.width) * 100) - 50; + double normalizedY = ((centerPx.y / maxSizePx.height) * -100) + 50; + + return new Point(normalizedX, normalizedY); + } + + // Get the center point in pixels + public Point getCenterPx() { + return centerPx; + } + + // Get the area of the Detection, normalized so that the area of the frame is 100 + public double getArea() { + if (!isValid()) { + return INVALID_AREA; + } + + return (areaPx / (maxSizePx.width * maxSizePx.height)) * 100; + } + + // Get the leftmost bottom corner of the detection + public Point getBottomLeftCornerPx() { + return bottomLeftPx; + } + + // Get the rightmost bottom corner of the detection + public Point getBottomRightCornerPx() { + return bottomRightPx; + } +} \ No newline at end of file diff --git a/src/main/java/com/tearabite/ftctearabits/vision/OpenCVUtil.java b/src/main/java/com/tearabite/ftctearabits/vision/OpenCVUtil.java new file mode 100644 index 0000000..5941502 --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/vision/OpenCVUtil.java @@ -0,0 +1,102 @@ +package com.tearabite.ftctearabits.vision; + +import android.graphics.Paint; + +import org.opencv.core.Mat; +import org.opencv.core.MatOfInt; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.opencv.imgproc.Moments; + +import java.util.Collections; +import java.util.List; + +// CV Helper Functions +public class OpenCVUtil { + + public static String telem = "nothing"; + + // Draw a point + public static void drawPoint(Mat img, Point point, Scalar color) { + Imgproc.circle(img, point, 3, color, -1); + } + + // Get the center of a contour + public static Point getCenterOfContour(MatOfPoint contour) { + Moments moments = Imgproc.moments(contour); + return new Point(moments.m10 / moments.m00, moments.m01/ moments.m00); + } + + // Get the bottom left of a contour + public static Point getBottomLeftOfContour(MatOfPoint contour) { + Rect boundingRect = Imgproc.boundingRect(contour); + return new Point(boundingRect.x, boundingRect.y+boundingRect.height); + } + + // Get the bottom right of a contour + public static Point getBottomRightOfContour(MatOfPoint contour) { + Rect boundingRect = Imgproc.boundingRect(contour); + return new Point(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height); + } + + // Draw a contour + public static void drawContour(Mat img, MatOfPoint contour, Scalar color) { + Imgproc.drawContours(img, Collections.singletonList(contour), 0, color, 2); + } + + // Draw a convex hull around a contour + public static void drawConvexHull(Mat img, MatOfPoint contour, Scalar color) { + MatOfInt hull = new MatOfInt(); + Imgproc.convexHull(contour, hull); + Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, 2); + } + + // Draw a filled in convex hull around a contour + public static void fillConvexHull(Mat img, MatOfPoint contour, Scalar color) { + MatOfInt hull = new MatOfInt(); + Imgproc.convexHull(contour, hull); + Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, -1); + } + + // Convert indexes to points that is used in order to draw the contours + public static MatOfPoint convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) { + int[] arrIndex = indexes.toArray(); + Point[] arrContour = contour.toArray(); + Point[] arrPoints = new Point[arrIndex.length]; + + for (int i=0;i contours) { + if (contours.size() == 0) { + return null; + } + return getLargestContours(contours, 1).get(0); + } + + // Get the top largest contours + public static List getLargestContours(List contours, int numContours) { + Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a)); + return contours.subList(0, Math.min(numContours, contours.size())); + } + + public static class LinePaint extends Paint + { + public LinePaint(int color) + { + setColor(color); + setAntiAlias(true); + setStrokeCap(Paint.Cap.ROUND); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/tearabite/ftctearabits/vision/ScalarRange.java b/src/main/java/com/tearabite/ftctearabits/vision/ScalarRange.java new file mode 100644 index 0000000..be7908c --- /dev/null +++ b/src/main/java/com/tearabite/ftctearabits/vision/ScalarRange.java @@ -0,0 +1,13 @@ +package com.tearabite.ftctearabits.vision; + +import org.opencv.core.Scalar; + +import lombok.AllArgsConstructor; +import lombok.Data; + +@Data +@AllArgsConstructor +public class ScalarRange { + private Scalar upper; + private Scalar lower; +} \ No newline at end of file diff --git a/src/test/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimatorTest.java b/src/test/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimatorTest.java new file mode 100644 index 0000000..e7a1332 --- /dev/null +++ b/src/test/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimatorTest.java @@ -0,0 +1,80 @@ +package com.tearabite.ftctearabits.localization; + +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.fail; + +import com.acmerobotics.roadrunner.Pose2d; + +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagMetadata; +import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; + +import java.security.InvalidParameterException; +import java.util.stream.Stream; + +class AprilTagPoseEstimatorTest { + + @Test + public void estimatePose_null_throws() { + AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(); + assertThrows(InvalidParameterException.class, () -> estimator.estimatePose(null)); + } + + @ParameterizedTest + @MethodSource("provideEstimatePosTestValues") + public void estimatePos_notNull_returnsPose(AprilTagMetadata metadata, AprilTagPoseFtc poseFtc, Pose2d expectedPose) { + AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(new Pose2d(-7.77, 0.505, 0)); + AprilTagDetection detection = new AprilTagDetection(1, 0, 0, null, null, metadata, poseFtc, null, 0); + + Pose2d estimatedPose = estimator.estimatePose(detection); + + assertIsClose(estimatedPose, expectedPose); + } + + private boolean isClose(double a, double b) { + return Math.abs(a - b) < 0.1; + } + + private void assertIsClose(Pose2d a, Pose2d b) { + boolean isClose = isClose(a.position.x, b.position.x) + && isClose(a.position.y, b.position.y) + && isClose(a.heading.toDouble(), b.heading.toDouble()); + + if (!isClose) { + fail(String.format("Expected (%.1f, %.1f, %.1f) to be close to (%.1f, %.1f, %.1f)", + a.position.x, a.position.y, a.heading.toDouble(), + b.position.x, b.position.y, b.heading.toDouble())); + } + } + + private static Stream provideEstimatePosTestValues() { + final AprilTagMetadata metadata = new AprilTagMetadata( + 2, + "testTag", + 0, + new VectorF(60.25f, 35.41f, 4f), DistanceUnit.INCH, + new Quaternion(0.3536f, -0.6124f, 0.6124f, -0.3536f, 0)); + + return Stream.of( + Arguments.of( + metadata, + new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0), + new Pose2d(28.5, 35.9, 0)), + Arguments.of( + metadata, + new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, -45, 0), + new Pose2d(35.5, 18.9, 0)), + Arguments.of( + metadata, + new AprilTagPoseFtc(0, 0, 0, -45, 0, 0, 24, -45, 0), + new Pose2d(31.1, 41.3, -1)) + ); + } +} \ No newline at end of file