fdfddfd

build.gradle

@@ -47,11 +47,11 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.2'
 
-    //noinspection AnnotationProcessorOnCompilePath
+    implementation 'org.projectlombok:lombok:1.18.30'
-    compileOnly '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) {

settings.gradle

@@ -1 +1 @@
-include ':ftctearabits'
+include ':ielib'

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.experimental.SuperBuilder;
-import lombok.Setter;
 
-@NoArgsConstructor
+@Getter
-@AllArgsConstructor
+@SuperBuilder(toBuilder=true)
-@Builder(toBuilder = true)
+public class AprilTagPoseEstimator extends AprilTagPoseEstimatorCore {
-public class AprilTagPoseEstimator {
 
     /*
     * 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))));
-    }
 }

src/test/java/com/tearabite/ielib/localization/localization/AprilTagPoseEstimatorTest.java

@@ -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<Arguments> 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()));
-        }
-    }
-}