Run connect trees of T3 only if goal found

diff --git a/base/main.cc b/base/main.cc
index 6fdb3152411e519a1ab47d5efa6e9a435c0c2532..0a874317c38603f6e8a07d3d35b34e533e710298 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -241,6 +241,7 @@ escapeloop:
         p.root()->ccost(0);
         p.goal()->children().clear();
         // connect trees
+        if (gf) {
         while (gon != p.goal()) {
                 p.p_root_.nodes().push_back(new RRTNode(
                                 gon->x(),
@@ -255,6 +256,7 @@ escapeloop:
                 gon = gon->parent();
         }
         ron->add_child(p.goal(), p.p_root_.cost(ron, p.goal()));
+        }
         // end of connect trees
         if (gf)
                 p.tlog(p.findt());