Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use stricter C++ type conversions (#4357)

Browse Source 
Now, implicit narrowing conversions are only used with wpi::Now(). This
also fixes clang-tidy warnings about C-style casts. For example:
```
== clang-tidy /__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc ==
/__w/allwpilib/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc:95:18: warning: C-style casts are discouraged; use static_cast/const_cast/reinterpret_cast [google-readability-casting]
  auto curTime = units::second_t(m_timer.Get());
                 ^
```
In that case at least, the cast was removed entirely since Get() already
returns a units::second_t.
This commit is contained in:
Tyler Veness
2022-08-17 13:42:36 -07:00
committed by GitHub
parent 151dabb2af
commit ac9be78e27
139 changed files with 547 additions and 593 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -108,12 +108,12 @@ TEST(MathUtilTest, AngleModulus) {
EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
0_rad, 1e-10);
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)),
units::radian_t(wpi::numbers::pi));
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::numbers::pi)),
units::radian_t(wpi::numbers::pi));
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::numbers::pi / 2)),
units::radian_t(wpi::numbers::pi / 2));
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::numbers::pi / 2)),
units::radian_t(-wpi::numbers::pi / 2));
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{5 * wpi::numbers::pi}),
units::radian_t{wpi::numbers::pi});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-5 * wpi::numbers::pi}),
units::radian_t{wpi::numbers::pi});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{wpi::numbers::pi / 2}),
units::radian_t{wpi::numbers::pi / 2});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{-wpi::numbers::pi / 2}),
units::radian_t{-wpi::numbers::pi / 2});
}

4
wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
View File

@@ -41,8 +41,8 @@ TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
Matrixd<2, 1> B{0, 1};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
modelDynamics, B, units::second_t(0.02)};
frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{modelDynamics,
B, 20_ms};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};

4
wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
View File

@@ -28,7 +28,7 @@ TEST(HolonomicDriveControllerTest, ReachesReference) {
units::radians_per_second_t{2.0 * wpi::numbers::pi},
units::radians_per_second_squared_t{wpi::numbers::pi}}}};
frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
@@ -60,7 +60,7 @@ TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
4_rad_per_s, 2_rad_per_s / 1_s}}};
frc::ChassisSpeeds speeds = controller.Calculate(
frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
frc::Pose2d{0_m, 0_m, 1.57_rad}, frc::Pose2d{}, 0_mps, 1.57_rad);
EXPECT_EQ(0, speeds.omega.value());
}

2
wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
View File

@@ -65,7 +65,7 @@ TEST(LTVDifferentialDriveControllerTest, ReachesReference) {
frc::LTVDifferentialDriveController controller{
plant, kTrackwidth, {0.0625, 0.125, 2.5, 0.95, 0.95}, {12.0, 12.0}, kDt};
frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};

2
wpimath/src/test/native/cpp/controller/LTVUnicycleControllerTest.cpp
View File

@@ -19,7 +19,7 @@ TEST(LTVUnicycleControllerTest, ReachesReference) {
constexpr auto kDt = 0.02_s;
frc::LTVUnicycleController controller{{0.0625, 0.125, 2.5}, {4.0, 4.0}, kDt};
frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};

3
wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
View File

@@ -16,8 +16,7 @@ TEST(LinearPlantInversionFeedforwardTest, Calculate) {
Matrixd<2, 2> A{{1, 0}, {0, 1}};
Matrixd<2, 1> B{0, 1};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
units::second_t(0.02)};
frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, 20_ms};
Vectord<2> r{2, 2};
Vectord<2> nextR{3, 3};

2
wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
View File

@@ -9,7 +9,7 @@ class PIDInputOutputTest : public testing::Test {
protected:
frc2::PIDController* controller;
void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
void SetUp() override { controller = new frc2::PIDController{0, 0, 0}; }
void TearDown() override { delete controller; }
};

2
wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
View File

