Use normal distribution in sampling

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

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 5c46747fb8ae6f4214ef652c1b1bfa3df78d01ee..f83be79cfaa4751a91145969a047bdf1906d1fe1 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -21,6 +21,7 @@ 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"
@@ -59,9 +60,11 @@ 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_;
@@ -77,8 +80,11 @@ class RRTBase {
                 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 = 0.2;
                 const float GOAL_FOUND_ANGLE = M_PI / 32;
@@ -90,8 +96,10 @@ class RRTBase {
                 // 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<RRTNode *> iy_[IYSIZE];
@@ -106,10 +114,12 @@ class RRTBase {
                 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);
@@ -128,6 +138,10 @@ class RRTBase {
                 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,
@@ -145,6 +159,7 @@ class RRTBase {
 
                 // RRT Framework
                 SamplingInfo samplingInfo_;
+                bool useSamplingInfo_ = false;
                 RRTNode *sample();
                 float cost(RRTNode *init, RRTNode *goal);
                 RRTNode *nn(RRTNode *rs);