From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Mon, 30 Aug 2021 10:47:42 +0000 (+0200)
Subject: Update anytime improvement with last path
X-Git-Tag: v0.9.0~5^2~6
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/95313950a4662d698f64d9af9d122d965f208233

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 3bfd07e..9c847ed 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;
+	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 b0cfe22..191bf7f 100644
--- 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());