Add reset method, remove debug print

[hubacji1/rrts.git] / src / rrtext3.cc

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index 20d5b4465a8416c8817fa30298718d21fee97e54..f2d342ee476ee89e4ef68a6284918e6734046c06 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -1,14 +1,23 @@
 #include <queue>
 #include "rrtext.h"
 
-std::vector<RRTNode *> RRTExt3::first_path_optimization()
+void RRTExt3::reset()
+{
+        RRTS::reset();
+        this->orig_path().clear();
+        this->first_optimized_path().clear();
+        this->orig_path_cost_ = 9999.9;
+        this->first_optimized_path_cost_ = 9999.9;
+}
+
+void RRTExt3::first_path_optimization()
 {
         if (this->orig_path().size() == 0) {
                 this->orig_path_ = RRTS::path();
                 if (this->orig_path().size() == 0)
-                        return this->orig_path();
+                        return;
                 else
-                        this->orig_path_cost(cc(*this->orig_path().back()));
+                        this->orig_path_cost(this->orig_path().back()->cc);
         }
         class DijkstraNode : public RRTNode {
                 public:
@@ -24,7 +33,13 @@ std::vector<RRTNode *> RRTExt3::first_path_optimization()
                                 this->v = true;
                                 return false;
                         }
-                        using RRTNode::RRTNode;
+                        DijkstraNode(const RRTNode& n) {
+                            this->x(n.x());
+                            this->y(n.y());
+                            this->h(n.h());
+                            this->sp(n.sp());
+                            this->st(n.st());
+                        }
                         // override
                         DijkstraNode *p_ = nullptr;
                         DijkstraNode *p() const { return this->p_; }
@@ -49,14 +64,14 @@ std::vector<RRTNode *> RRTExt3::first_path_optimization()
                         || n->t(RRTNodeType::connected)
                 ) {
                         dn.push_back(DijkstraNode(*n));
-                        dn.back().cc = cc(*n);
+                        dn.back().cc = n->cc;
                         dn.back().s = &dn.back();
                         dn.back().n = n;
                         dn.back().i = dncnt++;
                 }
         }
         dn.push_back(DijkstraNode(*this->orig_path().back()));
-        dn.back().cc = cc(*this->orig_path().back());
+        dn.back().cc = this->orig_path().back()->cc;
         dn.back().s = &dn.back();
         dn.back().n = this->orig_path().back();
         dn.back().i = dncnt++;
@@ -91,25 +106,25 @@ std::vector<RRTNode *> RRTExt3::first_path_optimization()
                         ln = &n;
         }
         if (ln == nullptr || ln->p() == nullptr)
-                return this->orig_path();
+                return;
         while (ln->p() != nullptr) {
                 RRTNode *t = ln->n;
                 RRTNode *f = ln->p()->n;
                 this->steer(*f, *t);
                 if (std::get<0>(this->collide_steered_from(*f)))
-                        return this->orig_path();
+                        return;
                 this->join_steered(f);
                 t->p(&this->nodes().back());
                 t->c(this->cost_build(this->nodes().back(), *t));
                 ln = ln->p();
         }
-        return RRTS::path();
+        RRTS::compute_path();
 }
 
-std::vector<RRTNode *> RRTExt3::second_path_optimization()
+void RRTExt3::second_path_optimization()
 {
         if (this->first_optimized_path().size() == 0) {
-                return this->orig_path();
+                return;
         }
         class DijkstraNode : public RRTNode {
                 public:
@@ -125,7 +140,13 @@ std::vector<RRTNode *> RRTExt3::second_path_optimization()
                                 this->v = true;
                                 return false;
                         }
-                        using RRTNode::RRTNode;
+                        DijkstraNode(const RRTNode& n) {
+                            this->x(n.x());
+                            this->y(n.y());
+                            this->h(n.h());
+                            this->sp(n.sp());
+                            this->st(n.st());
+                        }
                         // override
                         DijkstraNode *p_ = nullptr;
                         DijkstraNode *p() const { return this->p_; }
@@ -150,14 +171,14 @@ std::vector<RRTNode *> RRTExt3::second_path_optimization()
                         || n->t(RRTNodeType::connected)
                 ) {
                         dn.push_back(DijkstraNode(*n));
-                        dn.back().cc = cc(*n);
+                        dn.back().cc = n->cc;
                         dn.back().s = &dn.back();
                         dn.back().n = n;
                         dn.back().i = dncnt++;
                 }
         }
         dn.push_back(DijkstraNode(*this->orig_path().back()));
-        dn.back().cc = cc(*this->orig_path().back());
+        dn.back().cc = this->orig_path().back()->cc;
         dn.back().s = &dn.back();
         dn.back().n = this->orig_path().back();
         dn.back().i = dncnt++;
@@ -195,32 +216,40 @@ std::vector<RRTNode *> RRTExt3::second_path_optimization()
                 }
         }
         if (fn == nullptr || fn->p() == nullptr)
-                return this->first_optimized_path();
+                return;
         DijkstraNode *ln = &dn.back();
         while (ln->p() != fn) {
                 RRTNode *t = ln->n;
                 RRTNode *f = ln->p()->n;
                 this->steer(*f, *t);
                 if (std::get<0>(this->collide_steered_from(*f)))
-                        return this->first_optimized_path();
+                        return;
                 this->join_steered(f);
                 t->p(&this->nodes().back());
                 t->c(this->cost_build(this->nodes().back(), *t));
                 ln = ln->p();
         }
-        return RRTS::path();
+        RRTS::compute_path();
 }
 
-std::vector<RRTNode *> RRTExt3::path()
+void RRTExt3::compute_path()
 {
-        this->first_optimized_path_ = this->first_path_optimization();
-        if (this->first_optimized_path_.size() > 0)
+        RRTS::compute_path();
+        auto tstart = std::chrono::high_resolution_clock::now();
+        this->first_path_optimization();
+        this->first_optimized_path_ = RRTS::path();
+        if (this->first_optimized_path_.size() > 0) {
                 this->first_optimized_path_cost(
-                        cc(*this->first_optimized_path_.back())
+                        this->first_optimized_path_.back()->cc
                 );
-        else
-                return this->orig_path();
-        return this->second_path_optimization();
+        } else {
+                return;
+        }
+        this->second_path_optimization();
+        auto tend = std::chrono::high_resolution_clock::now();
+        auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
+            tend - tstart);
+        this->log_opt_time_.push_back(tdiff.count());
 }
 
 Json::Value RRTExt3::json()
@@ -231,7 +260,7 @@ Json::Value RRTExt3::json()
                 unsigned int cu = 0;
                 unsigned int co = 0;
                 unsigned int pcnt = 0;
-                for (auto n: this->path()) {
+                for (auto n: this->orig_path()) {
                         jvo["orig_path"][pcnt][0] = n->x();
                         jvo["orig_path"][pcnt][1] = n->y();
                         jvo["orig_path"][pcnt][2] = n->h();