Fix goal found for Klemm2015

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index cca76816dd25ef603e99326a8187afa44be8887b..27992ee5fea39bf302cf8baffc4bade96c45696a 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -731,6 +731,7 @@ bool Klemm2015::next()
 {
         RRTNode *xn = nullptr;
         RRTNode *rs;
+        int ret = 0;
 #if GOALFIRST > 0
         if (this->samples().size() == 0)
                 rs = this->goal();
@@ -749,10 +750,14 @@ bool Klemm2015::next()
         //                std::cerr << "- xn: nullptr" << std::endl;
         //        }
                 this->swap();
-                this->connectstar(xn);
+                ret = this->connectstar(xn);
         } else {
                 this->swap();
         }
+        if (ret == 1) {
+                this->tlog(this->findt());
+                return true;
+        }
         return this->goal_found();
 }
 
@@ -834,10 +839,6 @@ int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
        } else {
                 // rewire
                 this->rewire(nvs, ns);
-                if (this->goal_found(ns, this->cost)) {
-                        this->tlog(this->findt());
-                        ret = 1;
-                }
        }
         for (auto n: steered) {
                 if (n != steered[1])
@@ -910,11 +911,33 @@ int Klemm2015::extendstarC(RRTNode *rs)
                                 // rewire
                                 this->rewire(nvs, ns);
                                 pn = ns;
-                                if (this->goal_found(pn, this->cost)) {
-                                        this->tlog(this->findt());
-                                        en_add = false;
-                                        ret = 1;
-                                }
+if (IS_NEAR(pn, rs)) { // GOAL FOUND !
+        RRTNode *tmp;
+        if (this->orig_root_ == this->root()) { // rs is in G tree
+                // add all rs parents to pn
+                tmp = rs->parent();
+        } else { // rs is in R tree
+                tmp = pn->parent();
+                pn = rs;
+                this->swap();
+        }
+        while (tmp != this->goal()) {
+                this->nodes().push_back(new RRTNode(
+                                tmp->x(),
+                                tmp->y(),
+                                tmp->h()));
+                this->nodes().back()->s(tmp->s());
+                this->nodes().back()->tree('R');
+                pn->add_child(
+                                this->nodes().back(),
+                                this->cost(pn, this->nodes().back()));
+                pn = this->nodes().back();
+                tmp = tmp->parent();
+        }
+        pn->add_child(tmp, this->cost(pn, tmp)); // add goal()
+        en_add = false;
+        ret = 1;
+}
                         }
                 }
         }