Program Listing for File spline.hpp¶
↰ Return to documentation for file (include/spline.hpp
)
#ifndef _SQUIGGLES_SPLINE_HPP_
#define _SQUIGGLES_SPLINE_HPP_
#include <initializer_list>
#include <memory>
#include <vector>
#include "constraints.hpp"
#include "geometry/controlvector.hpp"
#include "geometry/profilepoint.hpp"
#include "math/quinticpolynomial.hpp"
#include "physicalmodel/passthroughmodel.hpp"
#include "physicalmodel/physicalmodel.hpp"
namespace squiggles {
class SplineGenerator {
public:
SplineGenerator(Constraints iconstraints,
std::shared_ptr<PhysicalModel> imodel =
std::make_shared<PassthroughModel>(),
double idt = 0.1);
std::vector<ProfilePoint> generate(std::vector<Pose> iwaypoints);
std::vector<ProfilePoint> generate(std::initializer_list<Pose> iwaypoints);
std::vector<ProfilePoint> generate(std::vector<ControlVector> iwaypoints);
std::vector<ProfilePoint>
generate(std::initializer_list<ControlVector> iwaypoints);
protected:
Constraints constraints;
std::shared_ptr<PhysicalModel> model;
double dt;
const int T_MIN = 2;
const int T_MAX = 15;
const double K_DEFAULT_VEL = 0.12;
struct GeneratedPoint {
GeneratedPoint(Pose ipose, double icurvature = 0.0)
: pose(ipose), curvature(icurvature) {}
std::string to_string() const {
return "GeneratedPoint: {" + pose.to_string() +
", curvature: " + std::to_string(curvature) + "}";
}
Pose pose;
double curvature;
};
struct GeneratedVector {
GeneratedVector(GeneratedPoint ipoint,
double ivel,
double iaccel,
double ijerk)
: point(ipoint), vel(ivel), accel(iaccel), jerk(ijerk) {}
GeneratedPoint point;
double vel;
double accel;
double jerk;
std::string to_string() const {
return "GeneratedVector: {" + point.to_string() +
", vel: " + std::to_string(vel) +
", accel: " + std::to_string(accel) +
", jerk: " + std::to_string(jerk) + "}";
}
};
struct ConstrainedState {
ConstrainedState(Pose ipose,
double icurvature,
double idistance,
double imax_vel,
double imin_accel,
double imax_accel)
: pose(ipose),
curvature(icurvature),
distance(idistance),
max_vel(imax_vel),
min_accel(imin_accel),
max_accel(imax_accel) {}
ConstrainedState() = default;
Pose pose = Pose();
double curvature = 0;
double distance = 0;
double max_vel = 0;
double min_accel = 0;
double max_accel = 0;
std::string to_string() const {
return "ConstrainedState: {x: " + std::to_string(pose.x) +
", y: " + std::to_string(pose.y) +
", yaw: " + std::to_string(pose.yaw) +
", k: " + std::to_string(curvature) +
", dist: " + std::to_string(distance) +
", v: " + std::to_string(max_vel) +
", min_a: " + std::to_string(min_accel) +
", max_a: " + std::to_string(max_accel) + "}";
}
};
template <class Iter>
std::vector<ProfilePoint> _generate(Iter start, Iter end);
std::vector<GeneratedPoint> gen_raw_path(ControlVector start,
ControlVector end);
std::vector<ProfilePoint>
parameterize(const ControlVector start,
const ControlVector end,
const std::vector<GeneratedPoint>& raw_path,
const double preferred_start_vel,
const double preferred_end_vel);
std::vector<ProfilePoint>
integrate_constrained_states(std::vector<ConstrainedState> constrainedStates);
ProfilePoint get_point_at_time(const ControlVector start,
const ControlVector end,
std::vector<ProfilePoint> points,
double t);
ProfilePoint lerp_point(QuinticPolynomial x_qp,
QuinticPolynomial y_qp,
ProfilePoint start,
ProfilePoint end,
double i);
QuinticPolynomial get_x_spline(const ControlVector start,
const ControlVector end,
const double duration);
QuinticPolynomial get_y_spline(const ControlVector start,
const ControlVector end,
const double duration);
void enforce_accel_lims(ConstrainedState* state);
void forward_pass(ConstrainedState* predecessor, ConstrainedState* successor);
void backward_pass(ConstrainedState* predecessor,
ConstrainedState* successor);
double vf(double vi, double a, double ds);
double ai(double vf, double vi, double s);
static constexpr double K_EPSILON = 1e-5;
};
} // namespace squiggles
#endif