2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
7 /*! \brief RRT* extensions.
9 * The extensions are used to implement or change the default behavior of the
13 * \defgroup ext-col Collision detection extensions
14 * \defgroup ext-store Node storage and searching tree extensions
15 * \defgroup ext-cost Cost extensions
16 * \defgroup ext-opt Path optimization extensions
17 * \defgroup ext-sampl Random sampling extensions
18 * \defgroup ext-steer Steering procedure extensions
19 * \defgroup ext-aux Auxilliary extensions
30 #define GRID 1 // in meters
31 #define GRID_WIDTH 40 // in meters
32 #define GRID_HEIGHT 40 // in meters
33 #define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
34 #define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
37 #define GRID_MAX_HI 60
41 /*! \brief Finish when more than 1000 iterations.
45 class RRTExt18 : public virtual RRTS {
47 bool should_finish() const;
50 /*! \brief Finish when goal found or more than 1000 iterations.
54 class RRTExt17 : public virtual RRTS {
56 bool should_finish() const;
59 /*! \brief Use Reeds & Shepp steering procedure.
63 class RRTExt16 : public virtual RRTS {
65 void steer(RRTNode const& f, RRTNode const& t);
68 /*! \brief Log goal's cumulative cost each iteration.
72 class RRTExt15 : public virtual RRTS {
74 std::vector<double> log_path_cost_;
76 Json::Value json() const;
77 void json(Json::Value jvi);
81 /*! \brief Random sampling in the circuit between root and goal.
84 * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
86 class RRTExt14 : public virtual RRTS {
89 double circle_r_ = 0.0;
90 std::uniform_real_distribution<double> udr_;
91 std::uniform_real_distribution<double> udt_;
92 std::uniform_real_distribution<double> udh_;
99 /*! \brief Use Dijkstra algorithm to find path between interesting nodes.
101 * The search for interesting nodes starts at root, the last drivable nodes is
102 * added to interesting nodes until goal is reached.
106 class RRTExt13 : public virtual RRTS {
110 RRTNode* node = nullptr;
115 DijkstraNode(RRTNode* n);
117 class DijkstraNodeComparator {
119 int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
121 class DijkstraNodeBackwardComparator {
123 int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
125 std::vector<RRTNode*> opath_;
126 double ogoal_cc_ = 0.0;
128 std::vector<DijkstraNode> dn_;
129 void interesting_forward();
130 void interesting_backward();
131 void dijkstra_forward();
132 void dijkstra_backward();
136 Json::Value json() const;
137 void json(Json::Value jvi);
141 /* \brief Different `steer` procedures.
143 Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
145 class RRTExt12 : public virtual RRTS {
147 void steer1(RRTNode &f, RRTNode &t);
156 class RRTExt11 : public virtual RRTS {
158 bool goal_found(RRTNode &f);
161 /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
163 * Use Reeds & Shepp path length for building tree data structure and Euclidean
164 * distance + (abs) heading difference + 0.1 * backward-forward direction
165 * changes for searching it.
168 * \see https://doi.org/10.1109/TITS.2015.2477355
170 class RRTExt10 : public virtual RRTS {
172 double cost_build(RRTNode const& f, RRTNode const& t) const;
173 double cost_search(RRTNode const& f, RRTNode const& t) const;
176 /* \brief Use grid data structure to store RRT nodes.
178 This approach speeds up the search process for the nearest neighbor and
179 the near vertices procedures.
181 class RRTExt9 : public virtual RRTS {
185 bool changed_ = false;
186 std::vector<RRTNode *> nodes_;
188 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
189 void store_node(RRTNode *n);
194 return this->changed_;
196 std::vector<RRTNode *> &nodes()
203 Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
209 double h_max_ = 2 * M_PI;
211 unsigned int xi(RRTNode n);
212 unsigned int yi(RRTNode n);
213 unsigned int hi(RRTNode n);
217 void store_node(RRTNode n);
218 RRTNode *nn(RRTNode &t);
219 std::vector<RRTNode *> nv(RRTNode &t);
222 /*! \brief Use 3D k-d tree data structure to store RRT nodes.
224 * This approach speeds up the search process for the nearest neighbor and the
225 * near vertices procedures. This extension implements 3D K-d tree.
228 * \see https://en.wikipedia.org/wiki/K-d_tree
230 class RRTExt8 : public virtual RRTS {
234 RRTNode* node = nullptr;
235 KdNode* left = nullptr;
236 KdNode* right = nullptr;
239 KdNode* kdroot_ = nullptr;
240 std::vector<KdNode> kdnodes_;
241 void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
242 void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
243 void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
247 void store(RRTNode n);
248 void find_nn(RRTNode const& t);
249 void find_nv(RRTNode const& t);
252 /* \brief Use k-d tree data structure to store RRT nodes.
254 This approach speeds up the search process for the nearest neighbor and
255 the near vertices procedures. This extension implements 2D K-d tree.
257 \see https://en.wikipedia.org/wiki/K-d_tree
259 class RRTExt7 : public virtual RRTS {
263 RRTNode *node_ = nullptr;
264 KdNode *left_ = nullptr;
265 KdNode *right_ = nullptr;
268 RRTNode *node() const { return this->node_; }
269 KdNode *&left() { return this->left_; }
270 KdNode *&right() { return this->right_; }
273 KdNode *kdroot_ = nullptr;
274 void delete_kd_nodes(KdNode *n);
275 void store_node(RRTNode *n, KdNode *&r, int l);
276 void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
280 void store_node(RRTNode n);
281 RRTNode *nn(RRTNode &t);
282 std::vector<RRTNode *> nv(RRTNode &t);
285 /*! \brief Reeds & Shepp (build, search).
287 * Use Reeds & Shepp path length for building tree data structure as well as for
292 class RRTExt6 : public virtual RRTS {
294 double cost_build(RRTNode const& f, RRTNode const& t) const;
295 double cost_search(RRTNode const& f, RRTNode const& t) const;
298 /* \brief Different costs extension.
300 Use different cost for bulding tree data structure and searching in the
301 structure. This one is complementary to `rrtext1.cc`.
303 class RRTExt5 : public virtual RRTS {
305 /* \brief Reeds and Shepp path length.
307 double cost_build(RRTNode &f, RRTNode &t);
308 /* \brief Euclidean distance.
310 double cost_search(RRTNode &f, RRTNode &t);
313 /* \brief Use grid data structure to store RRT nodes.
315 This approach speeds up the search process for the nearest neighbor and
316 the near vertices procedures.
318 class RRTExt4 : public virtual RRTS {
322 bool changed_ = false;
323 std::vector<RRTNode *> nodes_;
325 void nn(RRTNode *t, RRTNode **nn, RRTS *p);
326 void store_node(RRTNode *n);
331 return this->changed_;
333 std::vector<RRTNode *> &nodes()
340 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
346 unsigned int xi(RRTNode n);
347 unsigned int yi(RRTNode n);
351 void store_node(RRTNode n);
352 RRTNode *nn(RRTNode &t);
353 std::vector<RRTNode *> nv(RRTNode &t);
356 /* \brief Use Dijkstra algorithm to find the shorter path.
358 class RRTExt3 : public virtual RRTS {
362 std::vector<RRTNode *> orig_path_;
363 double orig_path_cost_ = 9999;
364 std::vector<RRTNode *> first_optimized_path_;
365 double first_optimized_path_cost_ = 9999;
366 void first_path_optimization();
367 void second_path_optimization();
370 void json(Json::Value jvi);
373 std::vector<RRTNode *> &orig_path()
375 return this->orig_path_;
377 double &orig_path_cost() { return this->orig_path_cost_; }
378 void orig_path_cost(double c) { this->orig_path_cost_ = c; }
379 std::vector<RRTNode *> &first_optimized_path()
381 return this->first_optimized_path_;
383 double &first_optimized_path_cost() {
384 return this->first_optimized_path_cost_;
386 void first_optimized_path_cost(double c) {
387 this->first_optimized_path_cost_ = c;
391 /*! \brief Use cute_c2 library for collision detection.
394 * \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
396 class RRTExt2 : public virtual RRTS {
400 std::vector<c2Poly> c2_obstacles_;
401 bool collide(RRTNode const& n);
402 bool collide_steered();
405 Json::Value json() const;
406 void json(Json::Value jvi);
409 /* \brief Different costs extension.
411 Use different cost for bulding tree data structure and searching in the
414 class RRTExt1 : public virtual RRTS {
416 /* \brief Reeds and Shepp path length.
418 double cost_build(RRTNode &f, RRTNode &t);
419 /* \brief Matej's heuristics.
421 double cost_search(RRTNode &f, RRTNode &t);
425 #endif /* RRTS_RRTEXT_H */