From f7cbd9f9a410af77409c9138db4bb1d32ffe132c Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Sun, 19 Mar 2023 18:07:42 +0100
Subject: [PATCH] Fix steer toward goal in rrts next method

---
 rrts/src/rrts.cc | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

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());
-- 
2.39.2