@@ -18,7 +18,7 @@ static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi /
TEST(RamseteControllerTest, ReachesReference) {
frc::RamseteController controller{2.0 * 1_rad * 1_rad / (1_m * 1_m),
0.7 / 1_rad};
frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};

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

@@ -18,22 +18,21 @@
#include "units/time.h"
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
frc::Pose2d(),
frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d{},
frc::Pose2d{},
{0.02, 0.02, 0.01, 0.02, 0.02},
{0.01, 0.01, 0.001},
{0.1, 0.1, 0.01}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
frc::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
frc::DifferentialDriveOdometry odometry{frc::Rotation2d{}};
std::default_random_engine generator;
std::normal_distribution<double> distribution(0.0, 1.0);
@@ -58,15 +57,15 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
groundTruthState.velocity * groundTruthState.curvature});
if (lastVisionUpdateTime + kVisionUpdateRate < t) {
if (lastVisionPose != frc::Pose2d()) {
if (lastVisionPose != frc::Pose2d{}) {
estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
lastVisionPose =
groundTruthState.pose +
frc::Transform2d(
frc::Translation2d(distribution(generator) * 0.1 * 1_m,
distribution(generator) * 0.1 * 1_m),
frc::Rotation2d(distribution(generator) * 0.01 * 1_rad));
frc::Transform2d{
frc::Translation2d{distribution(generator) * 0.1 * 1_m,
distribution(generator) * 0.1 * 1_m},
frc::Rotation2d{distribution(generator) * 0.01 * 1_rad}};
lastVisionUpdateTime = t;
}
@@ -77,7 +76,7 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)),
frc::Rotation2d{units::radian_t{distribution(generator) * 0.001}},
input, leftDistance, rightDistance);
double error = groundTruthState.pose.Translation()
@@ -94,5 +93,5 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
0.05);
EXPECT_NEAR(0.0, maxError, 0.1);
EXPECT_NEAR(0.0, maxError, 0.125);
}

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

