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

[wpimath] Fix C++ pose estimator poseEstimate initialization (#7249)

Browse Source
This commit is contained in:
Joseph Eng
2024-10-21 22:29:04 -07:00
committed by GitHub
parent 40af8db28a
commit 6745fc7c2f
7 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -24,4 +24,6 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
: PoseEstimator<DifferentialDriveWheelSpeeds,
DifferentialDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {}
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {
ResetPose(m_odometryImpl.GetPose());
}

4
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -26,4 +26,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const wpi::array<double, 3>& visionMeasurementStdDevs)
: PoseEstimator<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {}
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {
ResetPose(m_odometryImpl.GetPose());
}

5
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -45,6 +45,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
/**
* Constructs a PoseEstimator.
*
* @warning The initial pose estimate will always be the default pose,
* regardless of the odometry's current pose.
*
* @param kinematics A correctly-configured kinematics object for your
* drivetrain.
* @param odometry A correctly-configured odometry object for your drivetrain.
@@ -60,7 +63,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) {
: m_odometry(odometry) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}

4
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -85,7 +85,9 @@ class SwerveDrivePoseEstimator
: PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
wpi::array<SwerveModulePosition, NumModules>>(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {
this->ResetPose(initialPose);
}
private:
SwerveDriveOdometry<NumModules> m_odometryImpl;

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

@@ -359,8 +359,18 @@ TEST(DifferentialDrivePoseEstimatorTest, TestSampleAt) {
TEST(DifferentialDrivePoseEstimatorTest, TestReset) {
frc::DifferentialDriveKinematics kinematics{1_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{},
{1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
kinematics,
frc::Rotation2d{},
0_m,
0_m,
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m,

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

@@ -368,8 +368,17 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) {
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{},
frc::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
kinematics,
frc::Rotation2d{},
frc::MecanumDriveWheelPositions{},
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m},

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

@@ -394,10 +394,15 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
frc::Rotation2d{},
{frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}},
frc::Pose2d{},
frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}},
{1.0, 1.0, 1.0},
{1.0, 1.0, 1.0}};
// Test initial pose
EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset position
{
frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}};