mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
SCRIPT: wpiformat
This commit is contained in:
PJ Reiniger
committed by
Peter Johnson
Peter Johnson
parent
ae6bdc9d25
commit
2109161534
@@ -30,7 +30,8 @@ void testFollowTrajectory(
|
||||
const wpi::units::second_t dt, const wpi::units::second_t kVisionUpdateRate,
|
||||
const wpi::units::second_t kVisionUpdateDelay, const bool checkError,
|
||||
const bool debug) {
|
||||
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{wpi::util::empty_array};
|
||||
wpi::util::array<wpi::math::SwerveModulePosition, 4> positions{
|
||||
wpi::util::empty_array};
|
||||
|
||||
estimator.ResetPosition(wpi::math::Rotation2d{}, positions, startingPose);
|
||||
|
||||
@@ -40,14 +41,16 @@ void testFollowTrajectory(
|
||||
wpi::units::second_t t = 0_s;
|
||||
|
||||
std::vector<std::pair<wpi::units::second_t, wpi::math::Pose2d>> visionPoses;
|
||||
std::vector<std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
|
||||
std::vector<
|
||||
std::tuple<wpi::units::second_t, wpi::units::second_t, wpi::math::Pose2d>>
|
||||
visionLog;
|
||||
|
||||
double maxError = -std::numeric_limits<double>::max();
|
||||
double errorSum = 0;
|
||||
|
||||
if (debug) {
|
||||
wpi::util::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
|
||||
wpi::util::print(
|
||||
"time, est_x, est_y, est_theta, true_x, true_y, true_theta\n");
|
||||
}
|
||||
|
||||
while (t < trajectory.TotalTime()) {
|
||||
@@ -59,9 +62,10 @@ void testFollowTrajectory(
|
||||
visionPoses.back().first + kVisionUpdateRate < t) {
|
||||
auto visionPose =
|
||||
visionMeasurementGenerator(groundTruthState) +
|
||||
wpi::math::Transform2d{wpi::math::Translation2d{distribution(generator) * 0.1_m,
|
||||
distribution(generator) * 0.1_m},
|
||||
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
|
||||
wpi::math::Transform2d{
|
||||
wpi::math::Translation2d{distribution(generator) * 0.1_m,
|
||||
distribution(generator) * 0.1_m},
|
||||
wpi::math::Rotation2d{distribution(generator) * 0.05_rad}};
|
||||
visionPoses.push_back({t, visionPose});
|
||||
}
|
||||
|
||||
@@ -92,11 +96,11 @@ void testFollowTrajectory(
|
||||
positions);
|
||||
|
||||
if (debug) {
|
||||
wpi::util::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
|
||||
xhat.Y().value(), xhat.Rotation().Radians().value(),
|
||||
groundTruthState.pose.X().value(),
|
||||
groundTruthState.pose.Y().value(),
|
||||
groundTruthState.pose.Rotation().Radians().value());
|
||||
wpi::util::print(
|
||||
"{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(),
|
||||
xhat.Y().value(), xhat.Rotation().Radians().value(),
|
||||
groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(),
|
||||
groundTruthState.pose.Rotation().Radians().value());
|
||||
}
|
||||
|
||||
double error = groundTruthState.pose.Translation()
|
||||
@@ -112,7 +116,8 @@ void testFollowTrajectory(
|
||||
}
|
||||
|
||||
if (debug) {
|
||||
wpi::util::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n");
|
||||
wpi::util::print(
|
||||
"apply_time, measured_time, vision_x, vision_y, vision_theta\n");
|
||||
|
||||
wpi::units::second_t apply_time;
|
||||
wpi::units::second_t measure_time;
|
||||
@@ -120,9 +125,9 @@ void testFollowTrajectory(
|
||||
for (auto record : visionLog) {
|
||||
std::tie(apply_time, measure_time, vision_pose) = record;
|
||||
wpi::util::print("{}, {}, {}, {}, {}\n", apply_time.value(),
|
||||
measure_time.value(), vision_pose.X().value(),
|
||||
vision_pose.Y().value(),
|
||||
vision_pose.Rotation().Radians().value());
|
||||
measure_time.value(), vision_pose.X().value(),
|
||||
vision_pose.Y().value(),
|
||||
vision_pose.Rotation().Radians().value());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -144,7 +149,8 @@ void testFollowTrajectory(
|
||||
TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::Translation2d{-1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, 1_m}};
|
||||
|
||||
wpi::math::SwerveModulePosition fl;
|
||||
wpi::math::SwerveModulePosition fr;
|
||||
@@ -152,31 +158,35 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
|
||||
wpi::math::SwerveModulePosition br;
|
||||
|
||||
wpi::math::SwerveDrivePoseEstimator<4> estimator{
|
||||
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
|
||||
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
|
||||
|
||||
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 135_deg},
|
||||
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
|
||||
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
|
||||
wpi::math::Trajectory trajectory =
|
||||
wpi::math::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
|
||||
wpi::math::Pose2d{3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 135_deg},
|
||||
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
|
||||
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
|
||||
|
||||
testFollowTrajectory(
|
||||
kinematics, estimator, trajectory,
|
||||
[&](wpi::math::Trajectory::State& state) {
|
||||
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
|
||||
state.velocity * state.curvature};
|
||||
state.velocity * state.curvature};
|
||||
},
|
||||
[&](wpi::math::Trajectory::State& state) { return state.pose; },
|
||||
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
|
||||
20_ms, 100_ms, 250_ms, true, false);
|
||||
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
|
||||
{0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms, 250_ms, true,
|
||||
false);
|
||||
}
|
||||
|
||||
TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::Translation2d{-1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, 1_m}};
|
||||
|
||||
wpi::math::SwerveModulePosition fl;
|
||||
wpi::math::SwerveModulePosition fr;
|
||||
@@ -184,15 +194,17 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
|
||||
wpi::math::SwerveModulePosition br;
|
||||
|
||||
wpi::math::SwerveDrivePoseEstimator<4> estimator{
|
||||
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
|
||||
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
|
||||
|
||||
wpi::math::Trajectory trajectory = wpi::math::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg}, wpi::math::Pose2d{3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 135_deg},
|
||||
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
|
||||
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
|
||||
wpi::math::Trajectory trajectory =
|
||||
wpi::math::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector{wpi::math::Pose2d{0_m, 0_m, 45_deg},
|
||||
wpi::math::Pose2d{3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 135_deg},
|
||||
wpi::math::Pose2d{-3_m, 0_m, -90_deg},
|
||||
wpi::math::Pose2d{0_m, 0_m, 45_deg}},
|
||||
wpi::math::TrajectoryConfig(2_mps, 2.0_mps_sq));
|
||||
|
||||
for (wpi::units::degree_t offset_direction_degs = 0_deg;
|
||||
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
|
||||
@@ -201,21 +213,21 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
|
||||
auto pose_offset = wpi::math::Rotation2d{offset_direction_degs};
|
||||
auto heading_offset = wpi::math::Rotation2d{offset_heading_degs};
|
||||
|
||||
auto initial_pose =
|
||||
trajectory.InitialPose() +
|
||||
wpi::math::Transform2d{wpi::math::Translation2d{pose_offset.Cos() * 1_m,
|
||||
pose_offset.Sin() * 1_m},
|
||||
heading_offset};
|
||||
auto initial_pose = trajectory.InitialPose() +
|
||||
wpi::math::Transform2d{
|
||||
wpi::math::Translation2d{pose_offset.Cos() * 1_m,
|
||||
pose_offset.Sin() * 1_m},
|
||||
heading_offset};
|
||||
|
||||
testFollowTrajectory(
|
||||
kinematics, estimator, trajectory,
|
||||
[&](wpi::math::Trajectory::State& state) {
|
||||
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
|
||||
state.velocity * state.curvature};
|
||||
state.velocity * state.curvature};
|
||||
},
|
||||
[&](wpi::math::Trajectory::State& state) { return state.pose; },
|
||||
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms, 100_ms,
|
||||
250_ms, false, false);
|
||||
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
|
||||
100_ms, 250_ms, false, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -228,7 +240,8 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
|
||||
// pose would converge to that measurement.
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::Translation2d{-1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, 1_m}};
|
||||
|
||||
wpi::math::SwerveModulePosition fl;
|
||||
wpi::math::SwerveModulePosition fr;
|
||||
@@ -236,9 +249,12 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
|
||||
wpi::math::SwerveModulePosition br;
|
||||
|
||||
wpi::math::SwerveDrivePoseEstimator<4> estimator{
|
||||
kinematics, wpi::math::Rotation2d{},
|
||||
{fl, fr, bl, br}, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
|
||||
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
|
||||
kinematics,
|
||||
wpi::math::Rotation2d{},
|
||||
{fl, fr, bl, br},
|
||||
wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
|
||||
{0.1, 0.1, 0.1},
|
||||
{0.45, 0.45, 0.45}};
|
||||
|
||||
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, {fl, fr, bl, br});
|
||||
|
||||
@@ -282,7 +298,8 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
|
||||
TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::Translation2d{-1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, 1_m}};
|
||||
|
||||
wpi::math::SwerveModulePosition fl;
|
||||
wpi::math::SwerveModulePosition fr;
|
||||
@@ -290,8 +307,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
|
||||
wpi::math::SwerveModulePosition br;
|
||||
|
||||
wpi::math::SwerveDrivePoseEstimator<4> estimator{
|
||||
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
|
||||
kinematics, wpi::math::Rotation2d{}, {fl, fr, bl, br},
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
|
||||
|
||||
// Add enough measurements to fill up the buffer
|
||||
for (auto time = 0_s; time < 4_s; time += 20_ms) {
|
||||
@@ -302,7 +319,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
|
||||
|
||||
// Apply a vision measurement from 3 seconds ago
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m}, wpi::math::Rotation2d{0.1_rad}},
|
||||
wpi::math::Pose2d{wpi::math::Translation2d{10_m, 10_m},
|
||||
wpi::math::Rotation2d{0.1_rad}},
|
||||
1_s, {0.1, 0.1, 0.1});
|
||||
|
||||
EXPECT_NEAR(odometryPose.X().value(),
|
||||
@@ -317,7 +335,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
|
||||
TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::Translation2d{-1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::SwerveDrivePoseEstimator estimator{
|
||||
kinematics,
|
||||
wpi::math::Rotation2d{},
|
||||
@@ -335,20 +354,26 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
|
||||
// error
|
||||
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
|
||||
wpi::util::array<wpi::math::SwerveModulePosition, 4> wheelPositions{
|
||||
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
|
||||
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
|
||||
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}},
|
||||
wpi::math::SwerveModulePosition{wpi::units::meter_t{time}, wpi::math::Rotation2d{}}}};
|
||||
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
|
||||
wheelPositions);
|
||||
{wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
|
||||
wpi::math::Rotation2d{}},
|
||||
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
|
||||
wpi::math::Rotation2d{}},
|
||||
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
|
||||
wpi::math::Rotation2d{}},
|
||||
wpi::math::SwerveModulePosition{wpi::units::meter_t{time},
|
||||
wpi::math::Rotation2d{}}}};
|
||||
estimator.UpdateWithTime(wpi::units::second_t{time},
|
||||
wpi::math::Rotation2d{}, wheelPositions);
|
||||
}
|
||||
|
||||
// Sample at an added time
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
// Sample between updates (test interpolation)
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
// Sampling before the oldest value returns the oldest value
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
@@ -358,37 +383,44 @@ TEST(SwerveDrivePoseEstimatorTest, TestSampleAt) {
|
||||
|
||||
// Add a vision measurement after the odometry measurements (while keeping all
|
||||
// of the old odometry measurements)
|
||||
estimator.AddVisionMeasurement(wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}},
|
||||
2.2_s);
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose2d{2_m, 0_m, wpi::math::Rotation2d{1_rad}}, 2.2_s);
|
||||
|
||||
// Make sure nothing changed (except the newest value)
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{1.02_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{1.01_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
|
||||
// Add a vision measurement before the odometry measurements that's still in
|
||||
// the buffer
|
||||
estimator.AddVisionMeasurement(wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}},
|
||||
0.9_s);
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose2d{1_m, 0.2_m, wpi::math::Rotation2d{}}, 0.9_s);
|
||||
|
||||
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(2.5_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{1.02_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{1.01_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{1_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose2d{2_m, 0.1_m, wpi::math::Rotation2d{}}),
|
||||
estimator.SampleAt(2.5_s));
|
||||
}
|
||||
|
||||
TEST(SwerveDrivePoseEstimatorTest, TestReset) {
|
||||
wpi::math::SwerveDriveKinematics<4> kinematics{
|
||||
wpi::math::Translation2d{1_m, 1_m}, wpi::math::Translation2d{1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, -1_m}, wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::Translation2d{-1_m, -1_m},
|
||||
wpi::math::Translation2d{-1_m, 1_m}};
|
||||
wpi::math::SwerveDrivePoseEstimator estimator{
|
||||
kinematics,
|
||||
wpi::math::Rotation2d{},
|
||||
@@ -406,7 +438,8 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
|
||||
|
||||
// Test reset position
|
||||
{
|
||||
wpi::math::SwerveModulePosition modulePosition{1_m, wpi::math::Rotation2d{}};
|
||||
wpi::math::SwerveModulePosition modulePosition{1_m,
|
||||
wpi::math::Rotation2d{}};
|
||||
estimator.ResetPosition(
|
||||
wpi::math::Rotation2d{},
|
||||
{modulePosition, modulePosition, modulePosition, modulePosition},
|
||||
@@ -420,9 +453,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
|
||||
|
||||
// Test orientation and wheel positions
|
||||
{
|
||||
wpi::math::SwerveModulePosition modulePosition{2_m, wpi::math::Rotation2d{}};
|
||||
wpi::math::SwerveModulePosition modulePosition{2_m,
|
||||
wpi::math::Rotation2d{}};
|
||||
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
|
||||
modulePosition, modulePosition});
|
||||
modulePosition, modulePosition});
|
||||
}
|
||||
|
||||
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
|
||||
@@ -441,9 +475,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
|
||||
|
||||
// Test orientation
|
||||
{
|
||||
wpi::math::SwerveModulePosition modulePosition{3_m, wpi::math::Rotation2d{}};
|
||||
wpi::math::SwerveModulePosition modulePosition{3_m,
|
||||
wpi::math::Rotation2d{}};
|
||||
estimator.Update(wpi::math::Rotation2d{}, {modulePosition, modulePosition,
|
||||
modulePosition, modulePosition});
|
||||
modulePosition, modulePosition});
|
||||
}
|
||||
|
||||
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
|
||||
|
||||
Reference in New Issue
Block a user