@@ -18,17 +18,16 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::MecanumDrivePoseEstimator estimator{
frc::Rotation2d(), frc::Pose2d(), kinematics,
frc::Rotation2d{}, frc::Pose2d{}, kinematics,
{0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()};
frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
@@ -50,15 +49,15 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
if (lastVisionUpdateTime + kVisionUpdateRate < t) {
if (lastVisionPose != frc::Pose2d()) {
if (lastVisionPose != frc::Pose2d{}) {
estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
lastVisionPose =
groundTruthState.pose +
frc::Transform2d(
frc::Translation2d(distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m),
frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
frc::Transform2d{
frc::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.1 * 1_rad}};
visionPoses.push_back(lastVisionPose);
lastVisionUpdateTime = t;
}
@@ -70,7 +69,7 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d(distribution(generator) * 0.05_rad),
frc::Rotation2d{distribution(generator) * 0.05_rad},
wheelSpeeds);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
@@ -85,5 +84,5 @@ TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
EXPECT_LT(maxError, 0.1);
EXPECT_LT(maxError, 0.125);
}

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

@@ -18,17 +18,16 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d(), frc::Pose2d(), kinematics,
frc::Rotation2d{}, frc::Pose2d{}, kinematics,
{0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()};
frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d{}};
frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 135_deg},
frc::Pose2d{-3_m, 0_m, -90_deg},
frc::Pose2d{0_m, 0_m, 45_deg}},
frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
std::default_random_engine generator;
@@ -50,15 +49,15 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
frc::Trajectory::State groundTruthState = trajectory.Sample(t);
if (lastVisionUpdateTime + kVisionUpdateRate < t) {
if (lastVisionPose != frc::Pose2d()) {
if (lastVisionPose != frc::Pose2d{}) {
estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
lastVisionPose =
groundTruthState.pose +
frc::Transform2d(
frc::Translation2d(distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m),
frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
frc::Transform2d{
frc::Translation2d{distribution(generator) * 0.1_m,
distribution(generator) * 0.1_m},
frc::Rotation2d{distribution(generator) * 0.1 * 1_rad}};
visionPoses.push_back(lastVisionPose);
lastVisionUpdateTime = t;
}
@@ -70,7 +69,7 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
auto xhat = estimator.UpdateWithTime(
t,
groundTruthState.pose.Rotation() +
frc::Rotation2d(distribution(generator) * 0.05_rad),
frc::Rotation2d{distribution(generator) * 0.05_rad},
moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
@@ -85,5 +84,5 @@ TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
}
EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05);
EXPECT_LT(maxError, 0.1);
EXPECT_LT(maxError, 0.125);
}

20
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -10,8 +10,8 @@
using namespace frc;
TEST(Pose2dTest, TransformBy) {
const Pose2d initial{1_m, 2_m, Rotation2d{45_deg}};
const Transform2d transform{Translation2d{5_m, 0_m}, Rotation2d{5_deg}};
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};
const auto transformed = initial + transform;
@@ -21,8 +21,8 @@ TEST(Pose2dTest, TransformBy) {
}
TEST(Pose2dTest, RelativeTo) {
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
const Pose2d final{5_m, 5_m, Rotation2d{45.0_deg}};
const Pose2d initial{0_m, 0_m, 45_deg};
const Pose2d final{5_m, 5_m, 45.0_deg};
const auto finalRelativeToInitial = final.RelativeTo(initial);
@@ -32,20 +32,20 @@ TEST(Pose2dTest, RelativeTo) {
}
TEST(Pose2dTest, Equality) {
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
const Pose2d b{0_m, 5_m, Rotation2d{43_deg}};
const Pose2d a{0_m, 5_m, 43_deg};
const Pose2d b{0_m, 5_m, 43_deg};
EXPECT_TRUE(a == b);
}
TEST(Pose2dTest, Inequality) {
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
const Pose2d b{0_m, 5_ft, Rotation2d{43_deg}};
const Pose2d a{0_m, 5_m, 43_deg};
const Pose2d b{0_m, 5_ft, 43_deg};
EXPECT_TRUE(a != b);
}
TEST(Pose2dTest, Minus) {
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
const Pose2d final{5_m, 5_m, Rotation2d{45_deg}};
const Pose2d initial{0_m, 0_m, 45_deg};
const Pose2d final{5_m, 5_m, 45_deg};
const auto transform = final - initial;

2
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -29,7 +29,7 @@ TEST(Rotation2dTest, DegreesToRadians) {
TEST(Rotation2dTest, RotateByFromZero) {
const Rotation2d zero;
auto rotated = zero + Rotation2d(90_deg);
auto rotated = zero + Rotation2d{90_deg};
EXPECT_DOUBLE_EQ(wpi::numbers::pi / 2.0, rotated.Radians().value());
EXPECT_DOUBLE_EQ(90.0, rotated.Degrees().value());

2
wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -31,7 +31,7 @@ TEST(Translation2dTest, Difference) {
TEST(Translation2dTest, RotateBy) {
const Translation2d another{3_m, 0_m};
const auto rotated = another.RotateBy(Rotation2d(90_deg));
const auto rotated = another.RotateBy(90_deg);
EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());

4
wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp
View File

@@ -116,12 +116,12 @@ TEST(Translation3dTest, Inequality) {
TEST(Translation3dTest, PolarConstructor) {
Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
Translation3d one{std::sqrt(2) * 1_m, Rotation3d(zAxis, 45_deg)};
Translation3d one{std::sqrt(2) * 1_m, Rotation3d{zAxis, 45_deg}};
EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
EXPECT_NEAR(one.Z().value(), 0.0, kEpsilon);
Translation3d two{2_m, Rotation3d(zAxis, 60_deg)};
Translation3d two{2_m, Rotation3d{zAxis, 60_deg}};
EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
EXPECT_NEAR(two.Z().value(), 0.0, kEpsilon);

4
wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -13,7 +13,7 @@ using namespace frc;
TEST(Twist2dTest, Straight) {
const Twist2d straight{5_m, 0_m, 0_rad};
const auto straightPose = Pose2d().Exp(straight);
const auto straightPose = Pose2d{}.Exp(straight);
EXPECT_DOUBLE_EQ(5.0, straightPose.X().value());
EXPECT_DOUBLE_EQ(0.0, straightPose.Y().value());
@@ -52,7 +52,7 @@ TEST(Twist2dTest, Inequality) {
}
TEST(Twist2dTest, Pose2dLog) {
const Pose2d end{5_m, 5_m, Rotation2d{90_deg}};
const Pose2d end{5_m, 5_m, 90_deg};
const Pose2d start;
const auto twist = start.Log(end);

12
wpimath/src/test/native/cpp/geometry/Twist3dTest.cpp
View File

@@ -13,7 +13,7 @@ using namespace frc;
TEST(Twist3dTest, StraightX) {
const Twist3d straight{5_m, 0_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d().Exp(straight);
const auto straightPose = Pose3d{}.Exp(straight);
Pose3d expected{5_m, 0_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
@@ -21,7 +21,7 @@ TEST(Twist3dTest, StraightX) {
TEST(Twist3dTest, StraightY) {
const Twist3d straight{0_m, 5_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d().Exp(straight);
const auto straightPose = Pose3d{}.Exp(straight);
Pose3d expected{0_m, 5_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
@@ -29,7 +29,7 @@ TEST(Twist3dTest, StraightY) {
TEST(Twist3dTest, StraightZ) {
const Twist3d straight{0_m, 0_m, 5_m, 0_rad, 0_rad, 0_rad};
const auto straightPose = Pose3d().Exp(straight);
const auto straightPose = Pose3d{}.Exp(straight);
Pose3d expected{0_m, 0_m, 5_m, Rotation3d{}};
EXPECT_EQ(expected, straightPose);
@@ -40,8 +40,8 @@ TEST(Twist3dTest, QuarterCircle) {
const Twist3d quarterCircle{
5_m / 2.0 * wpi::numbers::pi, 0_m, 0_m, 0_rad, 0_rad,
units::radian_t(wpi::numbers::pi / 2.0)};
const auto quarterCirclePose = Pose3d().Exp(quarterCircle);
units::radian_t{wpi::numbers::pi / 2.0}};
const auto quarterCirclePose = Pose3d{}.Exp(quarterCircle);
Pose3d expected{5_m, 5_m, 0_m, Rotation3d{zAxis, 90_deg}};
EXPECT_EQ(expected, quarterCirclePose);
@@ -49,7 +49,7 @@ TEST(Twist3dTest, QuarterCircle) {
TEST(Twist3dTest, DiagonalNoDtheta) {
const Twist3d diagonal{2_m, 2_m, 0_m, 0_rad, 0_rad, 0_rad};
const auto diagonalPose = Pose3d().Exp(diagonal);
const auto diagonalPose = Pose3d{}.Exp(diagonal);
Pose3d expected{2_m, 2_m, 0_m, Rotation3d{}};
EXPECT_EQ(expected, diagonalPose);

18
wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
View File

@@ -13,16 +13,16 @@
TEST(TimeInterpolatableBufferTest, Interpolation) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
buffer.AddSample(0_s, frc::Rotation2d(0_rad));
EXPECT_TRUE(buffer.Sample(0_s).value() == frc::Rotation2d(0_rad));
buffer.AddSample(1_s, frc::Rotation2d(1_rad));
EXPECT_TRUE(buffer.Sample(0.5_s).value() == frc::Rotation2d(0.5_rad));
EXPECT_TRUE(buffer.Sample(1_s).value() == frc::Rotation2d(1_rad));
buffer.AddSample(3_s, frc::Rotation2d(2_rad));
EXPECT_TRUE(buffer.Sample(2_s).value() == frc::Rotation2d(1.5_rad));
buffer.AddSample(0_s, 0_rad);
EXPECT_TRUE(buffer.Sample(0_s).value() == 0_rad);
buffer.AddSample(1_s, 1_rad);
EXPECT_TRUE(buffer.Sample(0.5_s).value() == 0.5_rad);
EXPECT_TRUE(buffer.Sample(1_s).value() == 1_rad);
buffer.AddSample(3_s, 2_rad);
EXPECT_TRUE(buffer.Sample(2_s).value() == 1.5_rad);
buffer.AddSample(10.5_s, frc::Rotation2d(2_rad));
EXPECT_TRUE(buffer.Sample(0_s) == frc::Rotation2d(1_rad));
buffer.AddSample(10.5_s, 2_rad);
EXPECT_TRUE(buffer.Sample(0_s) == 1_rad);
}
TEST(TimeInterpolatableBufferTest, Pose2d) {

2
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -9,7 +9,7 @@ static constexpr double kEpsilon = 1E-9;
TEST(ChassisSpeedsTest, FieldRelativeConstruction) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
1.0_mps, 0.0_mps, 0.5_rad_per_s, -90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);

4
wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
View File

@@ -66,8 +66,8 @@ TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
units::meters_per_second_t(+0.381 * wpi::numbers::pi),
units::meters_per_second_t(-0.381 * wpi::numbers::pi)};
units::meters_per_second_t{+0.381 * wpi::numbers::pi},
units::meters_per_second_t{-0.381 * wpi::numbers::pi}};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);

6
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View File

@@ -13,10 +13,10 @@ static constexpr double kEpsilon = 1E-9;
using namespace frc;
TEST(DifferentialDriveOdometryTest, EncoderDistances) {
DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
DifferentialDriveOdometry odometry{45_deg};
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
units::meter_t(5 * wpi::numbers::pi));
const auto& pose =
odometry.Update(135_deg, 0_m, units::meter_t{5 * wpi::numbers::pi});
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);

2
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -61,7 +61,7 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::numbers::pi)};
units::radians_per_second_t{2 * wpi::numbers::pi}};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);

24
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -19,12 +19,12 @@ class MecanumDriveOdometryTest : public ::testing::Test {
};
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
odometry.ResetPosition(Pose2d(), 0_rad);
odometry.ResetPosition(Pose2d{}, 0_rad);
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
3.536_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
odometry.UpdateWithTime(0_s, 0_deg, wheelSpeeds);
auto secondPose = odometry.UpdateWithTime(0.0_s, 0_deg, wheelSpeeds);
EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
@@ -32,11 +32,11 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.ResetPosition(Pose2d(), 0_rad);
odometry.ResetPosition(Pose2d{}, 0_rad);
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, 0_deg, speeds);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
@@ -44,11 +44,11 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
odometry.ResetPosition(Pose2d(), 0_rad);
odometry.ResetPosition(Pose2d{}, 0_rad);
MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
39.986_mps};
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
odometry.UpdateWithTime(0_s, 0_deg, MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(1_s, 90_deg, speeds);
EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
@@ -56,12 +56,12 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
odometry.ResetPosition(Pose2d{}, 90_deg);
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
odometry.UpdateWithTime(0_s, 90_deg, MecanumDriveWheelSpeeds{});
auto pose = odometry.UpdateWithTime(0.10_s, 90_deg, speeds);
EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);

44
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -40,7 +40,7 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
SwerveModuleState state{5.0_mps, Rotation2d()};
SwerveModuleState state{5.0_mps, 0_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
@@ -65,7 +65,7 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
SwerveModuleState state{5_mps, Rotation2d(90_deg)};
SwerveModuleState state{5_mps, 90_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
@@ -75,7 +75,7 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::numbers::pi)};
units::radians_per_second_t{2 * wpi::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
@@ -91,7 +91,7 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::numbers::pi)};
units::radians_per_second_t{2 * wpi::numbers::pi}};
m_kinematics.ToSwerveModuleStates(speeds);
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
@@ -107,10 +107,10 @@ TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
SwerveModuleState fl{106.629_mps, 135_deg};
SwerveModuleState fr{106.629_mps, 45_deg};
SwerveModuleState bl{106.629_mps, -135_deg};
SwerveModuleState br{106.629_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
@@ -121,7 +121,7 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
units::radians_per_second_t(2 * wpi::numbers::pi)};
units::radians_per_second_t{2 * wpi::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
@@ -136,10 +136,10 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
SwerveModuleState fl{0.0_mps, 0_deg};
SwerveModuleState fr{150.796_mps, 0_deg};
SwerveModuleState bl{150.796_mps, -90_deg};
SwerveModuleState br{213.258_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
@@ -152,7 +152,7 @@ TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
m_kinematics.ToSwerveModuleStates(speeds, Translation2d{24_m, 0_m});
EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
@@ -167,10 +167,10 @@ TEST_F(SwerveDriveKinematicsTest,
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationForwardKinematics) {
SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
SwerveModuleState fl{23.43_mps, -140.19_deg};
SwerveModuleState fr{23.43_mps, -39.81_deg};
SwerveModuleState bl{54.08_mps, -109.44_deg};
SwerveModuleState br{54.08_mps, -70.56_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
@@ -180,10 +180,10 @@ TEST_F(SwerveDriveKinematicsTest,
}
TEST_F(SwerveDriveKinematicsTest, Desaturate) {
SwerveModuleState state1{5.0_mps, Rotation2d()};
SwerveModuleState state2{6.0_mps, Rotation2d()};
SwerveModuleState state3{4.0_mps, Rotation2d()};
SwerveModuleState state4{7.0_mps, Rotation2d()};
SwerveModuleState state1{5.0_mps, 0_deg};
SwerveModuleState state2{6.0_mps, 0_deg};
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);

45
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -22,14 +22,14 @@ class SwerveDriveOdometryTest : public ::testing::Test {
};
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
SwerveModuleState state{5_mps, Rotation2d()};
SwerveModuleState state{5_mps, 0_deg};
m_odometry.ResetPosition(Pose2d(), 0_rad);
m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
SwerveModuleState(), SwerveModuleState(),
SwerveModuleState());
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
state, state);
m_odometry.ResetPosition(Pose2d{}, 0_rad);
m_odometry.UpdateWithTime(0_s, 0_deg, SwerveModuleState{},
SwerveModuleState{}, SwerveModuleState{},
SwerveModuleState{});
auto pose =
m_odometry.UpdateWithTime(0.1_s, 0_deg, state, state, state, state);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -37,17 +37,16 @@ TEST_F(SwerveDriveOdometryTest, TwoIterations) {
}
TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
SwerveModuleState fl{18.85_mps, 90_deg};
SwerveModuleState fr{42.15_mps, 26.565_deg};
SwerveModuleState bl{18.85_mps, -90_deg};
SwerveModuleState br{42.15_mps, -26.565_deg};
SwerveModuleState zero{0_mps, Rotation2d()};
SwerveModuleState zero{0_mps, 0_deg};
m_odometry.ResetPosition(Pose2d(), 0_rad);
m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
auto pose =
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
m_odometry.ResetPosition(Pose2d{}, 0_rad);
m_odometry.UpdateWithTime(0_s, 0_deg, zero, zero, zero, zero);
auto pose = m_odometry.UpdateWithTime(1_s, 90_deg, fl, fr, bl, br);
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
@@ -55,15 +54,15 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
m_odometry.ResetPosition(Pose2d{}, 90_deg);
SwerveModuleState state{5_mps, Rotation2d()};
SwerveModuleState state{5_mps, 0_deg};
m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
SwerveModuleState(), SwerveModuleState(),
SwerveModuleState());
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
state, state);
m_odometry.UpdateWithTime(0_s, 90_deg, SwerveModuleState{},
SwerveModuleState{}, SwerveModuleState{},
SwerveModuleState{});
auto pose =
m_odometry.UpdateWithTime(0.1_s, 90_deg, state, state, state, state);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);

26
wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
View File

@@ -93,31 +93,29 @@ class CubicHermiteSplineTest : public ::testing::Test {
} // namespace frc
TEST_F(CubicHermiteSplineTest, StraightLine) {
Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
Run(Pose2d{}, std::vector<Translation2d>(), Pose2d{3_m, 0_m, 0_deg});
}
TEST_F(CubicHermiteSplineTest, SCurve) {
Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
Translation2d(2_m, -1_m)};
Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
Pose2d start{0_m, 0_m, 90_deg};
std::vector<Translation2d> waypoints{Translation2d{1_m, 1_m},
Translation2d{2_m, -1_m}};
Pose2d end{3_m, 0_m, 90_deg};
Run(start, waypoints, end);
}
TEST_F(CubicHermiteSplineTest, OneInterior) {
Pose2d start{0_m, 0_m, 0_rad};
std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
std::vector<Translation2d> waypoints{Translation2d{2_m, 0_m}};
Pose2d end{4_m, 0_m, 0_rad};
Run(start, waypoints, end);
}
TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
EXPECT_THROW(
Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
SplineParameterizer::MalformedSplineException);
EXPECT_THROW(
Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
SplineParameterizer::MalformedSplineException);
EXPECT_THROW(Run(Pose2d{0_m, 0_m, 0_deg}, std::vector<Translation2d>{},
Pose2d{1_m, 0_m, 180_deg}),
SplineParameterizer::MalformedSplineException);
EXPECT_THROW(Run(Pose2d{10_m, 10_m, 90_deg}, std::vector<Translation2d>{},
Pose2d{10_m, 11_m, -90_deg}),
SplineParameterizer::MalformedSplineException);
}

13
wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
View File

@@ -65,23 +65,20 @@ class QuinticHermiteSplineTest : public ::testing::Test {
} // namespace frc
TEST_F(QuinticHermiteSplineTest, StraightLine) {
Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
Run(Pose2d{}, Pose2d{3_m, 0_m, 0_deg});
}
TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
Run(Pose2d{}, Pose2d{1_m, 1_m, 0_deg});
}
TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
Run(Pose2d{0_m, 0_m, 90_deg}, Pose2d{-1_m, 0_m, 90_deg});
}
TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
Pose2d(1_m, 0_m, Rotation2d(180_deg))),
EXPECT_THROW(Run(Pose2d{0_m, 0_m, 0_deg}, Pose2d{1_m, 0_m, 180_deg}),
SplineParameterizer::MalformedSplineException);
EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
EXPECT_THROW(Run(Pose2d{10_m, 10_m, 90_deg}, Pose2d{10_m, 11_m, -90_deg}),
SplineParameterizer::MalformedSplineException);
}

8
wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
View File

@@ -72,12 +72,12 @@ TEST(DifferentialDriveVoltageConstraintTest, HighCurvature) {
DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
Pose2d{1_m, 0_m, Rotation2d{90_deg}}, std::vector<Translation2d>{},
Pose2d{0_m, 1_m, Rotation2d{180_deg}}, config));
Pose2d{1_m, 0_m, 90_deg}, std::vector<Translation2d>{},
Pose2d{0_m, 1_m, 180_deg}, config));
config.SetReversed(true);
EXPECT_NO_FATAL_FAILURE(TrajectoryGenerator::GenerateTrajectory(
Pose2d{0_m, 1_m, Rotation2d{180_deg}}, std::vector<Translation2d>{},
Pose2d{1_m, 0_m, Rotation2d{90_deg}}, config));
Pose2d{0_m, 1_m, 180_deg}, std::vector<Translation2d>{},
Pose2d{1_m, 0_m, 90_deg}, config));
}

