+/*
+ * 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 Collision check based on enlarged occupancy grid.
+ *
+ * \ingroup ext-col
+ */
+class RRTExt21 : public virtual RRTS {
+private:
+ unsigned int _grid_width = 0;
+ unsigned int _grid_height = 0;
+ float _grid_res = 0.0;
+ int8_t const *_grid_data = nullptr;
+ double _origin_x = 0.0;
+ double _origin_y = 0.0;
+ bool collide(RRTNode const &n);
+ bool collide_steered();
+public:
+ void set_grid_to_check(unsigned int w, unsigned int h, float r,
+ int8_t const *d, double x, double y);
+};
+
+/*! \brief Collision check based on occupancy grid.
+ *
+ * This approach expects obstacles to be represented by points and the collision
+ * occures whenever the point is inside the frame given by the car pose and the
+ * car size.
+ *
+ * \ingroup ext-col
+ */
+class RRTExt20 : public virtual RRTS {
+private:
+ std::vector<bcar::Point> const *_points_to_check = nullptr;
+ bool collide_steered();
+public:
+ void set_points_to_check(std::vector<bcar::Point> const *p);
+};
+
+/*! \brief Use Dubins paths-based steering procedure.
+ *
+ * \ingroup ext-steer
+ * \see https://github.com/AndrewWalker/Dubins-Curves
+ */
+class RRTExt19 : public virtual RRTS {
+private:
+ void steer(RRTNode const &f, RRTNode const &t);
+};
+
+/*! \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.
+/*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
*
- * 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.
+ * 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 {
protected:
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.
+/*! \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 {
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_;
RRTExt2();
Json::Value json() const;
void json(Json::Value jvi);
+ void reset();
};
-/*! \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