-#ifndef RRTEXT_H
-#define RRTEXT_H
+#ifndef RRTS_RRTEXT_H
+#define RRTS_RRTEXT_H
-#include "rrts.h"
+#include "rrts.hh"
// ext2
#include "cute_c2.h"
// ext9
#define GRID_MAX_HI 60
+namespace rrts {
+
+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.
+ *
+ * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
+ */
+class RRTExt14 : public virtual RRTS {
+private:
+ Point circle_c_;
+ double circle_r_ = 0.0;
+ std::uniform_real_distribution<double> udr_;
+ std::uniform_real_distribution<double> udt_;
+ std::uniform_real_distribution<double> udh_;
+ RRTNode sample();
+public:
+ RRTExt14();
+ void reset();
+};
+
/*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
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;
+ bool vi();
+ DijkstraNode(RRTNode* n);
+ };
+ class DijkstraNodeComparator {
+ 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 pick_interesting();
+ 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 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.
-
-*/
+ *
+ * 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.
+ */
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 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
-*/
+ *
+ * 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
+ */
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.
\see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
*/
class RRTExt2 : public virtual RRTS {
- private:
- c2Poly c2_bc_;
- c2x c2x_bc_;
- std::vector<c2Poly> c2_obstacles_;
- public:
- void init();
- void deinit();
-
- // Collide RRT procedures
- std::tuple<bool, unsigned int, unsigned int>
- collide_steered_from(RRTNode &f);
- std::tuple<bool, unsigned int, unsigned int>
- collide_tmp_steered_from(RRTNode &f);
-
- std::tuple<bool, unsigned int, unsigned int>
- collide_two_nodes(RRTNode &f, RRTNode &t);
-
- // getters, setters
- c2Poly &c2_bc() { return this->c2_bc_; }
- c2x &c2x_bc() { return this->c2x_bc_; }
- std::vector<c2Poly> &c2_obstacles() {
- return this->c2_obstacles_;
- };
+private:
+ c2Poly c2_bc_;
+ c2x c2x_bc_;
+ std::vector<c2Poly> c2_obstacles_;
+ bool collide(RRTNode const& n);
+ bool collide_steered();
+public:
+ RRTExt2();
+ Json::Value json() const;
+ void json(Json::Value jvi);
};
/*! \brief Different costs extension.
double cost_search(RRTNode &f, RRTNode &t);
};
-#endif /* RRTEXT_H */
+} // namespace rrts
+#endif /* RRTS_RRTEXT_H */
projects / hubacji1 / rrts.git / blobdiff