Merge branch 'fix/rrts-star'

diff --git a/README.md b/README.md
index fbe6399cb804cee37742bcdff0ae0e7acd7f4cf5..0d399bdb1c6fc0fb5aa41f070b2fde1d95ad7a39 100644 (file)
--- a/README.md
+++ b/README.md
@@ -15,6 +15,14 @@ cmake ../
 make
 ```
 
+To build with ninja:
+```
+mkdir build
+cd build
+cmake -DCMAKE_BUILD_TYPE=Release -G Ninja ../
+ninja -v
+```
+
 # RRT Extensions
 There is basic RRT\* algorithm in `rrts.cc` file. To test different approaches
 and upgrades to RRT, *extensions* are declared in `rrtext.h` and implemented in

diff --git a/api/rrts.h b/api/rrts.h
index 58f6ad8cfd4f4b7e35def14f444504aea6c0de0c..8c344cb6dc9e43f63a85661e07097e4da0052159 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -69,8 +69,8 @@ class RRTS {
                 collide_steered_from(RRTNode &f);
                 std::tuple<bool, unsigned int, unsigned int>
                 collide_two_nodes(RRTNode &f, RRTNode &t);
-                double cost_build(RRTNode &f, RRTNode &t);
-                double cost_search(RRTNode &f, RRTNode &t);
+                virtual double cost_build(RRTNode &f, RRTNode &t);
+                virtual double cost_search(RRTNode &f, RRTNode &t);
                 void sample();
                         std::default_random_engine gen_;
                         std::normal_distribution<double> ndx_;
@@ -114,6 +114,7 @@ class RRTS {
                 );
 
                 // getters, setters
+                unsigned int icnt() const { return this->icnt_; }
                 std::vector<RRTNode> &goals() { return this->goals_; }
                 std::vector<RRTNode> &nodes() { return this->nodes_; }
                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }

diff --git a/src/rrts.cc b/src/rrts.cc
index 4458d32108c5877ddeb648a579df7248a21fc17f..c09c59babb20414aa0ad9b71db048f851cc02427 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -171,7 +171,6 @@ bool RRTS::goal_found(RRTNode &f)
 // RRT* procedures
 bool RRTS::connect()
 {
-        bool conn = false;
         RRTNode *t = &this->steered().front();
         RRTNode *f = this->nn(this->samples().back());
         double cost = this->cost_search(*f, *t);
@@ -188,8 +187,7 @@ bool RRTS::connect()
         t = &this->nodes().back();
         t->p(f);
         t->c(this->cost_build(*f, *t));
-        conn = true;
-        return conn;
+        return true;
 }
 
 void RRTS::rewire()
@@ -234,7 +232,8 @@ std::vector<RRTNode *> RRTS::path()
 bool RRTS::next()
 {
         bool next = true;
-        this->icnt_++;
+        if (++this->icnt_ > 999)
+                next = false;
         this->sample();
         this->steer(
                 *this->nn(this->samples().back()),
@@ -264,8 +263,6 @@ bool RRTS::next()
                 next = !this->goal_found(this->nodes().back());
                 just_added = just_added->p();
         }
-        if (this->icnt_ > 999)
-                next = false;
         return next;
 }