Add number of nodes log

diff --git a/base/main.cc b/base/main.cc
index 06208af56f5f8c105f1518f15147b371f051af7a..554edae0b4df8eb48320cb3c1f66d591a836d09c 100644 (file)
--- a/base/main.cc
+++ b/base/main.cc
@@ -83,6 +83,9 @@ int main()
         // log cost
         for (j = 0; j < p.clog().size(); j++)
                 jvo["cost"][j] = p.clog()[j];
+        // log #nodes
+        for (j = 0; j < p.nlog().size(); j++)
+                jvo["node"][j] = p.nlog()[j];
         // log seconds
         for (j = 0; j < p.slog().size(); j++)
                 jvo["secs"][j] = p.slog()[j];

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 490508fa9c1b0e33685bd0b04f58ddfb770efd05..ffba2a3e585e0527a3af0453c00e946762f2e37f 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -64,6 +64,11 @@ std::vector<float> &RRTBase::clog()
         return this->clog_;
 }
 
+std::vector<float> &RRTBase::nlog()
+{
+        return this->nlog_;
+}
+
 std::vector<float> &RRTBase::slog()
 {
         return this->slog_;
@@ -94,8 +99,9 @@ bool RRTBase::tlog(std::vector<RRTNode *> t)
                         duration<float>>(
                                 std::chrono::high_resolution_clock::now() -
                                 this->tstart_).count());
-        this->tlog_.push_back(t);
         this->clog_.push_back(this->goal_->ccost());
+        this->nlog_.push_back(this->nodes_.size());
+        this->tlog_.push_back(t);
         return true;
 }
 

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 50836a67fcc1d4dee6f41ac800167d3c96e07ca6..045e3761a6186cbe759588544f51f9f39424d3a8 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -39,6 +39,7 @@ class RRTBase {
                 std::chrono::high_resolution_clock::time_point tend_;
 
                 std::vector<float> clog_; // costs of trajectories
+                std::vector<float> nlog_; // #nodes of RRT
                 std::vector<float> slog_; // seconds of trajectories
                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
         public:
@@ -56,6 +57,7 @@ class RRTBase {
                 std::vector<CircleObstacle> *cos();
                 std::vector<SegmentObstacle> *sos();
                 std::vector<float> &clog();
+                std::vector<float> &nlog();
                 std::vector<float> &slog();
                 std::vector<std::vector<RRTNode *>> &tlog();
                 bool goal_found();