+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+/*! \brief RRT* extensions.
+ *
+ * The extensions are used to implement or change the default behavior of the
+ * RRT* algorithm.
+ *
+ * \file
+ * \defgroup ext-col Collision detection extensions
+ * \defgroup ext-store Node storage and searching tree extensions
+ * \defgroup ext-cost Cost extensions
+ * \defgroup ext-opt Path optimization extensions
+ * \defgroup ext-sampl Random sampling extensions
+ * \defgroup ext-steer Steering procedure extensions
+ * \defgroup ext-aux Auxilliary extensions
+ */
#ifndef RRTS_RRTEXT_H
#define RRTS_RRTEXT_H
namespace rrts {
+/*! \brief Finish when more than 1000 iterations.
+ *
+ * \ingroup ext-aux
+ */
+class RRTExt18 : public virtual RRTS {
+private:
+ bool should_finish() const;
+};
+
+/*! \brief Finish when goal found or more than 1000 iterations.
+ *
+ * \ingroup ext-aux
+ */
+class RRTExt17 : public virtual RRTS {
+private:
+ bool should_finish() const;
+};
+
+/*! \brief Use Reeds & Shepp steering procedure.
+ *
+ * \ingroup ext-steer
+ */
+class RRTExt16 : public virtual RRTS {
+private:
+ void steer(RRTNode const& f, RRTNode const& t);
+};
+
+/*! \brief Log goal's cumulative cost each iteration.
+ *
+ * \ingroup ext-aux
+ */
+class RRTExt15 : public virtual RRTS {
+private:
+ std::vector<double> log_path_cost_;
+public:
+ Json::Value json() const;
+ void json(Json::Value jvi);
+ bool next();
+};
+
/*! \brief Random sampling in the circuit between root and goal.
*
+ * \ingroup ext-sampl
* \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
*/
class RRTExt14 : public virtual RRTS {
void reset();
};
-/*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
+/*! \brief Use Dijkstra algorithm to find path between interesting nodes.
+ *
+ * The search for interesting nodes starts at root, the last drivable nodes is
+ * added to interesting nodes until goal is reached.
+ *
+ * \ingroup ext-opt
+ */
class RRTExt13 : public virtual RRTS {
- private:
+private:
+ class DijkstraNode {
public:
- void reset();
- std::vector<RRTNode *> orig_path_;
- double orig_path_cost_ = 9999;
- std::vector<RRTNode *> first_optimized_path_;
- double first_optimized_path_cost_ = 9999;
- void first_path_optimization();
- void second_path_optimization();
- void compute_path();
- Json::Value json();
- void json(Json::Value jvi);
-
- // getter, setter
- std::vector<RRTNode *> &orig_path()
- {
- return this->orig_path_;
- };
- double &orig_path_cost() { return this->orig_path_cost_; }
- void orig_path_cost(double c) { this->orig_path_cost_ = c; }
- std::vector<RRTNode *> &first_optimized_path()
- {
- return this->first_optimized_path_;
- };
- double &first_optimized_path_cost() {
- return this->first_optimized_path_cost_;
- }
- void first_optimized_path_cost(double c) {
- this->first_optimized_path_cost_ = c;
- }
+ RRTNode* node = nullptr;
+ unsigned int i = 0;
+ bool v = false;
+ double d = 0.0;
+ bool vi();
+ DijkstraNode(RRTNode* n);
+ };
+ class DijkstraNodeComparator {
+ public:
+ int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+ };
+ class DijkstraNodeBackwardComparator {
+ public:
+ int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+ };
+ std::vector<RRTNode*> opath_;
+ double ogoal_cc_ = 0.0;
+ double otime_ = 0.0;
+ std::vector<DijkstraNode> dn_;
+ void interesting_forward();
+ void interesting_backward();
+ void dijkstra_forward();
+ void dijkstra_backward();
+ void compute_path();
+public:
+ RRTExt13();
+ Json::Value json() const;
+ void json(Json::Value jvi);
+ void reset();
};
-/*! \brief Different `steer` procedures.
+/* \brief Different `steer` procedures.
Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
*/
bool next();
};
-/*! \brief Goal Zone.
+/* \brief Goal Zone.
*/
class RRTExt11 : public virtual RRTS {
bool goal_found(RRTNode &f);
};
-/*! \brief Different costs extension.
-
-Use different cost for bulding tree data structure and searching in the
-structure. The cost function is from Elbanhawi, Mohamed, Milan Simic, and Reza
-Jazar. “Randomized Bidirectional B-Spline Parameterization Motion Planning.”
-IEEE Transactions on Intelligent Transportation Systems 17, no. 2 (February
-2016): 406–19. https://doi.org/10.1109/TITS.2015.2477355.
-
-*/
+/*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
+ *
+ * Use Reeds & Shepp path length for building tree data structure and Euclidean
+ * distance + (abs) heading difference + 0.1 * backward-forward direction
+ * changes for searching it.
+ *
+ * \ingroup ext-cost
+ * \see https://doi.org/10.1109/TITS.2015.2477355
+ */
class RRTExt10 : public virtual RRTS {
- public:
- /*! \brief Reeds and Shepp path length.
- */
- double cost_build(RRTNode &f, RRTNode &t);
- /*! \brief Heuristics distance.
- */
- double cost_search(RRTNode &f, RRTNode &t);
+protected:
+ double cost_build(RRTNode const& f, RRTNode const& t) const;
+ double cost_search(RRTNode const& f, RRTNode const& t) const;
};
-/*! \brief Use grid data structure to store RRT nodes.
+/* \brief Use grid data structure to store RRT nodes.
This approach speeds up the search process for the nearest neighbor and
the near vertices procedures.
std::vector<RRTNode *> nv(RRTNode &t);
};
-/*! \brief Use k-d tree data structure to store RRT nodes.
-
-This approach speeds up the search process for the nearest neighbor and
-the near vertices procedures. This extension implements 3D K-d tree.
-
-\see https://en.wikipedia.org/wiki/K-d_tree
-*/
+/*! \brief Use 3D k-d tree data structure to store RRT nodes.
+ *
+ * This approach speeds up the search process for the nearest neighbor and the
+ * near vertices procedures. This extension implements 3D K-d tree.
+ *
+ * \ingroup ext-store
+ * \see https://en.wikipedia.org/wiki/K-d_tree
+ */
class RRTExt8 : public virtual RRTS {
- private:
- class KdNode {
- private:
- RRTNode *node_ = nullptr;
- KdNode *left_ = nullptr;
- KdNode *right_ = nullptr;
- public:
- // getter, setter
- RRTNode *node() const { return this->node_; }
- KdNode *&left() { return this->left_; }
- KdNode *&right() { return this->right_; }
- KdNode(RRTNode *n);
- };
- KdNode *kdroot_ = nullptr;
- void delete_kd_nodes(KdNode *n);
- void store_node(RRTNode *n, KdNode *&r, int l);
- void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
- void nv(
- std::vector<RRTNode*>& n,
- RRTNode &t,
- KdNode *r,
- int l,
- double const& d
- );
+private:
+ class KdNode {
public:
- void delete_kd_nodes()
- {
- this->delete_kd_nodes(this->kdroot_);
- this->kdroot_ = nullptr;
- }
- void init();
- void reset();
- void deinit();
- void store_node(RRTNode n);
- RRTNode *nn(RRTNode &t);
- std::vector<RRTNode *> nv(RRTNode &t);
+ RRTNode* node = nullptr;
+ KdNode* left = nullptr;
+ KdNode* right = nullptr;
+ KdNode(RRTNode* n);
+ };
+ KdNode* kdroot_ = nullptr;
+ std::vector<KdNode> kdnodes_;
+ void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
+ void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
+ void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
+public:
+ RRTExt8();
+ void reset();
+ void store(RRTNode n);
+ void find_nn(RRTNode const& t);
+ void find_nv(RRTNode const& t);
};
-/*! \brief Use k-d tree data structure to store RRT nodes.
+/* \brief Use k-d tree data structure to store RRT nodes.
This approach speeds up the search process for the nearest neighbor and
the near vertices procedures. This extension implements 2D K-d tree.
std::vector<RRTNode *> nv(RRTNode &t);
};
-/*! \brief Reeds and Shepp cost for building and search.
-*/
+/*! \brief Reeds & Shepp (build, search).
+ *
+ * Use Reeds & Shepp path length for building tree data structure as well as for
+ * searching it.
+ *
+ * \ingroup ext-cost
+ */
class RRTExt6 : public virtual RRTS {
- public:
- /*! \brief Reeds and Shepp path length.
- */
- double cost_build(RRTNode &f, RRTNode &t);
- /*! \brief Reeds and Shepp path length.
- */
- double cost_search(RRTNode &f, RRTNode &t);
+private:
+ double cost_build(RRTNode const& f, RRTNode const& t) const;
+ double cost_search(RRTNode const& f, RRTNode const& t) const;
};
-/*! \brief Different costs extension.
+/* \brief Different costs extension.
Use different cost for bulding tree data structure and searching in the
structure. This one is complementary to `rrtext1.cc`.
*/
class RRTExt5 : public virtual RRTS {
public:
- /*! \brief Reeds and Shepp path length.
+ /* \brief Reeds and Shepp path length.
*/
double cost_build(RRTNode &f, RRTNode &t);
- /*! \brief Euclidean distance.
+ /* \brief Euclidean distance.
*/
double cost_search(RRTNode &f, RRTNode &t);
};
-/*! \brief Use grid data structure to store RRT nodes.
+/* \brief Use grid data structure to store RRT nodes.
This approach speeds up the search process for the nearest neighbor and
the near vertices procedures.
std::vector<RRTNode *> nv(RRTNode &t);
};
-/*! \brief Use Dijkstra algorithm to find the shorter path.
+/* \brief Use Dijkstra algorithm to find the shorter path.
*/
class RRTExt3 : public virtual RRTS {
private:
}
};
-/*! \brief Use cute_c2 for collision detection.
-
-\see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
-*/
+/*! \brief Use cute_c2 library for collision detection.
+ *
+ * \ingroup ext-col
+ * \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
+ */
class RRTExt2 : public virtual RRTS {
private:
c2Poly c2_bc_;
void json(Json::Value jvi);
};
-/*! \brief Different costs extension.
+/* \brief Different costs extension.
Use different cost for bulding tree data structure and searching in the
structure.
*/
class RRTExt1 : public virtual RRTS {
public:
- /*! \brief Reeds and Shepp path length.
+ /* \brief Reeds and Shepp path length.
*/
double cost_build(RRTNode &f, RRTNode &t);
- /*! \brief Matej's heuristics.
+ /* \brief Matej's heuristics.
*/
double cost_search(RRTNode &f, RRTNode &t);
};
projects / hubacji1 / rrts.git / blobdiff