Log trajectory only if exists

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index a51b4536d0503ef876efed0b7a923e0770f09bdd..078d4c6a9238e0a7c09d05e453260d59f393b0e9 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -165,11 +165,15 @@ float RRTBase::ocost(RRTNode *n)
 
 bool RRTBase::tlog(std::vector<RRTNode *> t)
 {
-        this->slog_.push_back(this->elapsed());
-        this->clog_.push_back(this->goal_->ccost());
-        this->nlog_.push_back(this->nodes_.size());
-        this->tlog_.push_back(t);
-        return true;
+        if (t.size() > 0) {
+                this->slog_.push_back(this->elapsed());
+                this->clog_.push_back(t.front()->ccost() - t.back()->ccost());
+                this->nlog_.push_back(this->nodes_.size());
+                this->tlog_.push_back(t);
+                return true;
+        } else {
+                return false;
+        }
 }
 
 void RRTBase::tstart()