Store computed path in class variable

This change has the consequences for the _optimize path_ extensions,
particularly `ext3` and `ext13`. These extensions are also updated.

diff --git a/api/rrtext.h b/api/rrtext.h
index f70d8a284c316a8c461500d2ae0a7b18db563d09..406afbb31cdbac6535d524ace860836bec80a025 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -24,9 +24,9 @@ class RRTExt13 : public virtual RRTS {
                 double orig_path_cost_ = 9999;
                 std::vector<RRTNode *> first_optimized_path_;
                 double first_optimized_path_cost_ = 9999;
-                std::vector<RRTNode *> first_path_optimization();
-                std::vector<RRTNode *> second_path_optimization();
-                std::vector<RRTNode *> path();
+                void first_path_optimization();
+                void second_path_optimization();
+                void compute_path();
                 Json::Value json();
                 void json(Json::Value jvi);
 
@@ -291,9 +291,9 @@ class RRTExt3 : public virtual RRTS {
                 double orig_path_cost_ = 9999;
                 std::vector<RRTNode *> first_optimized_path_;
                 double first_optimized_path_cost_ = 9999;
-                std::vector<RRTNode *> first_path_optimization();
-                std::vector<RRTNode *> second_path_optimization();
-                std::vector<RRTNode *> path();
+                void first_path_optimization();
+                void second_path_optimization();
+                void compute_path();
                 Json::Value json();
                 void json(Json::Value jvi);
 

diff --git a/api/rrts.h b/api/rrts.h
index f5b7de580eb2ecf6da17bd34efe1b4cb9e750fd1..8fa3fb773ebf9c61547e15eea5fdb0186a6bd402 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -147,6 +147,7 @@ class RRTS {
                 std::vector<Obstacle> obstacles_;
                 std::vector<RRTNode> samples_;
                 std::vector<RRTNode> steered_;
+                std::vector<RRTNode *> path_;
                 double log_path_time_ = 0.1;
                 unsigned int log_path_iter_ = 20;
 
@@ -238,7 +239,11 @@ class RRTS {
                 virtual void deinit();
                 /*! \brief Return path found by RRT*.
                 */
-                virtual std::vector<RRTNode *> path();
+                virtual std::vector<RRTNode *>& path()
+                {
+                        return this->path_;
+                }
+                virtual void compute_path();
                 /*! \brief Return ``true`` if algorithm should stop.
 
                 Update counters (iteration, seconds, ...) and return if

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 6a0f633d3a62c41f26f18895d8ec84029f9d5ad4..d0db3d119d6bd5bac81354d256899e032ba549dd 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,12 +1,12 @@
 #include <queue>
 #include "rrtext.h"
 #include <iostream>
-std::vector<RRTNode *> RRTExt13::first_path_optimization()
+void RRTExt13::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(this->orig_path().back()->cc);
         }
@@ -117,25 +117,25 @@ std::vector<RRTNode *> RRTExt13::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 *> RRTExt13::second_path_optimization()
+void RRTExt13::second_path_optimization()
 {
         if (this->first_optimized_path().size() == 0) {
-                return this->orig_path();
+                return;
         }
         class DijkstraNode : public RRTNode {
                 public:
@@ -247,38 +247,40 @@ std::vector<RRTNode *> RRTExt13::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 *> RRTExt13::opt_path()
+void RRTExt13::compute_path()
 {
+        RRTS::compute_path();
         auto tstart = std::chrono::high_resolution_clock::now();
-        this->first_optimized_path_ = this->first_path_optimization();
-        if (this->first_optimized_path_.size() > 0)
+        this->first_path_optimization();
+        this->first_optimized_path_ = RRTS::path();
+        if (this->first_optimized_path_.size() > 0) {
                 this->first_optimized_path_cost(
                         this->first_optimized_path_.back()->cc
                 );
-        else
-                return this->orig_path();
-        auto opt2 = 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());
-        return opt2;
 }
 
 Json::Value RRTExt13::json()

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index aff866c0325d093c41b74e2023285a85aedb2861..8077b7e8f390ff6aa05044467383f24de2fdd76a 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -1,12 +1,12 @@
 #include <queue>
 #include "rrtext.h"
 
-std::vector<RRTNode *> RRTExt3::first_path_optimization()
+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(this->orig_path().back()->cc);
         }
@@ -97,25 +97,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:
@@ -207,38 +207,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()
 {
+        RRTS::compute_path();
         auto tstart = std::chrono::high_resolution_clock::now();
-        this->first_optimized_path_ = this->first_path_optimization();
-        if (this->first_optimized_path_.size() > 0)
+        this->first_path_optimization();
+        this->first_optimized_path_ = RRTS::path();
+        if (this->first_optimized_path_.size() > 0) {
                 this->first_optimized_path_cost(
                         this->first_optimized_path_.back()->cc
                 );
-        else
-                return this->orig_path();
-        auto opt2 = 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());
-        return opt2;
 }
 
 Json::Value RRTExt3::json()

diff --git a/src/rrts.cc b/src/rrts.cc
index 44d0d72d4c73c44c94008c8a2bd9b4c48da24ba9..a309bc3065daa0709d4fca22a25ade43fefcd3ed 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -500,20 +500,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()
@@ -578,9 +577,19 @@ 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;
 }