Stop when nodes().size() overflow

diff --git a/base/main.cc b/base/main.cc
index 1442d4a0da3183bed719ce8bcd948876019c46c9..b364173b14c0ed5caa4ebb3fd3be8652732e1704 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -197,7 +197,9 @@ int main()
         p.p_goal_.tstart();
         pthread_create(&rt, NULL, &next_run, (void *) &ra);
         pthread_create(&gt, NULL, &next_run, (void *) &ga);
-        while (!gf && p.elapsed() < TMAX) {
+        while (!gf && p.elapsed() < TMAX &&
+                        p.p_root_.nodes().size() < NOFNODES &&
+                        p.p_goal_.nodes().size() < NOFNODES) {
                 // overlap trees
                 for (int i = 0; i < IXSIZE; i++) {
                 for (int j = 0; j < IYSIZE; j++) {

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 0cabc238b81ab76b4e16ac2791e8749c21f27513..6e0cc4c2a20963eca81d8fdd60eeccf858219288 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -87,7 +87,7 @@ RRTBase::RRTBase():
         root_(new RRTNode()),
         goal_(new RRTNode())
 {
-        this->nodes_.reserve(20000);
+        this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(this->root_);
         this->add_iy(this->root_);
         this->add_ixy(this->root_);
@@ -97,7 +97,7 @@ RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
         root_(init),
         goal_(goal)
 {
-        this->nodes_.reserve(20000);
+        this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(init);
         this->add_iy(init);
         this->add_ixy(init);

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 2799d84691f11cb2d629b4776eda9ac49570260a..7a5c9323bea93d6c12b8eaf630bf774cd543e084 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -26,6 +26,8 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "rrtnode.h"
 #include "sample.h"
 
+#define NOFNODES 20000
+
 #define IXSIZE 100
 #define IXSTEP (1.0 * ((VMAX) - (VMIN)) / IXSIZE)
 #define IXI(x) ({ __typeof__ (x) _x = (x); \