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 odometry APIs to improve feature parity (#4645)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Jordan McMichael
2022-11-18 23:42:00 -05:00
committed by GitHub
parent e2d49181da
commit 902e8686d3
53 changed files with 266 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -18,6 +18,8 @@
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d{},
0_m,
0_m,
frc::Pose2d{},
{0.02, 0.02, 0.01, 0.02, 0.02},
{0.01, 0.01, 0.001},

4
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -19,8 +19,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
frc::Pose2d{},
wheelPositions,
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
@@ -103,8 +103,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
frc::Pose2d{},
wheelPositions,
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},

4
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -23,8 +23,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
frc::Pose2d{},
{fl, fr, bl, br},
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
@@ -115,8 +115,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
frc::Pose2d{},
{fl, fr, bl, br},
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},

2
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View File

@@ -13,7 +13,7 @@ static constexpr double kEpsilon = 1E-9;
using namespace frc;
TEST(DifferentialDriveOdometryTest, EncoderDistances) {
DifferentialDriveOdometry odometry{45_deg};
DifferentialDriveOdometry odometry{45_deg, 0_m, 0_m};
const auto& pose =
odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi});

8
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -27,7 +27,7 @@ class MecanumDriveOdometryTest : public ::testing::Test {
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
odometry.ResetPosition(Pose2d{}, 0_rad, wheelDeltas);
odometry.ResetPosition(0_rad, wheelDeltas, Pose2d{});
odometry.Update(0_deg, wheelDeltas);
auto secondPose = odometry.Update(0_deg, wheelDeltas);
@@ -38,7 +38,7 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.ResetPosition(Pose2d{}, 0_rad, zero);
odometry.ResetPosition(0_rad, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
@@ -51,7 +51,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
odometry.ResetPosition(Pose2d{}, 0_rad, zero);
odometry.ResetPosition(0_rad, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
39.986_m};
odometry.Update(0_deg, MecanumDriveWheelPositions{});
@@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.ResetPosition(Pose2d{}, 90_deg, zero);
odometry.ResetPosition(90_deg, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};

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

@@ -32,7 +32,7 @@ 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(0_rad, {zero, zero, zero, zero}, Pose2d{});
m_odometry.Update(0_deg, {zero, zero, zero, zero});
@@ -50,7 +50,7 @@ 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});
m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
@@ -59,7 +59,7 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
m_odometry.ResetPosition(Pose2d{}, 90_deg, {zero, zero, zero, zero});
m_odometry.ResetPosition(90_deg, {zero, zero, zero, zero}, Pose2d{});
SwerveModulePosition position{0.5_m, 0_deg};