15
wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
View File

@@ -21,9 +21,8 @@ TEST(EllipticalRegionConstraintTest, Constraint) {
auto config = TrajectoryConfig(13_fps, 13_fps_sq);
MaxVelocityConstraint maxVelConstraint(maxVelocity);
EllipticalRegionConstraint regionConstraint(frc::Translation2d{5_ft, 2.5_ft},
10_ft, 5_ft, Rotation2d(180_deg),
maxVelConstraint);
EllipticalRegionConstraint regionConstraint(
frc::Translation2d{5_ft, 2.5_ft}, 10_ft, 5_ft, 180_deg, maxVelConstraint);
config.AddConstraint(regionConstraint);
auto trajectory = TestTrajectory::GetTrajectory(config);
@@ -45,14 +44,12 @@ TEST(EllipticalRegionConstraintTest, IsPoseInRegion) {
constexpr auto maxVelocity = 2_fps;
MaxVelocityConstraint maxVelConstraint(maxVelocity);
EllipticalRegionConstraint regionConstraintNoRotation(
frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(0_deg),
maxVelConstraint);
frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 0_deg, maxVelConstraint);
EXPECT_FALSE(regionConstraintNoRotation.IsPoseInRegion(
frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
frc::Pose2d{2.1_ft, 1_ft, 0_rad}));
EllipticalRegionConstraint regionConstraintWithRotation(
frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(90_deg),
maxVelConstraint);
frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, 90_deg, maxVelConstraint);
EXPECT_TRUE(regionConstraintWithRotation.IsPoseInRegion(
frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
frc::Pose2d{2.1_ft, 1_ft, 0_rad}));
}

