Compute path only at the end of each iteration

diff --git a/src/rrts.cc b/src/rrts.cc
index 33600fcf8147631ddff4999ea3f577d840b904a9..8e1280e972730e125be69758569e83824f64b70d 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -303,6 +303,7 @@ RRTS::next()
        unsigned int ss = this->steered_.size();
        this->join_steered(&this->nodes_.back());
        RRTNode* just_added = &this->nodes_.back();
+       bool gf = false;
        while (ss > 0 && just_added->p() != nullptr) {
                //if (!this->goal_drivable_from(*just_added)) {
                //      ss--;
@@ -326,7 +327,7 @@ RRTS::next()
                                        || ncc < this->goal_.cc()) {
                                this->goal_.p(this->nodes_.back());
                                this->goal_.c(nc);
-                               this->compute_path();
+                               gf = true;
                        }
                }
                ss--;
@@ -342,6 +343,9 @@ RRTS::next()
                        *just_added));
        }
 #endif
+       if (gf) {
+               this->compute_path();
+       }
        ////if (!this->goal_drivable_from(this->nodes_.back())) {
        ////    return this->should_continue();
        ////}