Add log tree structure/methods

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d1597ded713f3d8785f26c34f05ef64e52ecfebc..084218f2e51e4a235aeaae76d0505ddc38ac9929 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Near vertices `nv2` procedure based on indexing over `y` axis.
 - OpenMP parallelization of nearest neighbour and near vertices procedures.
 - Rebase method that changes (rebases) RRT root to another RRT node.
+- Structure and method for logging tree edges.
 
 ### Changed
 - Build with Ninja.

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index e04b039016b476d0224b9413906744e8d8402302..67eb35346d1dc94a80768ead5ccbebf9356c46db 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -31,6 +31,9 @@ RRTBase::~RRTBase()
         for (auto s: this->samples_)
                 if (s != this->goal_)
                         delete s;
+        for (auto edges: this->rlog_)
+                for (auto e: edges)
+                        delete e;
         delete this->root_;
         delete this->goal_;
 }
@@ -96,6 +99,11 @@ std::vector<float> &RRTBase::nlog()
         return this->nlog_;
 }
 
+std::vector<std::vector<RRTEdge *>> &RRTBase::rlog()
+{
+        return this->rlog_;
+}
+
 std::vector<float> &RRTBase::slog()
 {
         return this->slog_;
@@ -119,6 +127,28 @@ float RRTBase::elapsed()
         return dt.count();
 }
 
+bool RRTBase::logr(RRTNode *root)
+{
+        std::vector<RRTEdge *> e; // Edges to log
+        std::vector<RRTNode *> s; // DFS stack
+        std::vector<RRTNode *> r; // reset visited_
+        RRTNode *tmp;
+        s.push_back(root);
+        while (s.size() > 0) {
+                tmp = s.back();
+                s.pop_back();
+                if (!tmp->visit()) {
+                        r.push_back(tmp);
+                        for (auto ch: tmp->children()) {
+                                s.push_back(ch);
+                                e.push_back(new RRTEdge(tmp, ch));
+                        }
+                }
+        }
+        this->rlog_.push_back(e);
+        return true;
+}
+
 bool RRTBase::tlog(std::vector<RRTNode *> t)
 {
         this->slog_.push_back(this->elapsed());

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 73475177e47e01ab1fbfe73eb1018cd1168cfae5..2d692850ef84a31341707a00d7e3da7264bbda44 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -44,6 +44,7 @@ class RRTBase {
 
                 std::vector<float> clog_; // costs of trajectories
                 std::vector<float> nlog_; // #nodes of RRT
+                std::vector<std::vector<RRTEdge *>> rlog_; // log tree starting at root
                 std::vector<float> slog_; // seconds of trajectories
                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
         public:
@@ -65,12 +66,14 @@ class RRTBase {
                 std::vector<SegmentObstacle> *sos();
                 std::vector<float> &clog();
                 std::vector<float> &nlog();
+                std::vector<std::vector<RRTEdge *>> &rlog();
                 std::vector<float> &slog();
                 std::vector<std::vector<RRTNode *>> &tlog();
                 bool goal_found();
                 float elapsed();
 
                 // setter
+                bool logr(RRTNode *root);
                 bool tlog(std::vector<RRTNode *> t);
                 void tstart();
                 void tend();