Fix duplicated nodes when steered

Require steer(f, t) method to set _steered member variable to include f
and t, i.e. f == _steered.front() and t == _steered.back().

This has consequences for collide_steered method as if there is only one
node left, it is already part of the tree and all the other (steered)
nodes collide. In such a case, drop the steered as colliding.

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 2e85862c529e2e14fd29ca977af035124d367d17..8c578b30ae9a58856d69f18491436a68a6997594 100644 (file)
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -115,6 +115,12 @@ protected:
        virtual void find_nv(RRTNode const& t);
        virtual void compute_path();
 protected:
+       /*! \brief Return nodes from f to t inclusive.
+        *
+        * The "inclusive" means that f is at the same pose (x, y, h) as
+        * this->_steered.front() and t is at the same pose (x, y, h) as
+        * this->_steered.back().
+        */
        virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
        virtual bool collide_steered() = 0;
        virtual RRTNode sample() = 0;

diff --git a/rrts/src/rrtext2.cc b/rrts/src/rrtext2.cc
index 087e863f309063078f35f23f10c9dc95bd8cbc02..5dcceae76a15c7a9a6c6fd540ca416d721eb4495 100644 (file)
--- a/rrts/src/rrtext2.cc
+++ b/rrts/src/rrtext2.cc
@@ -37,7 +37,9 @@ RRTExt2::collide_steered()
                i++;
        }
        this->_steered.erase(this->_steered.begin() + i, this->_steered.end());
-       return this->_steered.size() == 0;
+       // The first node of this->_steered is the same as nn. Therefore, if
+       // there is only one node left, it is nn, and other nodes collide.
+       return this->_steered.size() == 1;
 }
 
 RRTExt2::RRTExt2() : RRTS()

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 7cd4684deadee7ab413b97553309b178becd3678..ea302e9d85fbb6edb48571f4a8532f674e167fd0 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -151,6 +151,14 @@ RRTS::connect()
 {
        RRTNode* f = this->_nn;
        RRTNode* t = &this->_steered.front();
+       // Require the steer method to return first node equal to nn:
+       assert(std::abs(t->x() - f->x()) < 1e-3);
+       assert(std::abs(t->y() - f->x()) < 1e-3);
+       assert(std::abs(t->h() - f->x()) < 1e-3);
+       // When f and t has different directions, the node (f == t) is cusp:
+       // TODO
+       this->_steered.erase(this->_steered.begin());
+       t = &this->_steered.front();
 #if USE_RRTS
        double cost = f->cc() + this->cost_build(*f, *t);
        for (auto n: this->nv_) {