From d6d42284969ca7c0b149a31f45d79101fe6ff133 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 30 Aug 2021 12:45:02 +0200 Subject: [PATCH] Add recompute path cc method --- incl/rrts.hh | 1 + src/rrts.cc | 16 ++++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/incl/rrts.hh b/incl/rrts.hh index a9ef93c..3bfd07e 100644 --- a/incl/rrts.hh +++ b/incl/rrts.hh @@ -71,6 +71,7 @@ protected: double eta_ = 0.5; double time_ = 0.0; double last_goal_cc_ = 0.0; + void recompute_path_cc(); double min_gamma_eta() const; bool should_continue() const; void join_steered(RRTNode* f); diff --git a/src/rrts.cc b/src/rrts.cc index 029997e..b0cfe22 100644 --- a/src/rrts.cc +++ b/src/rrts.cc @@ -59,6 +59,22 @@ RRTNode::operator==(RRTNode const& n) return this == &n; } +void +RRTS::recompute_path_cc() +{ + this->path_.clear(); + RRTNode* g = &this->goal_; + while (g != nullptr) { + this->path_.push_back(g); + g = g->p(); + } + std::reverse(this->path_.begin(), this->path_.end()); + for (unsigned int i = 1; i < this->path_.size(); i++) { + this->path_[i]->c(this->cost_build(*this->path_[i - 1], + *this->path_[i])); + } +} + double RRTS::min_gamma_eta() const { -- 2.39.2