Update anytime improvement with last path

When random sample's cost is greater than last path cc and therefore
there will be resampling, use random node from the last path instead.

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 3bfd07e83460c353a20cd8c2576ca9b096135ef3..9c847ed0b075addf807dd06ce9b571eae6a4935b 100644 (file)
--- 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;
+       std::vector<RRTNode> last_path_;
        void recompute_path_cc();
        double min_gamma_eta() const;
        bool should_continue() const;

diff --git a/src/rrts.cc b/src/rrts.cc
index b0cfe222efa5be7d32cba618945e14a7593fdb86..191bf7fbfd7f3449c2ea53297946419d4ced6402 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -301,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
@@ -356,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());