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

Fix SplineHelper bug (#2018)

Browse Source 
Add unit test to check interior waypoints
This commit is contained in:
Prateek Machiraju
2019-11-01 12:28:11 -04:00
committed by Peter Johnson
parent f33bd9f050
commit f6e311ef86
3 changed files with 49 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
View File

@@ -73,6 +73,22 @@ class CubicHermiteSplineTest : public ::testing::Test {
EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
a.Rotation().Radians().to<double>(), 1E-9);
// Check interior waypoints
bool interiorsGood = true;
for (auto& waypoint : waypoints) {
bool found = false;
for (auto& state : poses) {
if (std::abs(
waypoint.Distance(state.first.Translation()).to<double>()) <
1E-9) {
found = true;
}
}
interiorsGood &= found;
}
EXPECT_TRUE(interiorsGood);
// Check last point.
EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
b.Translation().X().to<double>(), 1E-9);
@@ -97,3 +113,10 @@ TEST_F(CubicHermiteSplineTest, SCurve) {
Pose2d end{3_m, 0_m, Rotation2d{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)};
Pose2d end{4_m, 0_m, 0_rad};
Run(start, waypoints, end);
}