Add firsts, firsts getter

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index da51b5af802f5274dca94915b7657c883301d153..5768286c8f3c28c665980fdea2c623aed1727da2 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -139,6 +139,11 @@ std::vector<RRTNode *> &RRTBase::dnodes()
         return this->dnodes_;
 }
 
+std::queue<RRTNode *> &RRTBase::firsts()
+{
+        return this->firsts_;
+}
+
 PolygonObstacle &RRTBase::frame()
 {
         return this->frame_;

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 1f1b6f479334ff7491baace6a7cb3bc44ac27b59..4ece3a7ab98358e2f9f94c6f86ef93c28a4d0815 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"
@@ -63,6 +64,7 @@ class RRTBase {
 
                 std::vector<RRTNode *> nodes_;
                 std::vector<RRTNode *> dnodes_;
+                std::queue<RRTNode *> firsts_;
                 PolygonObstacle frame_;
                 std::vector<RRTNode *> samples_;
                 std::vector<CircleObstacle> *cobstacles_;
@@ -94,6 +96,7 @@ class RRTBase {
                 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];