1+ #ifndef _BSPLINE_OPTIMIZER_H_
2+ #define _BSPLINE_OPTIMIZER_H_
3+
4+ #include < Eigen/Eigen>
5+ #include < path_searching/dyn_a_star.h>
6+ #include < bspline_opt/uniform_bspline.h>
7+ #include < plan_env/grid_map.h>
8+ #include < ros/ros.h>
9+ #include " bspline_opt/lbfgs.hpp"
10+
11+ // Gradient and elasitc band optimization
12+
13+ // Input: a signed distance field and a sequence of points
14+ // Output: the optimized sequence of points
15+ // The format of points: N x 3 matrix, each row is a point
16+ namespace ego_planner
17+ {
18+
19+ class ControlPoints
20+ {
21+ public:
22+ double clearance;
23+ int size;
24+ Eigen::MatrixXd points;
25+ std::vector<std::vector<Eigen::Vector3d>> base_point; // The point at the statrt of the direction vector (collision point)
26+ std::vector<std::vector<Eigen::Vector3d>> direction; // Direction vector, must be normalized.
27+ std::vector<bool > flag_temp; // A flag that used in many places. Initialize it everytime before using it.
28+ // std::vector<bool> occupancy;
29+
30+ void resize (const int size_set)
31+ {
32+ size = size_set;
33+
34+ base_point.clear ();
35+ direction.clear ();
36+ flag_temp.clear ();
37+ // occupancy.clear();
38+
39+ points.resize (3 , size_set);
40+ base_point.resize (size);
41+ direction.resize (size);
42+ flag_temp.resize (size);
43+ // occupancy.resize(size);
44+ }
45+ };
46+
47+ class BsplineOptimizer
48+ {
49+
50+ public:
51+ BsplineOptimizer () {}
52+ ~BsplineOptimizer () {}
53+
54+ /* main API */
55+ void setEnvironment (const GridMap::Ptr &env);
56+ void setParam (ros::NodeHandle &nh);
57+ Eigen::MatrixXd BsplineOptimizeTraj (const Eigen::MatrixXd &points, const double &ts,
58+ const int &cost_function, int max_num_id, int max_time_id);
59+
60+ /* helper function */
61+
62+ // required inputs
63+ void setControlPoints (const Eigen::MatrixXd &points);
64+ void setBsplineInterval (const double &ts);
65+ void setCostFunction (const int &cost_function);
66+ void setTerminateCond (const int &max_num_id, const int &max_time_id);
67+
68+ // optional inputs
69+ void setGuidePath (const vector<Eigen::Vector3d> &guide_pt);
70+ void setWaypoints (const vector<Eigen::Vector3d> &waypts,
71+ const vector<int > &waypt_idx); // N-2 constraints at most
72+
73+ void optimize ();
74+
75+ Eigen::MatrixXd getControlPoints ();
76+
77+ AStar::Ptr a_star_;
78+ std::vector<Eigen::Vector3d> ref_pts_;
79+
80+ std::vector<std::vector<Eigen::Vector3d>> initControlPoints (Eigen::MatrixXd &init_points, bool flag_first_init = true );
81+ bool BsplineOptimizeTrajRebound (Eigen::MatrixXd &optimal_points, double ts); // must be called after initControlPoints()
82+ bool BsplineOptimizeTrajRefine (const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points);
83+
84+ inline int getOrder (void ) { return order_; }
85+
86+ private:
87+ GridMap::Ptr grid_map_;
88+
89+ enum FORCE_STOP_OPTIMIZE_TYPE
90+ {
91+ DONT_STOP,
92+ STOP_FOR_REBOUND,
93+ STOP_FOR_ERROR
94+ } force_stop_type_;
95+
96+ // main input
97+ // Eigen::MatrixXd control_points_; // B-spline control points, N x dim
98+ double bspline_interval_; // B-spline knot span
99+ Eigen::Vector3d end_pt_; // end of the trajectory
100+ // int dim_; // dimension of the B-spline
101+ //
102+ vector<Eigen::Vector3d> guide_pts_; // geometric guiding path points, N-6
103+ vector<Eigen::Vector3d> waypoints_; // waypts constraints
104+ vector<int > waypt_idx_; // waypts constraints index
105+ //
106+ int max_num_id_, max_time_id_; // stopping criteria
107+ int cost_function_; // used to determine objective function
108+ double start_time_; // global time for moving obstacles
109+
110+ /* optimization parameters */
111+ int order_; // bspline degree
112+ double lambda1_; // jerk smoothness weight
113+ double lambda2_, new_lambda2_; // distance weight
114+ double lambda3_; // feasibility weight
115+ double lambda4_; // curve fitting
116+
117+ int a;
118+ //
119+ double dist0_; // safe distance
120+ double max_vel_, max_acc_; // dynamic limits
121+
122+ int variable_num_; // optimization variables
123+ int iter_num_; // iteration of the solver
124+ Eigen::VectorXd best_variable_; //
125+ double min_cost_; //
126+
127+ ControlPoints cps_;
128+
129+ /* cost function */
130+ /* calculate each part of cost function with control points q as input */
131+
132+ static double costFunction (const std::vector<double > &x, std::vector<double > &grad, void *func_data);
133+ void combineCost (const std::vector<double > &x, vector<double > &grad, double &cost);
134+
135+ // q contains all control points
136+ void calcSmoothnessCost (const Eigen::MatrixXd &q, double &cost,
137+ Eigen::MatrixXd &gradient, bool falg_use_jerk = true );
138+ void calcFeasibilityCost (const Eigen::MatrixXd &q, double &cost,
139+ Eigen::MatrixXd &gradient);
140+ void calcDistanceCostRebound (const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost);
141+ void calcFitnessCost (const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient);
142+ bool check_collision_and_rebound (void );
143+
144+ static int earlyExit (void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls);
145+ static double costFunctionRebound (void *func_data, const double *x, double *grad, const int n);
146+ static double costFunctionRefine (void *func_data, const double *x, double *grad, const int n);
147+
148+ bool rebound_optimize ();
149+ bool refine_optimize ();
150+ void combineCostRebound (const double *x, double *grad, double &f_combine, const int n);
151+ void combineCostRefine (const double *x, double *grad, double &f_combine, const int n);
152+
153+ /* for benckmark evaluation only */
154+ public:
155+ typedef unique_ptr<BsplineOptimizer> Ptr ;
156+
157+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
158+ };
159+
160+ } // namespace ego_planner
161+ #endif
0 commit comments