eaglesfe
/
ielib
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix robot heading for AprilTagPoseEstimator

This commit is contained in:
Scott Barnes 2024-02-28 16:03:03 -06:00
parent d9bc67376d
commit deed5942d5
3 changed files with 61 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
build.gradle
View File

@ -32,6 +32,11 @@ android {
release { }
debug { }
}
testOptions {
unitTests.all {
useJUnitPlatform()
}
}
}
repositories {

11
src/main/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimator.java
View File

@ -6,7 +6,6 @@ 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;
@ -82,13 +81,13 @@ public class AprilTagPoseEstimator {
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 offsetX = this.robotOffset.position.x;
double offsetY = 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);
double rx = cx + offsetX * cos(robotRotation) - offsetY * sin(robotRotation);
double ry = cy + offsetX * sin(robotRotation) + offsetY * cos(robotRotation);
double rh = (yaw - cameraRotationOnRobot + 2 * PI) % (2 * PI);
return new Pose2d(rx, ry, rh);
}

78
src/test/java/com/tearabite/ftctearabits/localization/AprilTagPoseEstimatorTest.java
View File

@ -21,6 +21,13 @@ 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();
@ -29,15 +36,56 @@ class AprilTagPoseEstimatorTest {
@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);
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;
}
@ -53,28 +101,4 @@ class AprilTagPoseEstimatorTest {
b.position.x, b.position.y, b.heading.toDouble()));
}
}
private static Stream<Arguments> 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))
);
}
}