Fix negative cost of final path

diff --git a/base/main.cc b/base/main.cc
index b364173b14c0ed5caa4ebb3fd3be8652732e1704..87ab9799470e6417e8081010b5c7fb24c9d4b6b5 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -233,6 +233,9 @@ for (auto gn: p.p_goal_.ixy_[i][j].nodes()) {
                 gon = p.p_goal_.goal()->parent();
                 mc = p.p_goal_.goal()->ccost();
         }
+        p.root()->remove_parent();  // needed if p.p_goal_.goal_found()
+        p.root()->ccost(0);
+        p.goal()->children().clear();
         // connect trees
         while (gon != p.goal()) {
                 p.p_root_.nodes().push_back(new RRTNode(
@@ -249,7 +252,6 @@ for (auto gn: p.p_goal_.ixy_[i][j].nodes()) {
         }
         ron->add_child(p.goal(), p.p_root_.cost(ron, p.goal()));
         // end of connect trees
-        p.root()->remove_parent();  // needed if p.p_goal_.goal_found()
         if (gf)
                 p.tlog(p.findt());
         if (p.opt_path())