Fix duplicated nodes when steered

[hubacji1/iamcar2.git] / rrts / src / rrtext2.cc

diff --git a/rrts/src/rrtext2.cc b/rrts/src/rrtext2.cc
index 087e863f309063078f35f23f10c9dc95bd8cbc02..5dcceae76a15c7a9a6c6fd540ca416d721eb4495 100644 (file)
--- 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()