Use normal distribution in sampling

[hubacji1/iamcar.git] / incl / rrtbase.h

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 50836a67fcc1d4dee6f41ac800167d3c96e07ca6..f83be79cfaa4751a91145969a047bdf1906d1fe1 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -20,16 +20,52 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #include <chrono>
 #include <cmath>
+#include <pthread.h>
+#include <queue>
+#include <random>
 #include <vector>
 #include "obstacle.h"
 #include "rrtnode.h"
+#include "sample.h"
+#include "slotplanner.h"
+
+#define NOFNODES 20000
+
+#define IXSIZE 100
+#define IXSTEP (1.0 * ((VMAX) - (VMIN)) / IXSIZE)
+#define IXI(x) ({ __typeof__ (x) _x = (x); \
+                std::abs((int) floor(_x / IXSTEP)); })
+#define IYSIZE 100
+#define IYSTEP (1.0 * ((HMAX) - (HMIN)) / IYSIZE)
+#define IYI(y) ({ __typeof__ (y) _y = (y); \
+                std::abs((int) floor(_y / IYSTEP)); })
+
+class Cell {
+        private:
+                std::vector<RRTNode *> nodes_;
+                pthread_mutex_t m_;
+                bool changed_ = false;
+        public:
+                Cell();
+
+                // getter
+                bool changed();
+                std::vector<RRTNode *> nodes();
+
+                // other
+                void add_node(RRTNode *n);
+};
 
 class RRTBase {
         private:
                 RRTNode *root_ = nullptr;
                 RRTNode *goal_ = nullptr;
+                std::vector<RRTNode *> goals_;
 
                 std::vector<RRTNode *> nodes_;
+                std::vector<RRTNode *> dnodes_;
+                std::queue<RRTNode *> firsts_;
+                PolygonObstacle frame_;
                 std::vector<RRTNode *> samples_;
                 std::vector<CircleObstacle> *cobstacles_;
                 std::vector<SegmentObstacle> *sobstacles_;
@@ -39,42 +75,103 @@ class RRTBase {
                 std::chrono::high_resolution_clock::time_point tend_;
 
                 std::vector<float> clog_; // costs of trajectories
+                std::vector<float> nlog_; // #nodes of RRT
+                std::vector<std::vector<RRTEdge *>> rlog_; // log tree
                 std::vector<float> slog_; // seconds of trajectories
                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
+                std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
+        protected:
+                std::default_random_engine gen_;
+                std::normal_distribution<float> ndx_;
+                std::normal_distribution<float> ndy_;
+                std::normal_distribution<float> ndh_;
         public:
-                const float GOAL_FOUND_DISTANCE = 1;
-                const float GOAL_FOUND_ANGLE = 2 * M_PI;
+                const float GOAL_FOUND_DISTANCE = 0.2;
+                const float GOAL_FOUND_ANGLE = M_PI / 32;
 
+                ~RRTBase();
                 RRTBase();
                 RRTBase(RRTNode *init, RRTNode *goal);
 
                 // getter
                 RRTNode *root();
                 RRTNode *goal();
+                std::vector<RRTNode *> &goals();
                 std::vector<RRTNode *> &nodes();
+                std::vector<RRTNode *> &dnodes();
+                std::queue<RRTNode *> &firsts();
+                PolygonObstacle &frame();
                 std::vector<RRTNode *> &samples();
-                std::vector<CircleObstacle> *cos();
-                std::vector<SegmentObstacle> *sos();
+                std::vector<RRTNode *> iy_[IYSIZE];
+                Cell ixy_[IXSIZE][IYSIZE];
+                std::vector<CircleObstacle> *co();
+                std::vector<SegmentObstacle> *so();
                 std::vector<float> &clog();
+                std::vector<float> &nlog();
+                std::vector<std::vector<RRTEdge *>> &rlog();
                 std::vector<float> &slog();
                 std::vector<std::vector<RRTNode *>> &tlog();
+                std::vector<RRTNode *> &slot_cusp();
                 bool goal_found();
                 float elapsed();
+                std::vector<RRTNode *> traj_cusp();
 
                 // setter
+                void root(RRTNode *node);
+                void goal(RRTNode *node);
+                void goals(std::vector<RRTNode *> g);
+                bool logr(RRTNode *root);
+                float ocost(RRTNode *n);
                 bool tlog(std::vector<RRTNode *> t);
                 void tstart();
                 void tend();
                 bool link_obstacles(
                                 std::vector<CircleObstacle> *cobstacles,
                                 std::vector<SegmentObstacle> *sobstacles);
+                bool add_iy(RRTNode *n);
+                bool add_ixy(RRTNode *n);
+                bool goal_found(bool f);
+                void slot_cusp(std::vector<RRTNode *> sc);
 
                 // other
+                bool glplot();
                 bool goal_found(
                                 RRTNode *node,
                                 float (*cost)(RRTNode *, RRTNode *));
+                bool goal_found(
+                        RRTNode *node,
+                        RRTNode *goal
+                );
                 bool collide(RRTNode *init, RRTNode *goal);
+                bool optp_dijkstra(
+                                std::vector<RRTNode *> &cusps,
+                                std::vector<int> &npi);
+                bool optp_rrp(
+                                std::vector<RRTNode *> &cusps,
+                                std::vector<int> &npi);
+                bool optp_smart(
+                                std::vector<RRTNode *> &cusps,
+                                std::vector<int> &npi);
+                bool opt_path();
+                bool rebase(RRTNode *nr);
                 std::vector<RRTNode *> findt();
+                std::vector<RRTNode *> findt(RRTNode *n);
+
+                // RRT Framework
+                SamplingInfo samplingInfo_;
+                bool useSamplingInfo_ = false;
+                RRTNode *sample();
+                float cost(RRTNode *init, RRTNode *goal);
+                RRTNode *nn(RRTNode *rs);
+                std::vector<RRTNode *> nv(RRTNode *node, float dist);
+                std::vector<RRTNode *> steer(RRTNode *init, RRTNode *goal);
+                std::vector<RRTNode *> steer(
+                                RRTNode *init,
+                                RRTNode *goal,
+                                float step);
+
+                // virtuals - implemented by child classes
+                virtual bool next() = 0;
 };
 
 #endif