From 83e7c95b1a75636e2b2084829d4c8aa97c6ce510 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 12 Jul 2021 11:25:30 +0200 Subject: [PATCH] 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. --- api/rrtext.h | 12 ++++++------ api/rrts.h | 7 ++++++- src/rrtext13.cc | 36 +++++++++++++++++++----------------- src/rrtext3.cc | 36 +++++++++++++++++++----------------- src/rrts.cc | 27 ++++++++++++++++++--------- 5 files changed, 68 insertions(+), 50 deletions(-) diff --git a/api/rrtext.h b/api/rrtext.h index f70d8a2..406afbb 100644 --- a/api/rrtext.h +++ b/api/rrtext.h @@ -24,9 +24,9 @@ class RRTExt13 : public virtual RRTS { double orig_path_cost_ = 9999; std::vector first_optimized_path_; double first_optimized_path_cost_ = 9999; - std::vector first_path_optimization(); - std::vector second_path_optimization(); - std::vector 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 first_optimized_path_; double first_optimized_path_cost_ = 9999; - std::vector first_path_optimization(); - std::vector second_path_optimization(); - std::vector 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 f5b7de5..8fa3fb7 100644 --- a/api/rrts.h +++ b/api/rrts.h @@ -147,6 +147,7 @@ class RRTS { std::vector obstacles_; std::vector samples_; std::vector steered_; + std::vector 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 path(); + virtual std::vector& 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 6a0f633..d0db3d1 100644 --- a/src/rrtext13.cc +++ b/src/rrtext13.cc @@ -1,12 +1,12 @@ #include #include "rrtext.h" #include -std::vector 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 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 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 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 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>( 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 aff866c..8077b7e 100644 --- a/src/rrtext3.cc +++ b/src/rrtext3.cc @@ -1,12 +1,12 @@ #include #include "rrtext.h" -std::vector 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 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 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 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 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>( 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 44d0d72..a309bc3 100644 --- a/src/rrts.cc +++ b/src/rrts.cc @@ -500,20 +500,19 @@ void RRTS::deinit() this->gf_ = false; } -std::vector RRTS::path() +void RRTS::compute_path() { - std::vector 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; } -- 2.39.2