Fix findt() for multiple goals

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index bf366dd056624663be91466f8dfdc95a38d2670e..dbcf838f74f7887133a1ab34b03cc3f476241e38 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1026,7 +1026,14 @@ bool RRTBase::rebase(RRTNode *nr)
 
 std::vector<RRTNode *> RRTBase::findt()
 {
-        return this->findt(this->goal_);
+        RRTNode *goal = this->goal_;
+        for (auto g: this->goals()) {
+                if (goal->parent() == nullptr || g->ccost() < g->ccost())
+                        goal = g;
+        }
+        if (goal->parent() == nullptr)
+                this->goal_found(false);
+        return this->findt(goal);
 }
 
 std::vector<RRTNode *> RRTBase::findt(RRTNode *n)