Goal is last of goals

diff --git a/base/main.cc b/base/main.cc
index 422e3308ad572ecb0b803a4927875fe50c67346e..e028b2899b01a17c16c1e24cd4d3498f92c79a57 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -197,9 +197,9 @@ int main()
                 p.samplingInfo_ = ps.getSamplingInfo();
                 p.useSamplingInfo_ = true;
         }
-        p.goals(ps.goals());
         if (ps.cusp().size() > 0) {
                 p.goal(ps.getMidd());
+                p.goals(ps.goals());
                 p.slot_cusp(ps.cusp().front()); // use first found solution
                 jvo["midd"][0] = p.goal()->x();
                 jvo["midd"][1] = p.goal()->y();

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 60ed148d957f172eede34706db748eaa2cb908c3..9f14eed4644664b24dfa72f2c1fecf898c6e531e 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -244,7 +244,6 @@ void RRTBase::goals(std::vector<RRTNode *> g)
                         pn = n;
                 }
         }
-        pn->add_child(this->goal_, this->cost(pn, this->goal_));
 }
 
 bool RRTBase::logr(RRTNode *root)