From 41c0c863c423bd98dccee6775e4939c2b226020e Mon Sep 17 00:00:00 2001 From: Jiri Hubacek Date: Wed, 4 Jul 2018 10:45:41 +0200 Subject: [PATCH] Fix enhance path when goal already found --- base/rrtbase.cc | 41 +++++++++++++++++++++++++++++++++-------- 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/base/rrtbase.cc b/base/rrtbase.cc index fef0440..29b3cec 100644 --- 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; } -- 2.39.2