Do not clear opt time log

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

diff --git a/src/rrts.cc b/src/rrts.cc
index aa37552146b94abd2130473b4eaa5f8d5dbf2912..6ed3d69803aa247dc229c416995ba6a155b2acb0 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -44,8 +44,32 @@ double RRTS::elapsed()
 
 void RRTS::log_path_cost()
 {
-        this->log_path_cost_.push_back(cc(this->goals().front()));
-        this->log_path_iter_ += 20;
+        if (this->log_path_cost_.size() == 0) {
+                this->log_path_cost_.push_back(this->goals().front().cc);
+        } else {
+                auto lc = this->log_path_cost_.back();
+                auto gc = this->goals().front().cc;
+                auto goal_is_better = this->goals().front().cc > 0 && lc < gc;
+                if (
+                        this->log_path_cost_.back() > 0
+                        && (
+                                this->goals().front().cc == 0
+                                || (
+                                        this->goals().front().cc > 0
+                                        && goal_is_better
+                                )
+                        )
+                ) {
+                        this->log_path_cost_.push_back(
+                                this->log_path_cost_.back()
+                        );
+                } else {
+                        this->log_path_cost_.push_back(
+                                this->goals().front().cc
+                        );
+                }
+        }
+        this->log_path_iter_ += 1;
 }
 
 bool RRTS::should_stop()
@@ -66,7 +90,7 @@ bool RRTS::should_finish()
         if (this->icnt_ > 1000) return true;
         //if (this->scnt_ > 2) return true;
         if (this->finishit) return true;
-        //if (this->gf()) return true;
+        if (this->gf()) return true;
         // but continue by default
         return false;
 }
@@ -385,7 +409,7 @@ bool RRTS::goal_found(RRTNode &f)
         );
         double adist = std::abs(f.h() - g.h());
         if (edist < 0.05 && adist < M_PI / 32) {
-                if (g.p() == nullptr || cc(f) + cost < cc(g)) {
+                if (g.p() == nullptr || f.cc + cost < g.cc) {
                         g.p(&f);
                         g.c(cost);
                 }
@@ -399,14 +423,14 @@ bool RRTS::connect()
 {
         RRTNode *t = &this->steered().front();
         RRTNode *f = this->nn(this->samples().back());
-        double cost = cc(*f) + this->cost_build(*f, *t);
+        double cost = f->cc + this->cost_build(*f, *t);
         for (auto n: this->nv(*t)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*n, *t))
-                        && cc(*n) + this->cost_build(*n, *t) < cost
+                        && n->cc + this->cost_build(*n, *t) < cost
                 ) {
                         f = n;
-                        cost = cc(*n) + this->cost_build(*n, *t);
+                        cost = n->cc + this->cost_build(*n, *t);
                 }
         }
         // steer from f->t and then continue with the steered.
@@ -443,7 +467,7 @@ void RRTS::rewire()
         for (auto n: this->nv(*f)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*f, *n))
-                        && cc(*f) + this->cost_build(*f, *n) < cc(*n)
+                        && f->cc + this->cost_build(*f, *n) < n->cc
                 ) {
                         this->tmp_steer(*f, *n);
                         if (this->tmp_steered_.size() > 0) {
@@ -464,6 +488,28 @@ void RRTS::init()
 {
 }
 
+void RRTS::reset()
+{
+        RRTNode init = RRTNode();
+        init.x(this->nodes().front().x());
+        init.y(this->nodes().front().y());
+        init.h(this->nodes().front().h());
+        this->nodes().clear();
+        this->store_node(RRTNode());
+        this->nodes().front().x(init.x());
+        this->nodes().front().y(init.y());
+        this->nodes().front().h(init.h());
+        this->samples().clear();
+        this->steered().clear();
+        this->path().clear();
+        this->gf(false);
+        for (auto& g: this->goals()) {
+                g.p(nullptr);
+                g.c_ = 0.0;
+                g.cc = 0.0;
+        }
+}
+
 void RRTS::deinit()
 {
         this->nodes().clear();
@@ -476,20 +522,19 @@ void RRTS::deinit()
         this->gf_ = false;
 }
 
-std::vector<RRTNode *> RRTS::path()
+void RRTS::compute_path()
 {
-        std::vector<RRTNode *> path;
         if (this->goals().size() == 0)
-                return path;
-        RRTNode *goal = &this->goals().back();
+                return;
+        RRTNode *goal = &this->goals().front();
         if (goal->p() == nullptr)
-                return path;
+                return;
+        this->path_.clear();
         while (goal != nullptr) {
-                path.push_back(goal);
+                this->path_.push_back(goal);
                 goal = goal->p();
         }
-        std::reverse(path.begin(), path.end());
-        return path;
+        std::reverse(this->path_.begin(), this->path_.end());
 }
 
 bool RRTS::next()
@@ -497,10 +542,10 @@ bool RRTS::next()
         if (this->icnt_ == 0)
                 this->tstart_ = std::chrono::high_resolution_clock::now();
         bool next = true;
-        if (this->icnt_ > this->log_path_iter_)
-            this->log_path_cost();
-        if (this->should_stop())
+        if (this->should_stop()) {
+                this->log_path_cost();
                 return false;
+        }
         if (this->samples().size() == 0) {
                 this->samples().push_back(RRTNode());
                 this->samples().back().x(this->goals().front().x());
@@ -513,8 +558,10 @@ bool RRTS::next()
                 *this->nn(this->samples().back()),
                 this->samples().back()
         );
-        if (this->steered().size() == 0)
+        if (this->steered().size() == 0) {
+                this->log_path_cost();
                 return next;
+        }
         auto col = this->collide_steered_from(
                 *this->nn(this->samples().back())
         );
@@ -524,8 +571,10 @@ bool RRTS::next()
                         this->steered().pop_back();
                 }
         }
-        if (!this->connect())
+        if (!this->connect()) {
+                this->log_path_cost();
                 return next;
+        }
         this->rewire();
         unsigned scnt = this->steered().size();
         this->join_steered(&this->nodes().back());
@@ -550,9 +599,20 @@ bool RRTS::next()
                         this->steered2_.push_back(jap);
                         jap = jap->p();
                 }
-                this->gf(this->goal_found(this->nodes().back()));
+                auto gf = this->goal_found(this->nodes().back());
+                this->gf(gf);
                 just_added = just_added->p();
         }
+        if (
+                this->gf()
+                && (
+                        this->path().size() == 0
+                        || this->goals().front().cc < this->path().back()->cc
+                )
+        ) {
+                this->compute_path();
+        }
+        this->log_path_cost();
         return next;
 }
 
@@ -626,7 +686,7 @@ Json::Value RRTS::json()
         }
         {
                 if (this->path().size() > 0) {
-                        jvo["cost"] = cc(*this->path().back());
+                        jvo["cost"] = this->path().back()->cc;
                         jvo["entry"][0] = this->goals().front().x();
                         jvo["entry"][1] = this->goals().front().y();
                         jvo["entry"][2] = this->goals().front().h();