Set T3 goal found when trees overlap

diff --git a/base/main.cc b/base/main.cc
index 852a47ce73c7b37f9dfe44f59c9f1cd6bc1def21..3098a69f748f47bd58e72743886d29f0c31b2726 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -216,6 +216,7 @@ int main()
                                 if (rn->ccost() + gn->ccost() < mc &&
                                                 IS_NEAR(rn, gn)) {
                                         gf = true;
+                                        p.goal_found(true);
                                         ron = rn;
                                         gon = gn;
                                         mc = rn->ccost() + gn->ccost();
@@ -227,7 +228,6 @@ int main()
         }
         pthread_join(rt, NULL);
         pthread_join(gt, NULL);
-        p.goal_found(gf);
         // connect trees
         while (gon != p.goal()) {
                 p.p_root_.nodes().push_back(new RRTNode(