Let's gooo

build.gradle

@@ -50,7 +50,7 @@ dependencies {
     implementation 'org.projectlombok:lombok:1.18.30'
     annotationProcessor 'org.projectlombok:lombok:1.18.30'
 
-    implementation 'com.acmerobotics.roadrunner:core:0.5.6'
+    api 'com.acmerobotics.roadrunner:core:0.5.6'
     api project(':ielib-core')
 }
 

src/main/java/com/tearabite/ielib/localization/AprilTagPoseEstimator.java

@@ -1,7 +1,6 @@
 package com.tearabite.ielib.localization;
 
 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;
@@ -10,11 +9,16 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
 
 import java.security.InvalidParameterException;
 
+import lombok.AllArgsConstructor;
+import lombok.Builder;
 import lombok.Getter;
-import lombok.experimental.SuperBuilder;
+import lombok.NoArgsConstructor;
+import lombok.Setter;
 
 @Getter
-@SuperBuilder(toBuilder=true)
+@NoArgsConstructor
+@AllArgsConstructor
+@Builder(toBuilder = true)
 public class AprilTagPoseEstimator extends AprilTagPoseEstimatorCore {
 
     /*
@@ -25,6 +29,8 @@ public class AprilTagPoseEstimator extends AprilTagPoseEstimatorCore {
     * https://www.desmos.com/calculator/n2iyatwssg
      */
 
+    @Setter private Pose2d robotOffset;
+
     /**
      * Estimates the pose of the robot using the AprilTagDetection object.
      * @param detection The AprilTagDetection object
@@ -47,6 +53,7 @@ public class AprilTagPoseEstimator extends AprilTagPoseEstimatorCore {
                 fieldOrientation.x,
                 fieldOrientation.y,
                 fieldOrientation.z,
-                fieldOrientation.w);
+                fieldOrientation.w,
+                this.robotOffset);
     }
 }

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

@@ -0,0 +1,104 @@
+package com.tearabite.ielib.localization;
+
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import com.acmerobotics.roadrunner.geometry.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 {
+
+    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()));
+        }
+    }
+}