From 3ede3b5a06e8f176f6690f49af79b80e6f4b1042 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 10 Jun 2019 13:09:49 +0200
Subject: [PATCH] Connect goals to goal

---
 base/rrtbase.cc | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index bc031bc..60ed148 100644
--- 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)
-- 
2.39.2