Improve pick interesting method

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 602788674d2f4dc5d20d573b4abae51e00cca122..5545c95f40ad84aa466a4b052818f4d572730401 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -37,24 +37,24 @@ RRTExt13::pick_interesting()
        return;
 #endif
 #if 1 // cusp and 1st non-drivable path poses are interesting
+       this->dn_.push_back(DijkstraNode(this->opath_.front()));
+       this->dn_.back().i = this->dn_.size() - 1;
        for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
-       //unsigned int i = 0; { // just for cusp, comment out drivable section
                RRTNode& ni = *this->opath_[i];
                this->bc_.set_pose(ni);
                for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
                        RRTNode& nj = *this->opath_[j];
                        RRTNode& njl = *this->opath_[j - 1];
-                       if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())
-                                       || njl.edist(nj) < this->eta_) {
-                               this->dn_.push_back(DijkstraNode(&njl));
-                               this->dn_.back().i = this->dn_.size() - 1;
-                       }
                        if (!this->bc_.drivable(nj)) {
                                this->dn_.push_back(DijkstraNode(&njl));
                                this->dn_.back().i = this->dn_.size() - 1;
                                i = j;
                                break;
                        }
+                       if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+                               this->dn_.push_back(DijkstraNode(&njl));
+                               this->dn_.back().i = this->dn_.size() - 1;
+                       }
                }
        }
        this->dn_.push_back(DijkstraNode(this->opath_.back()));