mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +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 bool debug) {
|
||||
wpi::math::MecanumDriveWheelPositions wheelPositions{};
|
||||
|
||||
estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions, startingPose);
|
||||
estimator.ResetPosition(wpi::math::Rotation2d{}, wheelPositions,
|
||||
startingPose);
|
||||
|
||||
std::default_random_engine generator;
|
||||
std::normal_distribution<double> distribution(0.0, 1.0);
|
||||
@@ -38,14 +39,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()) {
|
||||
@@ -57,9 +60,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});
|
||||
}
|
||||
|
||||
@@ -90,11 +94,11 @@ void testFollowTrajectory(
|
||||
wheelPositions);
|
||||
|
||||
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()
|
||||
@@ -110,7 +114,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;
|
||||
@@ -118,9 +123,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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -142,49 +147,55 @@ void testFollowTrajectory(
|
||||
TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
|
||||
wpi::math::MecanumDriveKinematics 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::MecanumDriveWheelPositions wheelPositions;
|
||||
|
||||
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
|
||||
wheelPositions, wpi::math::Pose2d{},
|
||||
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
|
||||
wpi::math::MecanumDrivePoseEstimator estimator{
|
||||
kinematics, wpi::math::Rotation2d{}, wheelPositions,
|
||||
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.0_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.0_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; },
|
||||
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,
|
||||
100_ms, 250_ms, true, false);
|
||||
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
|
||||
20_ms, 100_ms, 250_ms, true, false);
|
||||
}
|
||||
|
||||
TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
|
||||
wpi::math::MecanumDriveKinematics 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::MecanumDriveWheelPositions wheelPositions;
|
||||
|
||||
wpi::math::MecanumDrivePoseEstimator estimator{kinematics, wpi::math::Rotation2d{},
|
||||
wheelPositions, wpi::math::Pose2d{},
|
||||
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
|
||||
wpi::math::MecanumDrivePoseEstimator estimator{
|
||||
kinematics, wpi::math::Rotation2d{}, wheelPositions,
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
|
||||
|
||||
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.0_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.0_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) {
|
||||
@@ -193,21 +204,21 @@ TEST(MecanumDrivePoseEstimatorTest, 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -220,14 +231,18 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
|
||||
// pose would converge to that measurement.
|
||||
wpi::math::MecanumDriveKinematics 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::MecanumDriveWheelPositions wheelPositions;
|
||||
|
||||
wpi::math::MecanumDrivePoseEstimator estimator{
|
||||
kinematics, wpi::math::Rotation2d{},
|
||||
wheelPositions, wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
|
||||
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}};
|
||||
kinematics,
|
||||
wpi::math::Rotation2d{},
|
||||
wheelPositions,
|
||||
wpi::math::Pose2d{1_m, 2_m, wpi::math::Rotation2d{270_deg}},
|
||||
{0.1, 0.1, 0.1},
|
||||
{0.45, 0.45, 0.1}};
|
||||
|
||||
estimator.UpdateWithTime(0_s, wpi::math::Rotation2d{}, wheelPositions);
|
||||
|
||||
@@ -271,11 +286,16 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
|
||||
TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
|
||||
wpi::math::MecanumDriveKinematics 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::MecanumDrivePoseEstimator estimator{
|
||||
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
|
||||
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
|
||||
kinematics,
|
||||
wpi::math::Rotation2d{},
|
||||
wpi::math::MecanumDriveWheelPositions{},
|
||||
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) {
|
||||
@@ -287,7 +307,8 @@ TEST(MecanumDrivePoseEstimatorTest, 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(),
|
||||
@@ -302,10 +323,15 @@ TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
|
||||
TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
|
||||
wpi::math::MecanumDriveKinematics 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::MecanumDrivePoseEstimator estimator{
|
||||
kinematics, wpi::math::Rotation2d{}, wpi::math::MecanumDriveWheelPositions{},
|
||||
wpi::math::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}};
|
||||
kinematics,
|
||||
wpi::math::Rotation2d{},
|
||||
wpi::math::MecanumDriveWheelPositions{},
|
||||
wpi::math::Pose2d{},
|
||||
{1.0, 1.0, 1.0},
|
||||
{1.0, 1.0, 1.0}};
|
||||
|
||||
// Returns empty when null
|
||||
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
|
||||
@@ -315,18 +341,20 @@ TEST(MecanumDrivePoseEstimatorTest, TestSampleAt) {
|
||||
// error
|
||||
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
|
||||
wpi::math::MecanumDriveWheelPositions wheelPositions{
|
||||
wpi::units::meter_t{time}, wpi::units::meter_t{time}, wpi::units::meter_t{time},
|
||||
wpi::units::meter_t{time}};
|
||||
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation2d{},
|
||||
wheelPositions);
|
||||
wpi::units::meter_t{time}, wpi::units::meter_t{time},
|
||||
wpi::units::meter_t{time}, wpi::units::meter_t{time}};
|
||||
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));
|
||||
@@ -336,37 +364,44 @@ TEST(MecanumDrivePoseEstimatorTest, 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(MecanumDrivePoseEstimatorTest, TestReset) {
|
||||
wpi::math::MecanumDriveKinematics 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::MecanumDrivePoseEstimator estimator{
|
||||
kinematics,
|
||||
wpi::math::Rotation2d{},
|
||||
|
||||
Reference in New Issue
Block a user