Connect goals to goal

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index bc031bcd6c6db1ab67664590d8751c67ef2f52ab..60ed148d957f172eede34706db748eaa2cb908c3 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -236,6 +236,15 @@ void RRTBase::goal(RRTNode *node)
 void RRTBase::goals(std::vector<RRTNode *> g)
 {
         this->goals_ = g;
+        std::reverse(this->goals_.begin(), this->goals_.end());
+        RRTNode *pn = this->goals_.front();
+        for (auto n: this->goals_) {
+                if (n != pn) {
+                        pn->add_child(n, this->cost(pn ,n));
+                        pn = n;
+                }
+        }
+        pn->add_child(this->goal_, this->cost(pn, this->goal_));
 }
 
 bool RRTBase::logr(RRTNode *root)