5
wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
View File

@@ -47,7 +47,6 @@ TEST(RectangularRegionConstraintTest, IsPoseInRegion) {
frc::Translation2d{5_ft, 27_ft},
maxVelConstraint);
EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d()));
EXPECT_TRUE(
regionConstraint.IsPoseInRegion(Pose2d(3_ft, 14.5_ft, Rotation2d())));
EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d{}));
EXPECT_TRUE(regionConstraint.IsPoseInRegion(Pose2d{3_ft, 14.5_ft, 0_deg}));
}

3
wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
View File

@@ -34,8 +34,7 @@ TEST(TrajectoryGenerationTest, ObeysConstraints) {
TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
const auto t = TrajectoryGenerator::GenerateTrajectory(
std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
Pose2d(1_m, 0_m, Rotation2d(180_deg))},
std::vector<Pose2d>{Pose2d{0_m, 0_m, 0_deg}, Pose2d{1_m, 0_m, 180_deg}},
TrajectoryConfig(12_fps, 12_fps_sq));
ASSERT_EQ(t.States().size(), 1u);

12
wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
View File

@@ -31,11 +31,9 @@ void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
TEST(TrajectoryTransformsTest, TransformBy) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
config);
frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, 90_deg}, config);
auto transformedTrajectory =
trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
auto transformedTrajectory = trajectory.TransformBy({{1_m, 2_m}, 30_deg});
auto firstPose = transformedTrajectory.Sample(0_s).pose;
@@ -49,11 +47,9 @@ TEST(TrajectoryTransformsTest, TransformBy) {
TEST(TrajectoryTransformsTest, RelativeTo) {
frc::TrajectoryConfig config{3_mps, 3_mps_sq};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
frc::Pose2d{1_m, 2_m, 30_deg}, {}, frc::Pose2d{5_m, 7_m, 90_deg}, config);
auto transformedTrajectory =
trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
auto transformedTrajectory = trajectory.RelativeTo({1_m, 2_m, 30_deg});
auto firstPose = transformedTrajectory.Sample(0_s).pose;