From 30dd49d02851253fecc5de3b85656d2a22ec21fc Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 16 Mar 2023 15:52:34 +0100
Subject: [PATCH] Fix duplicated nodes when steered

Require steer(f, t) method to set _steered member variable to include f
and t, i.e. f == _steered.front() and t == _steered.back().

This has consequences for collide_steered method as if there is only one
node left, it is already part of the tree and all the other (steered)
nodes collide. In such a case, drop the steered as colliding.
---
 rrts/incl/rrts.hh   | 6 ++++++
 rrts/src/rrtext2.cc | 4 +++-
 rrts/src/rrts.cc    | 8 ++++++++
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 2e85862..8c578b3 100644
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -115,6 +115,12 @@ protected:
 	virtual void find_nv(RRTNode const& t);
 	virtual void compute_path();
 protected:
+	/*! \brief Return nodes from f to t inclusive.
+	 *
+	 * The "inclusive" means that f is at the same pose (x, y, h) as
+	 * this->_steered.front() and t is at the same pose (x, y, h) as
+	 * this->_steered.back().
+	 */
 	virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
 	virtual bool collide_steered() = 0;
 	virtual RRTNode sample() = 0;
diff --git a/rrts/src/rrtext2.cc b/rrts/src/rrtext2.cc
index 087e863..5dcceae 100644
--- a/rrts/src/rrtext2.cc
+++ b/rrts/src/rrtext2.cc
@@ -37,7 +37,9 @@ RRTExt2::collide_steered()
 		i++;
 	}
 	this->_steered.erase(this->_steered.begin() + i, this->_steered.end());
-	return this->_steered.size() == 0;
+	// The first node of this->_steered is the same as nn. Therefore, if
+	// there is only one node left, it is nn, and other nodes collide.
+	return this->_steered.size() == 1;
 }
 
 RRTExt2::RRTExt2() : RRTS()
diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 7cd4684..ea302e9 100644
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -151,6 +151,14 @@ RRTS::connect()
 {
 	RRTNode* f = this->_nn;
 	RRTNode* t = &this->_steered.front();
+	// Require the steer method to return first node equal to nn:
+	assert(std::abs(t->x() - f->x()) < 1e-3);
+	assert(std::abs(t->y() - f->x()) < 1e-3);
+	assert(std::abs(t->h() - f->x()) < 1e-3);
+	// When f and t has different directions, the node (f == t) is cusp:
+	// TODO
+	this->_steered.erase(this->_steered.begin());
+	t = &this->_steered.front();
 #if USE_RRTS
 	double cost = f->cc() + this->cost_build(*f, *t);
 	for (auto n: this->nv_) {
-- 
2.39.2