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;
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()
{
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_) {