mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-04 03:11:43 +00:00
SCRIPT: wpiformat
This commit is contained in:
PJ Reiniger
committed by
Peter Johnson
Peter Johnson
parent
ae6bdc9d25
commit
2109161534
@@ -45,7 +45,8 @@ 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();
|
||||
@@ -66,9 +67,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,9 +94,10 @@ void testFollowTrajectory(
|
||||
|
||||
auto xhat = estimator.UpdateWithTime(
|
||||
t,
|
||||
wpi::math::Rotation3d{groundTruthState.pose.Rotation() +
|
||||
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
|
||||
trajectory.InitialPose().Rotation()},
|
||||
wpi::math::Rotation3d{
|
||||
groundTruthState.pose.Rotation() +
|
||||
wpi::math::Rotation2d{distribution(generator) * 0.05_rad} -
|
||||
trajectory.InitialPose().Rotation()},
|
||||
leftDistance, rightDistance);
|
||||
|
||||
if (debug) {
|
||||
@@ -119,7 +122,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;
|
||||
@@ -127,9 +131,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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -155,49 +159,55 @@ void testFollowTrajectory(
|
||||
TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
|
||||
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
|
||||
|
||||
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{},
|
||||
{0.02, 0.02, 0.02, 0.01},
|
||||
{0.1, 0.1, 0.1, 0.1}};
|
||||
wpi::math::DifferentialDrivePoseEstimator3d estimator{
|
||||
kinematics,
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{},
|
||||
{0.02, 0.02, 0.02, 0.01},
|
||||
{0.1, 0.1, 0.1, 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_mps, 2_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_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(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
|
||||
wpi::math::DifferentialDriveKinematics kinematics{1.0_m};
|
||||
|
||||
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{},
|
||||
{0.02, 0.02, 0.02, 0.01},
|
||||
{0.1, 0.1, 0.1, 0.1}};
|
||||
wpi::math::DifferentialDrivePoseEstimator3d estimator{
|
||||
kinematics,
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{},
|
||||
{0.02, 0.02, 0.02, 0.01},
|
||||
{0.1, 0.1, 0.1, 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_mps, 2_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_mps_sq));
|
||||
|
||||
for (wpi::units::degree_t offset_direction_degs = 0_deg;
|
||||
offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) {
|
||||
@@ -206,21 +216,21 @@ TEST(DifferentialDrivePoseEstimator3dTest, 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -238,7 +248,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{1_m, 2_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
|
||||
wpi::math::Pose3d{1_m, 2_m, 0_m,
|
||||
wpi::math::Rotation3d{0_deg, 0_deg, 270_deg}},
|
||||
{0.02, 0.02, 0.02, 0.01},
|
||||
{0.1, 0.1, 0.1, 0.1}};
|
||||
|
||||
@@ -246,11 +257,16 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) {
|
||||
|
||||
for (int i = 0; i < 1000; i++) {
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose3d{0_m, 0_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s);
|
||||
wpi::math::Pose3d{0_m, 0_m, 0_m,
|
||||
wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}},
|
||||
0_s);
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose3d{3_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s);
|
||||
wpi::math::Pose3d{3_m, 1_m, 0_m,
|
||||
wpi::math::Rotation3d{0_deg, 0_deg, 90_deg}},
|
||||
0_s);
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose3d{2_m, 4_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
|
||||
wpi::math::Pose3d{2_m, 4_m, 0_m,
|
||||
wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}},
|
||||
0_s);
|
||||
}
|
||||
|
||||
@@ -289,8 +305,13 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
|
||||
wpi::math::DifferentialDriveKinematics kinematics{1_m};
|
||||
|
||||
wpi::math::DifferentialDrivePoseEstimator3d estimator{
|
||||
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::Pose3d{},
|
||||
{0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}};
|
||||
kinematics,
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{},
|
||||
{0.1, 0.1, 0.1, 0.1},
|
||||
{0.45, 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) {
|
||||
@@ -301,8 +322,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
|
||||
|
||||
// Apply a vision measurement from 3 seconds ago
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose3d{10_m, 10_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s,
|
||||
{0.1, 0.1, 0.1, 0.1});
|
||||
wpi::math::Pose3d{10_m, 10_m, 0_m,
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, 0.1_rad}},
|
||||
1_s, {0.1, 0.1, 0.1, 0.1});
|
||||
|
||||
EXPECT_NEAR(odometryPose.X().value(),
|
||||
estimator.GetEstimatedPosition().X().value(), 1e-6);
|
||||
@@ -320,9 +342,13 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) {
|
||||
|
||||
TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
|
||||
wpi::math::DifferentialDriveKinematics kinematics{1_m};
|
||||
wpi::math::DifferentialDrivePoseEstimator3d estimator{
|
||||
kinematics, wpi::math::Rotation3d{}, 0_m, 0_m, wpi::math::Pose3d{},
|
||||
{1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}};
|
||||
wpi::math::DifferentialDrivePoseEstimator3d estimator{kinematics,
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{},
|
||||
{1.0, 1.0, 1.0, 1.0},
|
||||
{1.0, 1.0, 1.0, 1.0}};
|
||||
|
||||
// Returns empty when null
|
||||
EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s));
|
||||
@@ -331,35 +357,45 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
|
||||
// Add a tiny tolerance for the upper bound because of floating point rounding
|
||||
// error
|
||||
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
|
||||
estimator.UpdateWithTime(wpi::units::second_t{time}, wpi::math::Rotation3d{},
|
||||
wpi::units::meter_t{time}, wpi::units::meter_t{time});
|
||||
estimator.UpdateWithTime(wpi::units::second_t{time},
|
||||
wpi::math::Rotation3d{}, wpi::units::meter_t{time},
|
||||
wpi::units::meter_t{time});
|
||||
}
|
||||
|
||||
// Sample at an added time
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
// Sample between updates (test interpolation)
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
// Sampling before the oldest value returns the oldest value
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
// Sampling after the newest value returns the newest value
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(2.5_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(2.5_s));
|
||||
|
||||
// Add a vision measurement after the odometry measurements (while keeping all
|
||||
// of the old odometry measurements)
|
||||
estimator.AddVisionMeasurement(
|
||||
wpi::math::Pose3d{2_m, 0_m, 0_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s);
|
||||
wpi::math::Pose3d{2_m, 0_m, 0_m,
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
|
||||
2.2_s);
|
||||
|
||||
// Make sure nothing changed (except the newest value)
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{1.02_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{1.01_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
EXPECT_EQ(
|
||||
std::optional(wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
|
||||
// Add a vision measurement before the odometry measurements that's still in
|
||||
// the buffer
|
||||
@@ -367,13 +403,17 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) {
|
||||
wpi::math::Pose3d{1_m, 0.2_m, 0_m, wpi::math::Rotation3d{}}, 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::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{1.02_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(1.02_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{1.01_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(1.01_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{1_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(0.5_s));
|
||||
EXPECT_EQ(std::optional(wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
EXPECT_EQ(std::optional(
|
||||
wpi::math::Pose3d{2_m, 0.1_m, 0_m, wpi::math::Rotation3d{}}),
|
||||
estimator.SampleAt(2.5_s));
|
||||
}
|
||||
|
||||
@@ -384,7 +424,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
|
||||
wpi::math::Rotation3d{},
|
||||
0_m,
|
||||
0_m,
|
||||
wpi::math::Pose3d{-1_m, -1_m, -1_m, wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
|
||||
wpi::math::Pose3d{-1_m, -1_m, -1_m,
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, 1_rad}},
|
||||
{1.0, 1.0, 1.0, 1.0},
|
||||
{1.0, 1.0, 1.0, 1.0}};
|
||||
|
||||
@@ -397,8 +438,9 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) {
|
||||
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value());
|
||||
|
||||
// Test reset position
|
||||
estimator.ResetPosition(wpi::math::Rotation3d{}, 1_m, 1_m,
|
||||
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
|
||||
estimator.ResetPosition(
|
||||
wpi::math::Rotation3d{}, 1_m, 1_m,
|
||||
wpi::math::Pose3d{1_m, 0_m, 0_m, wpi::math::Rotation3d{}});
|
||||
|
||||
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value());
|
||||
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
|
||||
|
||||
Reference in New Issue
Block a user