Make optimization independent on goal

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 1047fb8a077e64c62134b07dd91bb1c9b0ec4460..3a3770fc76f63dace3fc5c9307d52ae18fb0ae15 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -629,7 +629,7 @@ bool T2::opt_path()
 {
         if (this->tlog().size() == 0)
                 return false;
-        float oc = this->goal()->ccost();
+        float oc = this->tlog().back().front()->ccost();
         std::vector<RRTNode *> tmp_cusps;
         for (auto n: this->tlog().back()) {
                 if (n->parent() && sgn(n->s()) != sgn(n->parent()->s())) {
@@ -644,7 +644,7 @@ bool T2::opt_path()
         }
         cusps.push_back(this->root());
         std::reverse(cusps.begin(), cusps.end());
-        cusps.push_back(this->goal());
+        cusps.push_back(this->tlog().back().front());
         // Begin of Dijkstra
         std::vector<RRTNodeDijkstra> dnodes;
         for (unsigned int i = 0; i < cusps.size(); i++)
@@ -700,9 +700,11 @@ bool T2::opt_path()
                         pn = ns;
                 }
         }
-        pn->add_child(this->goal(), this->cost(pn, this->goal()));
+        pn->add_child(
+                        this->tlog().back().front(),
+                        this->cost(pn, this->tlog().back().front()));
         // End of Dijkstra
-        if (this->goal()->ccost() < oc)
+        if (this->tlog().back().front()->ccost() < oc)
                 return true;
         return false;
 }