From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Sun, 19 Mar 2023 17:07:42 +0000 (+0100)
Subject: Fix steer toward goal in rrts next method
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/commitdiff_plain/f7cbd9f9a410af77409c9138db4bb1d32ffe132c?ds=sidebyside

Fix steer toward goal in rrts next method
---

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 6bcf151..85bd8ed 100644
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -534,13 +534,15 @@ RRTS::next()
 	this->join_steered(&this->_nodes.back());
 	RRTNode* just_added = &this->_nodes.back();
 	bool gf = false;
-	while (ss > 0 && just_added->p() != nullptr) {
+	while (ss > 0 && just_added != nullptr) {
 		this->steer(*just_added, this->_goal);
 		if (this->collide_steered()) {
 			ss--;
 			just_added = just_added->p();
 			continue;
 		}
+		// The first of steered is the same as just_added.
+		this->_steered.erase(this->_steered.begin());
 		this->join_steered(just_added);
 		bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
 		bool gd = this->goal_drivable_from(this->_nodes.back());