10 #define GRID 1 // in meters
11 #define GRID_WIDTH 40 // in meters
12 #define GRID_HEIGHT 40 // in meters
13 #define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
14 #define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
17 #define GRID_MAX_HI 60
21 class RRTExt18 : public virtual RRTS {
23 bool should_finish() const;
26 class RRTExt17 : public virtual RRTS {
28 bool should_finish() const;
31 class RRTExt16 : public virtual RRTS {
33 void steer(RRTNode const& f, RRTNode const& t);
36 class RRTExt15 : public virtual RRTS {
38 std::vector<double> log_path_cost_;
40 Json::Value json() const;
41 void json(Json::Value jvi);
45 /*! \brief Random sampling in the circuit between root and goal.
47 * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
49 class RRTExt14 : public virtual RRTS {
52 double circle_r_ = 0.0;
53 std::uniform_real_distribution<double> udr_;
54 std::uniform_real_distribution<double> udt_;
55 std::uniform_real_distribution<double> udh_;
62 /*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
63 class RRTExt13 : public virtual RRTS {
67 RRTNode* node = nullptr;
71 DijkstraNode(RRTNode* n);
73 class DijkstraNodeComparator {
75 int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
77 std::vector<RRTNode*> opath_;
78 double ogoal_cc_ = 0.0;
80 std::vector<DijkstraNode> dn_;
81 void pick_interesting();
82 void dijkstra_forward();
83 void dijkstra_backward();
87 Json::Value json() const;
88 void json(Json::Value jvi);
92 /*! \brief Different `steer` procedures.
94 Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
96 class RRTExt12 : public virtual RRTS {
98 void steer1(RRTNode &f, RRTNode &t);
104 /*! \brief Goal Zone.
107 class RRTExt11 : public virtual RRTS {
109 bool goal_found(RRTNode &f);
112 /*! \brief Different costs extension.
114 * Use different cost for bulding tree data structure and searching in the
115 * structure. The cost function is from Elbanhawi, Mohamed, Milan Simic, and
116 * Reza Jazar. “Randomized Bidirectional B-Spline Parameterization Motion
117 * Planning.” IEEE Transactions on Intelligent Transportation Systems 17, no. 2
118 * (February 2016): 406–19. https://doi.org/10.1109/TITS.2015.2477355.
120 class RRTExt10 : public virtual RRTS {
122 double cost_build(RRTNode const& f, RRTNode const& t) const;
123 double cost_search(RRTNode const& f, RRTNode const& t) const;
126 /*! \brief Use grid data structure to store RRT nodes.
128 This approach speeds up the search process for the nearest neighbor and
129 the near vertices procedures.
131 class RRTExt9 : public virtual RRTS {
135 bool changed_ = false;
136 std::vector<RRTNode *> nodes_;
138 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
139 void store_node(RRTNode *n);
144 return this->changed_;
146 std::vector<RRTNode *> &nodes()
153 Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
159 double h_max_ = 2 * M_PI;
161 unsigned int xi(RRTNode n);
162 unsigned int yi(RRTNode n);
163 unsigned int hi(RRTNode n);
167 void store_node(RRTNode n);
168 RRTNode *nn(RRTNode &t);
169 std::vector<RRTNode *> nv(RRTNode &t);
172 /*! \brief Use k-d tree data structure to store RRT nodes.
174 * This approach speeds up the search process for the nearest neighbor and the
175 * near vertices procedures. This extension implements 3D K-d tree.
177 * \see https://en.wikipedia.org/wiki/K-d_tree
179 class RRTExt8 : public virtual RRTS {
183 RRTNode* node = nullptr;
184 KdNode* left = nullptr;
185 KdNode* right = nullptr;
188 KdNode* kdroot_ = nullptr;
189 std::vector<KdNode> kdnodes_;
190 void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
191 void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
192 void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
196 void store(RRTNode n);
197 void find_nn(RRTNode const& t);
198 void find_nv(RRTNode const& t);
201 /*! \brief Use k-d tree data structure to store RRT nodes.
203 This approach speeds up the search process for the nearest neighbor and
204 the near vertices procedures. This extension implements 2D K-d tree.
206 \see https://en.wikipedia.org/wiki/K-d_tree
208 class RRTExt7 : public virtual RRTS {
212 RRTNode *node_ = nullptr;
213 KdNode *left_ = nullptr;
214 KdNode *right_ = nullptr;
217 RRTNode *node() const { return this->node_; }
218 KdNode *&left() { return this->left_; }
219 KdNode *&right() { return this->right_; }
222 KdNode *kdroot_ = nullptr;
223 void delete_kd_nodes(KdNode *n);
224 void store_node(RRTNode *n, KdNode *&r, int l);
225 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
229 void store_node(RRTNode n);
230 RRTNode *nn(RRTNode &t);
231 std::vector<RRTNode *> nv(RRTNode &t);
234 /*! \brief Reeds and Shepp cost for building and search.
236 class RRTExt6 : public virtual RRTS {
238 /*! \brief Reeds and Shepp path length.
240 double cost_build(RRTNode &f, RRTNode &t);
241 /*! \brief Reeds and Shepp path length.
243 double cost_search(RRTNode &f, RRTNode &t);
246 /*! \brief Different costs extension.
248 Use different cost for bulding tree data structure and searching in the
249 structure. This one is complementary to `rrtext1.cc`.
251 class RRTExt5 : public virtual RRTS {
253 /*! \brief Reeds and Shepp path length.
255 double cost_build(RRTNode &f, RRTNode &t);
256 /*! \brief Euclidean distance.
258 double cost_search(RRTNode &f, RRTNode &t);
261 /*! \brief Use grid data structure to store RRT nodes.
263 This approach speeds up the search process for the nearest neighbor and
264 the near vertices procedures.
266 class RRTExt4 : public virtual RRTS {
270 bool changed_ = false;
271 std::vector<RRTNode *> nodes_;
273 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
274 void store_node(RRTNode *n);
279 return this->changed_;
281 std::vector<RRTNode *> &nodes()
288 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
294 unsigned int xi(RRTNode n);
295 unsigned int yi(RRTNode n);
299 void store_node(RRTNode n);
300 RRTNode *nn(RRTNode &t);
301 std::vector<RRTNode *> nv(RRTNode &t);
304 /*! \brief Use Dijkstra algorithm to find the shorter path.
306 class RRTExt3 : public virtual RRTS {
310 std::vector<RRTNode *> orig_path_;
311 double orig_path_cost_ = 9999;
312 std::vector<RRTNode *> first_optimized_path_;
313 double first_optimized_path_cost_ = 9999;
314 void first_path_optimization();
315 void second_path_optimization();
318 void json(Json::Value jvi);
321 std::vector<RRTNode *> &orig_path()
323 return this->orig_path_;
325 double &orig_path_cost() { return this->orig_path_cost_; }
326 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
327 std::vector<RRTNode *> &first_optimized_path()
329 return this->first_optimized_path_;
331 double &first_optimized_path_cost() {
332 return this->first_optimized_path_cost_;
334 void first_optimized_path_cost(double c) {
335 this->first_optimized_path_cost_ = c;
339 /*! \brief Use cute_c2 for collision detection.
341 \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
343 class RRTExt2 : public virtual RRTS {
347 std::vector<c2Poly> c2_obstacles_;
348 bool collide(RRTNode const& n);
349 bool collide_steered();
352 Json::Value json() const;
353 void json(Json::Value jvi);
356 /*! \brief Different costs extension.
358 Use different cost for bulding tree data structure and searching in the
361 class RRTExt1 : public virtual RRTS {
363 /*! \brief Reeds and Shepp path length.
365 double cost_build(RRTNode &f, RRTNode &t);
366 /*! \brief Matej's heuristics.
368 double cost_search(RRTNode &f, RRTNode &t);
372 #endif /* RRTS_RRTEXT_H */