Fix adding optimized path

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 303074196e192ca763287d1a5f6336623b3b577b..706eab2c10ca913ff05b8bb4a730b109b75c00cf 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -634,23 +634,33 @@ bool RRTBase::opt_path()
         if (!this->optp_dijkstra(cusps, npi))
                 return false;
         RRTNode *pn = cusps[npi[0]];
+        RRTNode *tmp = nullptr;
+        bool en_add = true;
         for (unsigned int i = 0; i < npi.size() - 1; i++) {
+                pn = cusps[npi[i]];
                 for (auto ns: this->steer(cusps[npi[i]], cusps[npi[i + 1]])) {
-                        if (IS_NEAR(cusps[npi[i]], ns)) {
+                        if (!en_add) {
                                 delete ns;
-                                continue;
-                        }
-                        if (IS_NEAR(ns, cusps[npi[i + 1]])) {
+                        } else if (IS_NEAR(cusps[npi[i]], ns)) {
+                                tmp = ns;
+                                while (tmp && tmp != cusps[npi[i]]) {
+                                        pn = tmp->parent();
+                                        delete tmp;
+                                        tmp = pn;
+                                }
+                                pn = cusps[npi[i]];
+                        } else 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;
+                                en_add = false;
+                        } else {
+                                pn->add_child(ns, this->cost(pn, ns));
+                                pn = ns;
                         }
-                        pn->add_child(ns, this->cost(pn, ns));
-                        pn = ns;
                 }
         }
         this->root()->update_ccost();