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] Allow multiple vision measurements from same timestamp (#4917)

Browse Source 
Co-authored-by: Jordan McMichael <jlmcmchl@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2023-01-12 02:04:30 -05:00
committed by GitHub
parent befd12911c
commit f7f19207e0
13 changed files with 409 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,6 +8,7 @@
#include <tuple>
#include <fmt/format.h>
#include <wpi/timestamp.h>
#include "frc/estimator/SwerveDrivePoseEstimator.h"
#include "frc/geometry/Pose2d.h"
@@ -215,3 +216,62 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
}
}
}
TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements appled at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::SwerveDriveKinematics<4> kinematics{
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::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{},
{fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br});
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}