Fix find trajectory return if no path

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 29b3cecf4d9439607b1a2c7c4503b1c2c9311c4c..779c19d520d45a00e7377341a48fe0d617bfad70 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -208,6 +208,8 @@ bool RRTBase::collide(RRTNode *init, RRTNode *goal)
 std::vector<RRTNode *> RRTBase::findt()
 {
         std::vector<RRTNode *> nodes;
+        if (!this->goal_->parent())
+                return nodes;
         RRTNode *tmp = this->goal_;
         while (tmp != this->root_) {
                 nodes.push_back(tmp);