Update anytime improvement with last path

[hubacji1/rrts.git] / src / rrts.cc

diff --git a/src/rrts.cc b/src/rrts.cc
index f96cef47f8854b645fc889b68544b4aa60dfe672..191bf7fbfd7f3449c2ea53297946419d4ced6402 100644 (file)
--- 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
 {
@@ -285,8 +301,7 @@ RRTS::next()
        double d1 = this->cost_search(this->nodes_.front(), rs);
        double d2 = this->cost_search(rs, this->goal_);
        if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
-               this->icnt_ -= 1;
-               return this->should_continue();
+               rs = this->last_path_[rand() % this->last_path_.size()];
        }
 }
 #endif
@@ -328,16 +343,6 @@ RRTS::next()
                ss--;
                just_added = just_added->p();
        }
-#if 1 // test if root -> just_added can be steered directly
-       this->steer(this->nodes_.front(), *just_added);
-       ss = this->steered_.size();
-       if (!this->collide_steered() && this->steered_.size() == ss) {
-               this->join_steered(&this->nodes_.front());
-               just_added->p(this->nodes_.back());
-               just_added->c(this->cost_build(this->nodes_.back(),
-                       *just_added));
-       }
-#endif
        if (gf) {
                this->compute_path();
        }
@@ -350,6 +355,10 @@ RRTS::reset()
 {
        if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
                this->last_goal_cc_ = this->goal_.cc();
+               this->last_path_.clear();
+               for (auto n: this->path_) {
+                       this->last_path_.push_back(*n);
+               }
        }
        this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
                this->goal_.e());