Return false if trying to rebase on root/goal

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 198e7a83341184efa61ca6aae4817569d6776598..6607fdf7486c1bc56930782e42072803daea0ac5 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -332,6 +332,8 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
 
 bool RRTBase::rebase(RRTNode *nr)
 {
+        if (this->goal_ == nr || this->root_ == nr)
+                return false;
         std::vector<RRTNode *> s; // DFS stack
         RRTNode *tmp;
         unsigned int i = 0;