]> rtime.felk.cvut.cz Git - hubacji1/rrts.git/blobdiff - api/rrtext.h
Add reset method, remove debug print
[hubacji1/rrts.git] / api / rrtext.h
index 128fe0c49334d7674b06ee454569d95f5439810a..4e1f825cbef208dfc7488f04e7f6fc7ba4831db5 100644 (file)
 
 // ext4
 #define GRID 1 // in meters
-#define GRID_WIDTH 20 // in meters
-#define GRID_HEIGHT 50 // in meters
+#define GRID_WIDTH 40 // in meters
+#define GRID_HEIGHT 40 // in meters
 #define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
 #define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
 
+// ext9
+#define GRID_MAX_HI 60
+
+/*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
+class RRTExt13 : public virtual RRTS {
+        private:
+        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;
+                }
+};
+
+/*! \brief Different `steer` procedures.
+
+Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
+*/
+class RRTExt12 : public virtual RRTS {
+        protected:
+                void steer1(RRTNode &f, RRTNode &t);
+                bool connect();
+        public:
+                bool next();
+};
+
+/*! \brief Goal Zone.
+
+*/
+class RRTExt11 : public virtual RRTS {
+        protected:
+                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.
+
+*/
+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);
+};
+
+/*! \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.
+*/
+class RRTExt9 : public virtual RRTS {
+        private:
+                class Cell {
+                        private:
+                                bool changed_ = false;
+                                std::vector<RRTNode *> nodes_;
+                        public:
+                                void nn(RRTNode *t, RRTNode **nn, RRTS *p);
+                                void store_node(RRTNode *n);
+
+                                // getter, setter
+                                bool changed() const
+                                {
+                                        return this->changed_;
+                                }
+                                std::vector<RRTNode *> &nodes()
+                                {
+                                        return this->nodes_;
+                                }
+
+                                Cell();
+                };
+                Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
+                double x_min_ = 0;
+                double x_max_ = 0;
+                double y_min_ = 0;
+                double y_max_ = 0;
+                double h_min_ = 0;
+                double h_max_ = 2 * M_PI;
+
+                unsigned int xi(RRTNode n);
+                unsigned int yi(RRTNode n);
+                unsigned int hi(RRTNode n);
+        public:
+                void init();
+                void deinit();
+                void store_node(RRTNode n);
+                RRTNode *nn(RRTNode &t);
+                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
@@ -38,8 +160,21 @@ class RRTExt8 : public virtual RRTS {
                 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
+                );
         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);
@@ -134,10 +269,10 @@ class RRTExt4 : public virtual RRTS {
                                 Cell();
                 };
                 Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
-                unsigned int x_min_ = 0;
-                unsigned int x_max_ = 0;
-                unsigned int y_min_ = 0;
-                unsigned int y_max_ = 0;
+                double x_min_ = 0;
+                double x_max_ = 0;
+                double y_min_ = 0;
+                double y_max_ = 0;
 
                 unsigned int xi(RRTNode n);
                 unsigned int yi(RRTNode n);
@@ -153,10 +288,17 @@ class RRTExt4 : public virtual RRTS {
 */
 class RRTExt3 : public virtual RRTS {
         private:
-                std::vector<RRTNode *> orig_path_;
-                double orig_path_cost_;
         public:
-                std::vector<RRTNode *> path();
+                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()
@@ -165,6 +307,16 @@ class RRTExt3 : public virtual RRTS {
                 };
                 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;
+                }
 };
 
 /*! \brief Use cute_c2 for collision detection.
@@ -183,6 +335,8 @@ class RRTExt2 : public virtual RRTS {
                 // 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);