Fix enhance path when goal already found

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index fef0440fa52d1be01e0c57c202ebb5578d51f887..29b3cecf4d9439607b1a2c7c4503b1c2c9311c4c 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -109,20 +109,45 @@ bool RRTBase::goal_found(
                 RRTNode *node,
                 float (*cost)(RRTNode *, RRTNode* ))
 {
-        if (this->goal_found_)
-                return false;
         float xx = pow(node->x() - this->goal_->x(), 2);
         float yy = pow(node->y() - this->goal_->y(), 2);
         float dh = std::abs(node->h() - this->goal_->h());
         if (pow(xx + yy, 0.5) < this->GOAL_FOUND_DISTANCE &&
                         dh < this->GOAL_FOUND_ANGLE) {
-                node->add_child(this->goal_, (*cost)(node, this->goal_));
-                if (this->collide(node, this->goal_)) {
-                        node->children().pop_back();
-                        return false;
+                if (this->goal_found_) {
+                        if (node->ccost() + (*cost)(node, this->goal_) <
+                                        this->goal_->ccost()) {
+                                RRTNode *op; // old parent
+                                float oc; // old cumulative cost
+                                float od; // old direct cost
+                                op = this->goal_->parent();
+                                oc = this->goal_->ccost();
+                                od = this->goal_->dcost();
+                                node->add_child(this->goal_,
+                                                (*cost)(node, this->goal_));
+                                if (this->collide(node, this->goal_)) {
+                                        node->children().pop_back();
+                                        this->goal_->parent(op);
+                                        this->goal_->ccost(oc);
+                                        this->goal_->dcost(od);
+                                } else {
+                                        op->rem_child(this->goal_);
+                                        return true;
+                                }
+                        } else {
+                                return false;
+                        }
+                } else {
+                        node->add_child(
+                                        this->goal_,
+                                        (*cost)(node, this->goal_));
+                        if (this->collide(node, this->goal_)) {
+                                node->children().pop_back();
+                                return false;
+                        }
+                        this->goal_found_ = true;
+                        return true;
                 }
-                this->goal_found_ = true;
-                return true;
         }
         return false;
 }