Fix path optimization procedure

- Use `cost` from planner class.
- Update path even if goal not found.
- Fix adding nodes after steering.
- Update all the costs in tree.

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index c7dd5ba0698167f625017103844deb1eca211cd8..b9a575d8237504805c0b2f8675763d373eb7c297 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -570,12 +570,12 @@ bool RRTBase::opt_path()
                 pq.pop();
                 for (unsigned int i = tmp.ni + 1; i < cusps.size(); i++) {
                         ch_cost = dnodes[tmp.ni].c +
-                                CO(cusps[tmp.ni], cusps[i]);
+                                this->cost(cusps[tmp.ni], cusps[i]);
                         steered = this->steer(cusps[tmp.ni], cusps[i]);
                         for (unsigned int j = 0; j < steered.size() - 1; j++)
                                 steered[j]->add_child(
                                                 steered[j + 1],
-                                                CO(
+                                                this->cost(
                                                         steered[j],
                                                         steered[j + 1]));
                         if (this->collide(
@@ -590,10 +590,12 @@ bool RRTBase::opt_path()
                         }
                 }
         }
-        if (tmp.ni != cusps.size() - 1)
-                return false;
         std::vector<int> npi; // new path indexes
-        int tmpi = tmp.ni;
+        unsigned int tmpi = 0;
+        for (auto n: dnodes) {
+                if (n.v && n.ni > tmpi)
+                        tmpi = n.ni;
+        }
         while (tmpi > 0) {
                 npi.push_back(tmpi);
                 tmpi = dnodes[tmpi].pi;
@@ -603,14 +605,25 @@ bool RRTBase::opt_path()
         RRTNode *pn = cusps[npi[0]];
         for (unsigned int i = 0; i < npi.size() - 1; i++) {
                 for (auto ns: this->steer(cusps[npi[i]], cusps[npi[i + 1]])) {
-                        pn->add_child(ns, CO(pn, ns));
+                        if (IS_NEAR(cusps[npi[i]], ns)) {
+                                delete ns;
+                                continue;
+                        }
+                        if (IS_NEAR(ns, cusps[npi[i + 1]])) {
+                                delete ns;
+                                cusps[npi[i + 1]]->parent()->rem_child(
+                                                cusps[npi[i + 1]]);
+                                pn->add_child(
+                                        cusps[npi[i + 1]],
+                                        this->cost(pn, cusps[npi[i + 1]]));
+                                break;
+                        }
+                        pn->add_child(ns, this->cost(pn, ns));
                         pn = ns;
                 }
         }
-        pn->add_child(
-                        this->tlog().back().front(),
-                        CO(pn, this->tlog().back().front()));
         // End of Dijkstra
+        this->root()->update_ccost();
         if (this->tlog().back().front()->ccost() < oc)
                 return true;
         return false;