From 43f73f977cfcf8dcf8cf68debb9f1924c2189a4d Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Wed, 15 Mar 2023 11:30:24 +0100 Subject: [PATCH] Remove unused extensions --- rrts/CMakeLists.txt | 2 - rrts/incl/rrtext.hh | 236 ----------------------------------- rrts/src/rrtext1.cc | 29 ----- rrts/src/rrtext11.cc | 42 ------- rrts/src/rrtext12.cc | 139 --------------------- rrts/src/rrtext20.cc | 55 --------- rrts/src/rrtext3.cc | 288 ------------------------------------------- rrts/src/rrtext4.cc | 118 ------------------ rrts/src/rrtext5.cc | 23 ---- rrts/src/rrtext6.cc | 27 ---- rrts/src/rrtext7.cc | 97 --------------- rrts/src/rrtext9.cc | 138 --------------------- 12 files changed, 1194 deletions(-) delete mode 100644 rrts/src/rrtext1.cc delete mode 100644 rrts/src/rrtext11.cc delete mode 100644 rrts/src/rrtext12.cc delete mode 100644 rrts/src/rrtext20.cc delete mode 100644 rrts/src/rrtext3.cc delete mode 100644 rrts/src/rrtext4.cc delete mode 100644 rrts/src/rrtext5.cc delete mode 100644 rrts/src/rrtext6.cc delete mode 100644 rrts/src/rrtext7.cc delete mode 100644 rrts/src/rrtext9.cc diff --git a/rrts/CMakeLists.txt b/rrts/CMakeLists.txt index e767f08..363fe13 100644 --- a/rrts/CMakeLists.txt +++ b/rrts/CMakeLists.txt @@ -31,7 +31,6 @@ link_libraries(jsoncpp_lib) add_library(rrts STATIC src/rrts.cc src/rrtext21.cc - src/rrtext20.cc src/rrtext19.cc src/rrtext18.cc src/rrtext17.cc @@ -41,7 +40,6 @@ add_library(rrts STATIC src/rrtext13.cc src/rrtext10.cc src/rrtext8.cc - src/rrtext6.cc src/rrtext2.cc src/reeds_shepp.cpp src/dubins.c diff --git a/rrts/incl/rrtext.hh b/rrts/incl/rrtext.hh index b20d1af..8276477 100644 --- a/rrts/incl/rrtext.hh +++ b/rrts/incl/rrtext.hh @@ -57,22 +57,6 @@ public: int8_t const *d, double x, double y); }; -/*! \brief Collision check based on occupancy grid. - * - * This approach expects obstacles to be represented by points and the collision - * occures whenever the point is inside the frame given by the car pose and the - * car size. - * - * \ingroup ext-col - */ -class RRTExt20 : public virtual RRTS { -private: - std::vector const *_points_to_check = nullptr; - bool collide_steered(); -public: - void set_points_to_check(std::vector const *p); -}; - /*! \brief Use Dubins paths-based steering procedure. * * \ingroup ext-steer @@ -183,26 +167,6 @@ public: void reset(); }; -/* \brief Different `steer` procedures. - -Use sampling in control input for `steer1`. Use R&S steering for `steer2`. -*/ -class RRTExt12 : public virtual RRTS { - protected: - void steer1(RRTNode &f, RRTNode &t); - bool connect(); - public: - bool next(); -}; - -/* \brief Goal Zone. - -*/ -class RRTExt11 : public virtual RRTS { - protected: - bool goal_found(RRTNode &f); -}; - /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search). * * Use Reeds & Shepp path length for building tree data structure and Euclidean @@ -218,52 +182,6 @@ protected: double cost_search(RRTNode const& f, RRTNode const& t) const; }; -/* \brief Use grid data structure to store RRT nodes. - -This approach speeds up the search process for the nearest neighbor and -the near vertices procedures. -*/ -class RRTExt9 : public virtual RRTS { - private: - class Cell { - private: - bool changed_ = false; - std::vector nodes_; - public: - void nn(RRTNode *t, RRTNode **nn, RRTS *p); - void store_node(RRTNode *n); - - // getter, setter - bool changed() const - { - return this->changed_; - } - std::vector &nodes() - { - return this->nodes_; - } - - Cell(); - }; - Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI]; - double x_min_ = 0; - double x_max_ = 0; - double y_min_ = 0; - double y_max_ = 0; - double h_min_ = 0; - double h_max_ = 2 * M_PI; - - unsigned int xi(RRTNode n); - unsigned int yi(RRTNode n); - unsigned int hi(RRTNode n); - public: - void init(); - void deinit(); - void store_node(RRTNode n); - RRTNode *nn(RRTNode &t); - std::vector nv(RRTNode &t); -}; - /*! \brief Use 3D k-d tree data structure to store RRT nodes. * * This approach speeds up the search process for the nearest neighbor and the @@ -294,145 +212,6 @@ public: void find_nv(RRTNode const& t); }; -/* \brief Use k-d tree data structure to store RRT nodes. - -This approach speeds up the search process for the nearest neighbor and -the near vertices procedures. This extension implements 2D K-d tree. - -\see https://en.wikipedia.org/wiki/K-d_tree -*/ -class RRTExt7 : public virtual RRTS { - private: - class KdNode { - private: - RRTNode *node_ = nullptr; - KdNode *left_ = nullptr; - KdNode *right_ = nullptr; - public: - // getter, setter - RRTNode *node() const { return this->node_; } - KdNode *&left() { return this->left_; } - KdNode *&right() { return this->right_; } - KdNode(RRTNode *n); - }; - KdNode *kdroot_ = nullptr; - void delete_kd_nodes(KdNode *n); - void store_node(RRTNode *n, KdNode *&r, int l); - void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d); - public: - void init(); - void deinit(); - void store_node(RRTNode n); - RRTNode *nn(RRTNode &t); - std::vector nv(RRTNode &t); -}; - -/*! \brief Reeds & Shepp (build, search). - * - * Use Reeds & Shepp path length for building tree data structure as well as for - * searching it. - * - * \ingroup ext-cost - */ -class RRTExt6 : public virtual RRTS { -private: - double cost_build(RRTNode const& f, RRTNode const& t) const; - double cost_search(RRTNode const& f, RRTNode const& t) const; -}; - -/* \brief Different costs extension. - -Use different cost for bulding tree data structure and searching in the -structure. This one is complementary to `rrtext1.cc`. -*/ -class RRTExt5 : public virtual RRTS { - public: - /* \brief Reeds and Shepp path length. - */ - double cost_build(RRTNode &f, RRTNode &t); - /* \brief Euclidean distance. - */ - double cost_search(RRTNode &f, RRTNode &t); -}; - -/* \brief Use grid data structure to store RRT nodes. - -This approach speeds up the search process for the nearest neighbor and -the near vertices procedures. -*/ -class RRTExt4 : public virtual RRTS { - private: - class Cell { - private: - bool changed_ = false; - std::vector nodes_; - public: - void nn(RRTNode *t, RRTNode **nn, RRTS *p); - void store_node(RRTNode *n); - - // getter, setter - bool changed() const - { - return this->changed_; - } - std::vector &nodes() - { - return this->nodes_; - } - - Cell(); - }; - Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left - double x_min_ = 0; - double x_max_ = 0; - double y_min_ = 0; - double y_max_ = 0; - - unsigned int xi(RRTNode n); - unsigned int yi(RRTNode n); - public: - void init(); - void deinit(); - void store_node(RRTNode n); - RRTNode *nn(RRTNode &t); - std::vector nv(RRTNode &t); -}; - -/* \brief Use Dijkstra algorithm to find the shorter path. -*/ -class RRTExt3 : public virtual RRTS { - private: - 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; - } -}; - /*! \brief Use cute_c2 library for collision detection. * * \ingroup ext-col @@ -452,20 +231,5 @@ public: void reset(); }; -/* \brief Different costs extension. - -Use different cost for bulding tree data structure and searching in the -structure. -*/ -class RRTExt1 : public virtual RRTS { - public: - /* \brief Reeds and Shepp path length. - */ - double cost_build(RRTNode &f, RRTNode &t); - /* \brief Matej's heuristics. - */ - double cost_search(RRTNode &f, RRTNode &t); -}; - } // namespace rrts #endif /* RRTS_RRTEXT_H */ diff --git a/rrts/src/rrtext1.cc b/rrts/src/rrtext1.cc deleted file mode 100644 index 4a7e864..0000000 --- a/rrts/src/rrtext1.cc +++ /dev/null @@ -1,29 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.h" -#include "reeds_shepp.h" - -double RRTExt1::cost_build(RRTNode &f, RRTNode &t) -{ - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); -} - -double RRTExt1::cost_search(RRTNode &f, RRTNode &t) -{ - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - double heur = std::min( - std::abs(t.h() - f.h()), - 2 * M_PI - std::abs(t.h() - f.h()) - ); - heur *= this->bc.mtr(); - cost = std::max(cost, heur); - return cost; -} diff --git a/rrts/src/rrtext11.cc b/rrts/src/rrtext11.cc deleted file mode 100644 index 67e8496..0000000 --- a/rrts/src/rrtext11.cc +++ /dev/null @@ -1,42 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.h" - -bool RRTExt11::goal_found(RRTNode &f) -{ - auto &g = this->goals().front(); - auto fbc = BicycleCar(); - fbc.x(f.x()); - fbc.y(f.y()); - fbc.h(f.h()); - auto gbc = BicycleCar(); - gbc.x(g.x()); - gbc.y(g.y()); - gbc.h(g.h()); - bool drivable = false; - if (this->entry_set) - drivable = gbc.drivable(fbc, this->entry.b, this->entry.e); - else - drivable = gbc.drivable(fbc); - if (drivable) { - this->steer(f, g); - if (std::get<0>(this->collide_steered_from(f))) - return false; - double cost = this->cost_build(f, g); - this->join_steered(&f); - if (g.p() == nullptr) { - g.p(&f); - g.c(cost); - this->path_cost_before_opt_ = g.cc; - } else if (f.cc + cost < g.cc) { - g.p(&f); - g.c(cost); - } - return true; - } - return false; -} diff --git a/rrts/src/rrtext12.cc b/rrts/src/rrtext12.cc deleted file mode 100644 index 8194adc..0000000 --- a/rrts/src/rrtext12.cc +++ /dev/null @@ -1,139 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.h" - -void RRTExt12::steer1(RRTNode &f, RRTNode &t) -{ - // assume that `t` is circle uniformly sampled - auto bc = BicycleCar(); - bc.x(f.x()); - bc.y(f.y()); - bc.h(f.h()); - bc.set_max_steer(); - auto max_steer = bc.st(); - // map t.h() in -PI...+PI to -max_steer...+max_steer - bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer); - bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5); - this->steered().clear(); - while (true) { - bc.next(); - auto n = RRTNode(bc); - if ( - this->steered().size() == 0 - && std::get<0>(this->collide_two_nodes(f, n)) - ) { - return; - } else if ( - ( - this->steered().size() > 0 - && std::get<0>(this->collide_two_nodes( - this->steered().back(), - n - )) - ) - || this->steered().size() > 25 - ) { - break; - } - this->steered().push_back(RRTNode(bc)); - } - bc.sp(-1*bc.sp()); - bc.next(); - bc.sp(-1*bc.sp()); - this->samples().back().x(bc.x()); - this->samples().back().y(bc.y()); - this->samples().back().h(bc.h()); -} -bool RRTExt12::connect() -{ - RRTNode *t = &this->steered().front(); - RRTNode *f = this->use_nn; - double cost = this->cost_search(*f, *t); - // steer from f->t and then continue with the steered. - this->tmp_steer(*f, *t); - if (this->tmp_steered_.size() > 0) { - auto col = this->collide_tmp_steered_from(*f); - if (std::get<0>(col)) - return false; - this->join_tmp_steered(f); - f = &this->nodes().back(); - } - if (sqrt( - pow(f->x() - t->x(), 2) - + pow(f->y() - t->y(), 2) - ) > 0.5) - return false; - // cont. - this->store_node(this->steered().front()); - t = &this->nodes().back(); - t->p(f); - t->c(this->cost_build(*f, *t)); - t->set_t(RRTNodeType::connected); - return true; -} -bool RRTExt12::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()) - return false; - if (this->samples().size() == 0) { - this->samples().push_back(RRTNode()); - this->samples().back().x(this->goals().front().x()); - this->samples().back().y(this->goals().front().y()); - this->samples().back().h(this->goals().front().h()); - this->use_nn = &this->nodes().front(); - } else { - this->sample(); - } - this->steer1( - *this->use_nn, - this->samples().back() - ); - if (this->steered().size() == 0) - return next; - auto col = this->collide_steered_from(*this->use_nn); - if (std::get<0>(col)) { - auto rcnt = this->steered().size() - std::get<1>(col); - while (rcnt-- > 0) { - this->steered().pop_back(); - } - } - if (!this->connect()) - return next; - this->rewire(); - unsigned scnt = this->steered().size(); - this->join_steered(&this->nodes().back()); - RRTNode *just_added = &this->nodes().back(); - while (scnt > 0) { - // store all the steered1 nodes - this->steered1_.push_back(just_added); - scnt--; - auto &g = this->goals().front(); - this->steer2(*just_added, g); - auto col = this->collide_steered_from(*just_added); - if (std::get<0>(col)) { - auto rcnt = this->steered().size() - std::get<1>(col); - while (rcnt-- > 0) { - this->steered().pop_back(); - } - } - this->join_steered(just_added); - // store all the steered2 nodes - RRTNode* jap = &this->nodes().back(); - while (jap != just_added) { - this->steered2_.push_back(jap); - jap = jap->p(); - } - this->gf(this->goal_found(this->nodes().back())); - just_added = just_added->p(); - } - return next; -} diff --git a/rrts/src/rrtext20.cc b/rrts/src/rrtext20.cc deleted file mode 100644 index d82ad5f..0000000 --- a/rrts/src/rrtext20.cc +++ /dev/null @@ -1,55 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2022 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include -#include "rrtext.hh" - -namespace rrts { - -bool -RRTExt20::collide_steered() -{ - assert(this->_points_to_check != nullptr); - std::vector points_to_check(*this->_points_to_check); - unsigned int i = 0; - for (auto &n: this->steered_) { - for (auto &p: points_to_check) { - Point const n_p(n); - CarSize const n_cs(this->bc()); - if (p.inside_of(n_p, n_cs.edist_to_rr())) { - break; - } - if (p.edist(n_p) < n_cs.edist_to_lf() - && p.edist(n_p) > n_cs.edist_to_rr()) { - BicycleCar n_bc(this->bc()); - n_bc.x(n.x()); - n_bc.y(n.y()); - n_bc.h(n.h()); - std::vector poly; - poly.push_back(n_bc.lf()); - poly.push_back(n_bc.lr()); - poly.push_back(n_bc.rr()); - poly.push_back(n_bc.rf()); - poly.push_back(n_bc.lf()); - if (p.inside_of(poly)) { - break; - } - - } - } - i++; - } - this->steered_.erase(this->steered_.begin() + i, this->steered_.end()); - return this->steered_.size() == 0; -} - -void -RRTExt20::set_points_to_check(std::vector const *p) -{ - this->_points_to_check = p; -} - -} /* namespace rrts */ diff --git a/rrts/src/rrtext3.cc b/rrts/src/rrtext3.cc deleted file mode 100644 index 1f91f5e..0000000 --- a/rrts/src/rrtext3.cc +++ /dev/null @@ -1,288 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include -#include "rrtext.h" - -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; - else - this->orig_path_cost(this->orig_path().back()->cc); - } - 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; - dn.reserve(this->orig_path().size()); - unsigned int dncnt = 0; - for (auto n: this->orig_path()) { - if ( - n->t(RRTNodeType::cusp) - || n->t(RRTNodeType::connected) - ) { - dn.push_back(DijkstraNode(*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 = 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()); - while (!pq.empty()) { - DijkstraNode f = pq.top(); - 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))) - continue; - if (cost < dn[i].cc) { - dn[i].cc = cost; - dn[i].p(f.s); - if (!dn[i].vi()) - pq.push(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 RRTExt3::second_path_optimization() -{ - 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; - dn.reserve(this->orig_path().size()); - unsigned int dncnt = 0; - for (auto n: this->orig_path()) { - if ( - n->t(RRTNodeType::cusp) - || n->t(RRTNodeType::connected) - ) { - dn.push_back(DijkstraNode(*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 = 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()); - while (!pq.empty()) { - DijkstraNode t = pq.top(); - 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]))) - continue; - if (cost < t.cc) { - t.cc = cost; - t.p(dn[i].s); - if (!dn[i].vi()) - pq.push(dn[i]); - } - } - } - DijkstraNode *fn = nullptr; - for (auto &n: dn) { - if (n.v) { - fn = &n; - break; - } - } - 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(); - } - RRTS::compute_path(); -} - -void RRTExt3::compute_path() -{ - 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()); -} - -Json::Value RRTExt3::json() -{ - 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; - } - return jvo; -} - -void RRTExt3::json(Json::Value jvi) -{ - return RRTS::json(jvi); -} diff --git a/rrts/src/rrtext4.cc b/rrts/src/rrtext4.cc deleted file mode 100644 index 5ef6f14..0000000 --- a/rrts/src/rrtext4.cc +++ /dev/null @@ -1,118 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.h" - -void RRTExt4::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p) -{ - double cost = p->cost_search(**nn, *t); - for (auto f: this->nodes()) { - if (p->cost_search(*f, *t) < cost) { - *nn = f; - cost = p->cost_search(*f, *t); - } - } -} - -void RRTExt4::Cell::store_node(RRTNode *n) -{ - this->nodes_.push_back(n); - this->changed_ = true; -} - -RRTExt4::Cell::Cell() -{ -} - -unsigned int RRTExt4::xi(RRTNode n) -{ - if (n.x() >= this->x_max_) - return GRID_MAX_XI - 1; - if (n.x() <= this->x_min_) - return 0; - return (unsigned int) (floor(n.x() - this->x_min_) / GRID); -} - -unsigned int RRTExt4::yi(RRTNode n) -{ - if (n.y() > this->y_max_) - return GRID_MAX_YI - 1; - if (n.y() <= this->y_min_) - return 0; - return (unsigned int) (floor(n.y() - this->y_min_) / GRID); -} - -// API -void RRTExt4::init() -{ - this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2; - this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2; - this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2; - this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2; -} - -void RRTExt4::deinit() -{ - for (unsigned int i = 0; i < GRID_MAX_XI; i++) - for (unsigned int j = 0; j < GRID_MAX_YI; j++) - this->grid_[i][j].nodes().clear(); -} - -void RRTExt4::store_node(RRTNode n) -{ - RRTS::store_node(n); - RRTNode *sn = &this->nodes().back(); - this->grid_[this->xi(n)][this->yi(n)].store_node(sn); -} - -RRTNode *RRTExt4::nn(RRTNode &t) -{ - RRTNode *nn = &this->nodes().front(); - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int l = 0; - while (this->cost_search(*nn, t) > l * GRID) { - int xi_min = txi - l; - if (xi_min < 0) - xi_min = 0; - int xi_max = txi + l; - if (xi_max > GRID_MAX_XI - 1) - xi_max = GRID_MAX_XI - 1; - int yi_min = tyi - l; - if (yi_min < 0) - yi_min = 0; - int yi_max = tyi + l; - if (yi_max > GRID_MAX_YI - 1) - yi_max = GRID_MAX_YI - 1; - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_max].nn(&t, &nn, this); - } - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_min].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_min][yi].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_max][yi].nn(&t, &nn, this); - } - l++; - } - return nn; -} - -std::vector RRTExt4::nv(RRTNode &t) -{ - std::vector nv; - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int l = 0; - double cost = std::min(GAMMA(this->nodes().size()), ETA); - for (auto f: this->grid_[txi][tyi].nodes()) - if (this->cost_search(*f, t) < cost) - nv.push_back(f); - return nv; -} diff --git a/rrts/src/rrtext5.cc b/rrts/src/rrtext5.cc deleted file mode 100644 index d394730..0000000 --- a/rrts/src/rrtext5.cc +++ /dev/null @@ -1,23 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.h" -#include "reeds_shepp.h" - -double RRTExt5::cost_build(RRTNode &f, RRTNode &t) -{ - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc.mtr()); - return rsss.distance(q0, q1); -} - -double RRTExt5::cost_search(RRTNode &f, RRTNode &t) -{ - double cost = 0; - cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2)); - return cost; -} diff --git a/rrts/src/rrtext6.cc b/rrts/src/rrtext6.cc deleted file mode 100644 index 2abf77a..0000000 --- a/rrts/src/rrtext6.cc +++ /dev/null @@ -1,27 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.hh" -#include "reeds_shepp.h" - -namespace rrts { - -double -RRTExt6::cost_build(RRTNode const& f, RRTNode const& t) const -{ - double q0[] = {f.x(), f.y(), f.h()}; - double q1[] = {t.x(), t.y(), t.h()}; - ReedsSheppStateSpace rsss(this->bc_.mtr()); - return rsss.distance(q0, q1); -} - -double -RRTExt6::cost_search(RRTNode const& f, RRTNode const& t) const -{ - return this->cost_build(f, t); -} - -} // namespace rrts diff --git a/rrts/src/rrtext7.cc b/rrts/src/rrtext7.cc deleted file mode 100644 index 9a7e374..0000000 --- a/rrts/src/rrtext7.cc +++ /dev/null @@ -1,97 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.h" - -RRTExt7::KdNode::KdNode(RRTNode *n) - : node_(n) - , left_(nullptr) - , right_(nullptr) -{ -} - -void RRTExt7::delete_kd_nodes(KdNode *n) -{ - if (!n) - return; - if (n->left() != nullptr) - delete_kd_nodes(n->left()); - if (n->right() != nullptr) - delete_kd_nodes(n->right()); - delete n; -} - -void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l) -{ - if (r == nullptr) - r = new KdNode(n); - else if (l % 2 == 0 && n->x() < r->node()->x()) - store_node(n, r->left(), l + 1); - else if (l % 2 == 0) - store_node(n, r->right(), l + 1); - else if (l % 2 == 1 && n->y() < r->node()->y()) - store_node(n, r->left(), l + 1); - else - store_node(n, r->right(), l + 1); -} - -void RRTExt7::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d) -{ - if (r == nullptr) - return; - if (this->cost_search(*r->node(), t) < d) { - n = r->node(); - d = this->cost_search(*r->node(), t); - } - if (l % 2 == 0 && t.x() < r->node()->x()) { - nn(n, t, r->left(), l + 1, d); - if (r->node()->x() - t.x() < d) - nn(n, t, r->right(), l + 1, d); - } else if (l % 2 == 0) { - nn(n, t, r->right(), l + 1, d); - if (t.x() - r->node()->x() < d) - nn(n, t, r->left(), l + 1, d); - } else if (l % 2 == 1 && t.y() < r->node()->y()) { - nn(n, t, r->left(), l + 1, d); - if (r->node()->y() - t.y() < d) - nn(n, t, r->right(), l + 1, d); - } else { - nn(n, t, r->right(), l + 1, d); - if (t.y() - r->node()->y() < d) - nn(n, t, r->left(), l + 1, d); - } -} - -// API -void RRTExt7::init() -{ -} - -void RRTExt7::deinit() -{ - this->delete_kd_nodes(this->kdroot_); -} - -void RRTExt7::store_node(RRTNode n) -{ - RRTS::store_node(n); - RRTNode *sn = &this->nodes().back(); - this->store_node(sn, this->kdroot_, 0); -} - -RRTNode *RRTExt7::nn(RRTNode &t) -{ - RRTNode *n = &this->nodes().front(); - double d = 9999; - this->nn(n, t, this->kdroot_, 0, d); - return n; -} - -std::vector RRTExt7::nv(RRTNode &t) -{ - std::vector nv; - return nv; -} diff --git a/rrts/src/rrtext9.cc b/rrts/src/rrtext9.cc deleted file mode 100644 index 972e0de..0000000 --- a/rrts/src/rrtext9.cc +++ /dev/null @@ -1,138 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2021 Jiri Vlasak - * - * SPDX-License-Identifier: GPL-3.0-only - */ - -#include "rrtext.h" - -void RRTExt9::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p) -{ - double cost = p->cost_search(**nn, *t); - for (auto f: this->nodes()) { - if (p->cost_search(*f, *t) < cost) { - *nn = f; - cost = p->cost_search(*f, *t); - } - } -} - -void RRTExt9::Cell::store_node(RRTNode *n) -{ - this->nodes_.push_back(n); - this->changed_ = true; -} - -RRTExt9::Cell::Cell() -{ -} - -unsigned int RRTExt9::xi(RRTNode n) -{ - if (n.x() >= this->x_max_) - return GRID_MAX_XI - 1; - if (n.x() <= this->x_min_) - return 0; - return (unsigned int) (floor(n.x() - this->x_min_) / GRID); -} - -unsigned int RRTExt9::yi(RRTNode n) -{ - if (n.y() > this->y_max_) - return GRID_MAX_YI - 1; - if (n.y() <= this->y_min_) - return 0; - return (unsigned int) (floor(n.y() - this->y_min_) / GRID); -} - -unsigned int RRTExt9::hi(RRTNode n) -{ - if (n.h() > this->h_max_) - return GRID_MAX_HI - 1; - if (n.h() <= this->h_min_) - return 0; - return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI)); -} - -// API -void RRTExt9::init() -{ - this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2; - this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2; - this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2; - this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2; -} - -void RRTExt9::deinit() -{ - for (unsigned int i = 0; i < GRID_MAX_XI; i++) - for (unsigned int j = 0; j < GRID_MAX_YI; j++) - for (unsigned int k = 0; k < GRID_MAX_HI; k++) - this->grid_[i][j][k].nodes().clear(); -} - -void RRTExt9::store_node(RRTNode n) -{ - RRTS::store_node(n); - RRTNode *sn = &this->nodes().back(); - this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn); -} - -RRTNode *RRTExt9::nn(RRTNode &t) -{ - RRTNode *nn = &this->nodes().front(); - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int thi = this->hi(t); - unsigned int l = 0; - while (this->cost_search(*nn, t) > l * GRID) { - int xi_min = txi - l; - if (xi_min < 0) - xi_min = 0; - int xi_max = txi + l; - if (xi_max > GRID_MAX_XI - 1) - xi_max = GRID_MAX_XI - 1; - int yi_min = tyi - l; - if (yi_min < 0) - yi_min = 0; - int yi_max = tyi + l; - if (yi_max > GRID_MAX_YI - 1) - yi_max = GRID_MAX_YI - 1; - int hi_min = thi - l; // TODO respect kinematic constraints - if (hi_min < 0) - hi_min = 0; - int hi_max = thi + l; - if (hi_max > GRID_MAX_HI - 1) - hi_max = GRID_MAX_HI - 1; - for (int hi = hi_min; hi <= hi_max; hi++) { - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_max][hi].nn(&t, &nn, this); - } - for (int xi = xi_min; xi <= xi_max; xi++) { - this->grid_[xi][yi_min][hi].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_min][yi][hi].nn(&t, &nn, this); - } - for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) { - this->grid_[xi_max][yi][hi].nn(&t, &nn, this); - } - } - l++; - } - return nn; -} - -std::vector RRTExt9::nv(RRTNode &t) -{ - std::vector nv; - unsigned int txi = this->xi(t); - unsigned int tyi = this->yi(t); - unsigned int thi = this->hi(t); - unsigned int l = 0; - double cost = std::min(GAMMA(this->nodes().size()), ETA); - for (auto f: this->grid_[txi][tyi][thi].nodes()) - if (this->cost_search(*f, t) < cost) - nv.push_back(f); - return nv; -} -- 2.39.2