34 #include <frc/spline/Spline.h>
41 #include <units/units.h>
42 #include <wpi/Twine.h>
51 using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
55 : runtime_error(what_arg) {}
75 std::vector<PoseWithCurvature> splinePoints;
78 splinePoints.push_back(spline.
GetPoint(t0));
82 std::stack<StackContents> stack;
83 stack.emplace(StackContents{t0, t1});
85 StackContents current;
86 PoseWithCurvature start;
87 PoseWithCurvature end;
90 while (!stack.empty()) {
91 current = stack.top();
96 const auto twist = start.first.Log(end.first);
98 if (units::math::abs(twist.dy) > kMaxDy ||
99 units::math::abs(twist.dx) > kMaxDx ||
100 units::math::abs(twist.dtheta) > kMaxDtheta) {
101 stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
102 stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
104 splinePoints.push_back(spline.
GetPoint(current.t1));
107 if (iterations++ >= kMaxIterations) {
109 "Could not parameterize a malformed spline. "
110 "This means that you probably had two or more adjacent "
111 "waypoints that were very close together with headings "
112 "in opposing directions.");
121 static constexpr units::meter_t kMaxDx = 5_in;
122 static constexpr units::meter_t kMaxDy = 0.05_in;
123 static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
125 struct StackContents {
137 static constexpr
int kMaxIterations = 5000;
139 friend class CubicHermiteSplineTest;
140 friend class QuinticHermiteSplineTest;