Remove multiple goals from next procedure

[hubacji1/rrts.git] / src / rrts.cc

diff --git a/src/rrts.cc b/src/rrts.cc
index 0aab4df0a037bcd07cf5f5c9e7ee97779737594e..2010c1c4c1895364a7beb4b75d6f8f759ed636fc 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -346,14 +346,13 @@ bool RRTS::next()
         RRTNode *just_added = &this->nodes().back();
         while (scnt > 0) {
                 scnt--;
-                for (auto &g: this->goals()) {
-                        this->steer(*just_added, g);
-                        if (std::get<0>(this->collide_steered_from(
-                                *just_added
-                        )))
-                                continue;
-                        this->join_steered(just_added);
-                }
+                auto &g = this->goals().front();
+                this->steer(*just_added, g);
+                if (std::get<0>(this->collide_steered_from(
+                        *just_added
+                )))
+                        continue;
+                this->join_steered(just_added);
                 this->gf(this->goal_found(this->nodes().back()));
                 just_added = just_added->p();
         }