Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rework function signatures for Pose Estimation / Odometry (#4642)

Browse Source 
Forcing the use of SwerveModulePostition[] and wpi::array<SwerveModulePosition, len> in place of variadic function signatures.
This commit is contained in:
Jordan McMichael
2022-11-17 13:36:33 -05:00
committed by GitHub
parent 0401597d3b
commit ce4c45df13
9 changed files with 84 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -32,11 +32,12 @@ class SwerveDriveOdometryTest : public ::testing::Test {
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
SwerveModulePosition position{0.5_m, 0_deg};
m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
m_odometry.Update(0_deg, zero, zero, zero, zero);
m_odometry.Update(0_deg, {zero, zero, zero, zero});
auto pose = m_odometry.Update(0_deg, position, position, position, position);
auto pose =
m_odometry.Update(0_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -49,8 +50,8 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
SwerveModulePosition bl{18.85_m, -90_deg};
SwerveModulePosition br{42.15_m, -26.565_deg};
m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
auto pose = m_odometry.Update(90_deg, fl, fr, bl, br);
m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
@@ -58,11 +59,12 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
m_odometry.ResetPosition(Pose2d{}, 90_deg, zero, zero, zero, zero);
m_odometry.ResetPosition(Pose2d{}, 90_deg, {zero, zero, zero, zero});
SwerveModulePosition position{0.5_m, 0_deg};
auto pose = m_odometry.Update(90_deg, position, position, position, position);
auto pose =
m_odometry.Update(90_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -116,7 +118,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
fl, fr, bl, br);
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
@@ -178,7 +180,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
br.angle = groundTruthState.pose.Rotation();
auto xhat = odometry.Update(
frc::Rotation2d{distribution(generator) * 0.05_rad}, fl, fr, bl, br);
frc::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();