From 038974fff032b843beda7da60c31d08ec700c69e Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Fri, 8 Mar 2024 06:24:50 -0600 Subject: [PATCH] fdfddfd --- build.gradle | 4 +- settings.gradle | 2 +- .../localization/AprilTagPoseEstimator.java | 63 +---------- .../AprilTagPoseEstimatorTest.java | 105 ------------------ 4 files changed, 8 insertions(+), 166 deletions(-) delete mode 100644 src/test/java/com/tearabite/ielib/localization/localization/AprilTagPoseEstimatorTest.java diff --git a/build.gradle b/build.gradle index 4bc7d9f..99d6928 100644 --- a/build.gradle +++ b/build.gradle @@ -47,11 +47,11 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.2' - //noinspection AnnotationProcessorOnCompilePath - compileOnly 'org.projectlombok:lombok:1.18.30' + implementation 'org.projectlombok:lombok:1.18.30' annotationProcessor 'org.projectlombok:lombok:1.18.30' implementation 'com.acmerobotics.roadrunner:core:0.5.6' + api project(':ielib-core') } task androidSourcesJar(type: Jar) { diff --git a/settings.gradle b/settings.gradle index b4135a7..b44ff4e 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1 +1 @@ -include ':ftctearabits' \ No newline at end of file +include ':ielib' \ No newline at end of file diff --git a/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimator.java b/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimator.java index bd39327..7ece205 100644 --- a/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimator.java +++ b/src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimator.java @@ -1,13 +1,7 @@ package com.tearabite.ielib.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 com.acmerobotics.roadrunner.geometry.Pose2d; +import com.tearabite.ielib.localization.AprilTagPoseEstimatorCore; import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; @@ -16,16 +10,12 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; import java.security.InvalidParameterException; -import lombok.AllArgsConstructor; -import lombok.Builder; import lombok.Getter; -import lombok.NoArgsConstructor; -import lombok.Setter; +import lombok.experimental.SuperBuilder; -@NoArgsConstructor -@AllArgsConstructor -@Builder(toBuilder = true) -public class AprilTagPoseEstimator { +@Getter +@SuperBuilder(toBuilder=true) +public class AprilTagPoseEstimator extends AprilTagPoseEstimatorCore { /* * This class is used to estimate the pose of the robot using the AprilTagDetection object. @@ -35,8 +25,6 @@ public class AprilTagPoseEstimator { * https://www.desmos.com/calculator/n2iyatwssg */ - @Getter @Setter @Builder.Default private Pose2d robotOffset = new Pose2d(0,0,0); - /** * Estimates the pose of the robot using the AprilTagDetection object. * @param detection The AprilTagDetection object @@ -61,45 +49,4 @@ public class AprilTagPoseEstimator { fieldOrientation.z, fieldOrientation.w); } - - /** - * Estimates the pose of the robot using the AprilTagDetection object. - * @param yaw The rotation about Z of the AprilTag within the camera's frame. - * @param bearing The angle between line projected to center of the april tag and the camera's center projection line. - * @param range The distance from the center of the camera lens to the center of the AprilTag. - * @param Tx The x position (in FTC field coordinates) of the AprilTag - * @param Ty The y position (in FTC field coordinates) of the AprilTag - * @param Ta The a component of the quaternion describing the orientation of the AprilTag - * @param Tb The b component of the quaternion describing the orientation of the AprilTag - * @param Tc The c component of the quaternion describing the orientation of the AprilTag - * @param Td The d component of the quaternion describing the orientation of the AprilTag - * @return The estimated pose of the robot. Heading is reported in radians. - */ - private Pose2d estimatePose(double yaw, double bearing, double range, double Tx, double Ty, double Ta, double Tb, double Tc, double Td) { - double Tyaw = getTYaw(Ta, Tb, Tc, Td); - double ch = (PI / 2) - Tyaw + yaw; - double cx = Tx + range * cos(bearing) * cos(ch) + range * sin(bearing) * sin(ch); - double cy = Ty + range * cos(bearing) * sin(ch) - range * sin(bearing) * cos(ch); - - double offsetX = this.robotOffset.getX(); - double offsetY = this.robotOffset.getY(); - double Cyaw = this.robotOffset.getHeading(); - double rh = (PI / 2) + Tyaw + yaw - Cyaw; - double rx = cx + offsetX * cos(rh) - offsetY * sin(rh); - double ry = cy + offsetX * sin(rh) + offsetY * cos(rh); - - 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/test/java/com/tearabite/ielib/localization/localization/AprilTagPoseEstimatorTest.java b/src/test/java/com/tearabite/ielib/localization/localization/AprilTagPoseEstimatorTest.java deleted file mode 100644 index c8c9907..0000000 --- a/src/test/java/com/tearabite/ielib/localization/localization/AprilTagPoseEstimatorTest.java +++ /dev/null @@ -1,105 +0,0 @@ -package com.tearabite.ielib.localization.localization; - -import static org.junit.jupiter.api.Assertions.assertThrows; -import static org.junit.jupiter.api.Assertions.fail; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.tearabite.ielib.localization.AprilTagPoseEstimator; - -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; - -public class AprilTagPoseEstimatorTest { - - private static 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)); - - @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 robotOffsets, Pose2d expectedPose) { - AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(robotOffsets); - AprilTagDetection detection = new AprilTagDetection( - 1, - 0, - 0, - null, - null, - metadata, - poseFtc, - null, - 0); - - Pose2d estimatedPose = estimator.estimatePose(detection); - - assertIsClose(estimatedPose, expectedPose); - } - - private static Stream provideEstimatePosTestValues() { - return Stream.of( - Arguments.of( - metadata, - new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0), - new Pose2d(-7.77, 0.505, 0), - new Pose2d(28.5, 35.9, 0)), - Arguments.of( - metadata, - new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, -45, 0), - new Pose2d(-7.77, 0.505, 0), - new Pose2d(35.5, 18.9, 0)), - Arguments.of( - metadata, - new AprilTagPoseFtc(0, 0, 0, -45, 0, 0, 24, -45, 0), - new Pose2d(-7.77, 0.505, 0), - new Pose2d(31.1, 41.3, -Math.PI / 4)), - - Arguments.of( - metadata, - new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0), - new Pose2d(8.9, -1.5, Math.PI), - new Pose2d(27.4, 36.9, Math.PI)), - - Arguments.of( - metadata, - new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0), - new Pose2d(8.9, -1.5, Math.PI * 3 / 4), - new Pose2d(28.9, 30.2, -Math.PI * 3 / 4)) - ); - } - - 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.getX(), b.getX()) - && isClose(a.getY(), b.getY()) - && isClose(a.getHeading(), b.getHeading()); - - if (!isClose) { - fail(String.format("Expected (%.1f, %.1f, %.1f) to be close to (%.1f, %.1f, %.1f)", - a.getX(), a.getY(), a.getHeading(), - b.getX(), b.getY(), b.getHeading())); - } - } -} \ No newline at end of file