From e81631dc40e60b224fdc2bc7ab743d6f5a6d8bea Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 19 Jul 2021 11:45:47 +0200 Subject: [PATCH] Rewrite ext13 --- CMakeLists.txt | 1 + incl/rrtext.hh | 54 +++---- src/rrtext13.cc | 423 +++++++++++++++++------------------------------- 3 files changed, 178 insertions(+), 300 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 30f9759..f981dfe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ link_libraries(jsoncpp_lib) add_library(rrts STATIC src/rrts.cc src/rrtext14.cc + src/rrtext13.cc src/rrtext10.cc src/rrtext8.cc src/rrtext2.cc diff --git a/incl/rrtext.hh b/incl/rrtext.hh index 299f620..3469c08 100644 --- a/incl/rrtext.hh +++ b/incl/rrtext.hh @@ -37,36 +37,32 @@ public: /*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */ class RRTExt13 : public virtual RRTS { - private: +private: + class DijkstraNode { public: - void reset(); - std::vector orig_path_; - double orig_path_cost_ = 9999; - std::vector first_optimized_path_; - double first_optimized_path_cost_ = 9999; - void first_path_optimization(); - void second_path_optimization(); - void compute_path(); - Json::Value json(); - void json(Json::Value jvi); - - // getter, setter - std::vector &orig_path() - { - return this->orig_path_; - }; - double &orig_path_cost() { return this->orig_path_cost_; } - void orig_path_cost(double c) { this->orig_path_cost_ = c; } - std::vector &first_optimized_path() - { - return this->first_optimized_path_; - }; - double &first_optimized_path_cost() { - return this->first_optimized_path_cost_; - } - void first_optimized_path_cost(double c) { - this->first_optimized_path_cost_ = c; - } + RRTNode* node = nullptr; + unsigned int i = 0; + bool v = false; + bool vi(); + DijkstraNode(RRTNode* n); + }; + class DijkstraNodeComparator { + public: + int operator() (DijkstraNode const& n1, DijkstraNode const& n2); + }; + std::vector opath_; + double ogoal_cc_ = 0.0; + double otime_ = 0.0; + std::vector dn_; + void pick_interesting(); + void dijkstra_forward(); + void dijkstra_backward(); + void compute_path(); +public: + RRTExt13(); + Json::Value json() const; + void json(Json::Value jvi); + void reset(); }; /*! \brief Different `steer` procedures. diff --git a/src/rrtext13.cc b/src/rrtext13.cc index 172ef6a..03ff714 100644 --- a/src/rrtext13.cc +++ b/src/rrtext13.cc @@ -1,314 +1,195 @@ #include -#include "rrtext.h" +#include "rrtext.hh" +#include +using namespace std; -void RRTExt13::reset() +namespace rrts { + +RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n) { - 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 RRTExt13::first_path_optimization() +bool +RRTExt13::DijkstraNode::vi() { - if (this->orig_path().size() == 0) { - this->orig_path_ = RRTS::path(); - if (this->orig_path().size() == 0) - return; - else - this->orig_path_cost(this->orig_path().back()->cc); + if (this->v) { + return true; } - class DijkstraNode : public RRTNode { - public: - DijkstraNode *s = nullptr; - RRTNode *n = nullptr; - unsigned int i = 0; - double cc = 9999; - bool v = false; - bool vi() - { - if (this->v) - return true; - this->v = true; - return false; - } - 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_; } - void p(DijkstraNode *p) { this->p_ = p; } - }; - class DijkstraNodeComparator { - public: - int operator() ( - const DijkstraNode &n1, - const DijkstraNode &n2 - ) - { - return n1.cc > n2.cc; + this->v = true; + return false; +} + +int +RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1, + DijkstraNode const& n2) +{ + return n1.node->cc() > n2.node->cc(); +} + +void +RRTExt13::pick_interesting() +{ + this->dn_.clear(); + this->dn_.reserve(this->path_.size()); + //for (auto n: this->opath_) { + // this->dn_.push_back(DijkstraNode(n)); + // this->dn_.back().i = this->dn_.size() - 1; + //} + //return; + for (unsigned int i = 0; i < this->opath_.size() - 1; i++) { + //unsigned int i = 0; { + RRTNode& ni = *this->opath_[i]; + this->bc_.set_pose(ni); + for (unsigned int j = i + 1; j < this->opath_.size(); j++) { + RRTNode& nj = *this->opath_[j]; + RRTNode& njl = *this->opath_[j - 1]; + if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp()) + || njl.edist(nj) < this->eta_) { + this->dn_.push_back(DijkstraNode(&njl)); + this->dn_.back().i = this->dn_.size() - 1; } - }; - std::vector dn; - unsigned int ops = this->orig_path().size(); - auto op = this->orig_path(); - dn.reserve(ops); - unsigned int dncnt = 0; - dn.push_back(DijkstraNode(*this->orig_path().front())); - dn.back().cc = this->orig_path().front()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().front(); - dn.back().i = dncnt++; - for (unsigned int i = 0; i < ops - 1; i++) { - auto ibc = BicycleCar(); - ibc.x(op[i]->x()); - ibc.y(op[i]->y()); - ibc.h(op[i]->h()); - for (unsigned int j = i + 1; j < ops; j++) { - auto jbc = BicycleCar(); - jbc.x(op[j]->x()); - jbc.y(op[j]->y()); - jbc.h(op[j]->h()); - if (!jbc.drivable(ibc)) { - dn.push_back(DijkstraNode(*op[j-1])); - dn.back().cc = op[j-1]->cc; - dn.back().s = &dn.back(); - dn.back().n = op[j-1]; - dn.back().i = dncnt++; + if (!this->bc_.drivable(nj)) { + this->dn_.push_back(DijkstraNode(&njl)); + this->dn_.back().i = this->dn_.size() - 1; i = j; break; } } } - dn.push_back(DijkstraNode(*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++; - std::priority_queue< - DijkstraNode, - std::vector, - DijkstraNodeComparator - > pq; - dn.front().vi(); - pq.push(dn.front()); + this->dn_.push_back(DijkstraNode(this->opath_.back())); + this->dn_.back().i = this->dn_.size() - 1; +} + +void +RRTExt13::dijkstra_forward() +{ + std::priority_queue, + DijkstraNodeComparator> pq; + this->dn_.front().vi(); + pq.push(this->dn_.front()); while (!pq.empty()) { - DijkstraNode f = pq.top(); + DijkstraNode fd = pq.top(); + RRTNode& f = *fd.node; pq.pop(); - for (unsigned int i = f.i + 1; i < dncnt; i++) { - double cost = f.cc + this->cost_search(f, dn[i]); - this->steer(f, dn[i]); - if (this->steered().size() == 0) - break; // TODO why not continue? - if (std::get<0>(this->collide_steered_from(f))) + for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) { + RRTNode& t = *this->dn_[i].node; + double cost = f.cc() + this->cost_build(f, t); + this->steer(f, t); + if (this->steered_.size() == 0) { + break; + } + if (this->collide_steered()) { continue; - if (cost < dn[i].cc) { - dn[i].cc = cost; - dn[i].p(f.s); - if (!dn[i].vi()) - pq.push(dn[i]); + } + this->bc_.set_pose(this->steered_.back()); + bool td = this->bc_.drivable(t); + bool tn = this->bc_.edist(t) < this->eta_; + if (cost < t.cc() && td && tn) { + this->join_steered(&f); + t.p(this->nodes_.back()); + t.c(this->cost_build(this->nodes_.back(), t)); + if (!this->dn_[i].vi()) { + pq.push(this->dn_[i]); + } } } } - DijkstraNode *ln = nullptr; - for (auto &n: dn) { - if (n.v) - ln = &n; - } - if (ln == nullptr || ln->p() == nullptr) - 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->join_steered(f); - t->p(&this->nodes().back()); - t->c(this->cost_build(this->nodes().back(), *t)); - ln = ln->p(); - } - RRTS::compute_path(); } -void RRTExt13::second_path_optimization() +void +RRTExt13::dijkstra_backward() { - if (this->first_optimized_path().size() == 0) { - return; - } - class DijkstraNode : public RRTNode { - public: - DijkstraNode *s = nullptr; - RRTNode *n = nullptr; - unsigned int i = 0; - double cc = 9999; - bool v = false; - bool vi() - { - if (this->v) - return true; - this->v = true; - return false; - } - 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_; } - void p(DijkstraNode *p) { this->p_ = p; } - }; - class DijkstraNodeComparator { - public: - int operator() ( - const DijkstraNode &n1, - const DijkstraNode &n2 - ) - { - return n1.cc > n2.cc; - } - }; - std::vector dn; - unsigned int ops = this->orig_path().size(); - auto op = this->orig_path(); - dn.reserve(ops); - unsigned int dncnt = 0; - dn.push_back(DijkstraNode(*this->orig_path().front())); - dn.back().cc = this->orig_path().front()->cc; - dn.back().s = &dn.back(); - dn.back().n = this->orig_path().front(); - dn.back().i = dncnt++; - for (unsigned int i = ops - 1; i > 1; i--) { - auto ibc = BicycleCar(); - ibc.x(op[i]->x()); - ibc.y(op[i]->y()); - ibc.h(op[i]->h()); - for (unsigned int j = i - 1; j > 0; j--) { - auto jbc = BicycleCar(); - jbc.x(op[j]->x()); - jbc.y(op[j]->y()); - jbc.h(op[j]->h()); - if (!jbc.drivable(ibc)) { - dn.push_back(DijkstraNode(*op[j-1])); - dn.back().cc = op[j-1]->cc; - dn.back().s = &dn.back(); - dn.back().n = op[j-1]; - dn.back().i = dncnt++; - i = j; - break; - } - } - } - dn.push_back(DijkstraNode(*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++; - std::priority_queue< - DijkstraNode, - std::vector, - DijkstraNodeComparator - > pq; - // TODO rewrite - dn.back().vi(); - pq.push(dn.back()); + std::priority_queue, + DijkstraNodeComparator> pq; + this->dn_.back().vi(); + pq.push(this->dn_.back()); while (!pq.empty()) { - DijkstraNode t = pq.top(); + DijkstraNode td = pq.top(); + RRTNode& t = *td.node; pq.pop(); - for (unsigned int i = t.i - 1; i > 0; i--) { - double cost = dn[i].cc + this->cost_search(dn[i], t); - this->steer(dn[i], t); - if (this->steered().size() == 0) - break; // TODO why not continue? - if (std::get<0>(this->collide_steered_from(dn[i]))) + for (unsigned int i = td.i - 1; i > 0; i--) { + RRTNode& f = *this->dn_[i].node; + double cost = f.cc() + this->cost_build(f, t); + this->steer(f, t); + if (this->steered_.size() == 0) { + break; + } + if (this->collide_steered()) { continue; - if (cost < t.cc) { - t.cc = cost; - t.p(dn[i].s); - if (!dn[i].vi()) - pq.push(dn[i]); + } + this->bc_.set_pose(this->steered_.back()); + bool td = this->bc_.drivable(t); + bool tn = this->bc_.edist(t) < this->eta_; + if (cost < t.cc() && td && tn) { + this->join_steered(&f); + t.p(this->nodes_.back()); + t.c(this->cost_build(this->nodes_.back(), t)); + if (!this->dn_[i].vi()) { + pq.push(this->dn_[i]); + } } } } - DijkstraNode *fn = nullptr; - for (auto &n: dn) { - if (n.v) { - fn = &n; - break; - } +} + +void +RRTExt13::compute_path() +{ + RRTS::compute_path(); + bool measure_time = false; + if (this->opath_.size() == 0) { + this->opath_ = this->path_; + this->ogoal_cc_ = this->goal_.cc(); + measure_time = true; } - if (fn == nullptr || fn->p() == nullptr) - 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->join_steered(f); - t->p(&this->nodes().back()); - t->c(this->cost_build(this->nodes().back(), *t)); - ln = ln->p(); + if (measure_time) { + this->otime_ = -this->ter_.scnt(); } + this->pick_interesting(); + this->dijkstra_forward(); + this->pick_interesting(); + this->dijkstra_backward(); RRTS::compute_path(); + if (measure_time) { + this->otime_ += this->ter_.scnt(); + } } -void RRTExt13::compute_path() +RRTExt13::RRTExt13() { - 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( - this->first_optimized_path_.back()->cc - ); - } 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()); + this->opath_.reserve(10000); + this->dn_.reserve(10000); } -Json::Value RRTExt13::json() +Json::Value +RRTExt13::json() const { - Json::Value jvo = RRTS::json(); - jvo["orig_path_cost"] = this->orig_path_cost(); - { - unsigned int cu = 0; - unsigned int co = 0; - unsigned int pcnt = 0; - 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(); - if (n->t(RRTNodeType::cusp)) - cu++; - if (n->t(RRTNodeType::connected)) - co++; - pcnt++; - } - jvo["orig_cusps-in-path"] = cu; - jvo["orig_connecteds-in-path"] = co; + auto jvo = RRTS::json(); + unsigned int i = 0; + for (auto n: this->opath_) { + jvo["opath"][i][0] = n->x(); + jvo["opath"][i][1] = n->y(); + jvo["opath"][i][2] = n->h(); + i++; } + jvo["ogoal_cc"] = this->ogoal_cc_; + jvo["otime"] = this->otime_; return jvo; } -void RRTExt13::json(Json::Value jvi) +void +RRTExt13::json(Json::Value jvi) { - return RRTS::json(jvi); + RRTS::json(jvi); } + +void +RRTExt13::reset() +{ + RRTS::reset(); + this->opath_.clear(); + this->ogoal_cc_ = 0.0; + this->otime_ = 0.0; +} + +} // namespace rrts -- 2.39.2