add_library(rrts STATIC
src/rrts.cc
src/rrtext21.cc
- src/rrtext20.cc
src/rrtext19.cc
src/rrtext18.cc
src/rrtext17.cc
src/rrtext13.cc
src/rrtext10.cc
src/rrtext8.cc
- src/rrtext6.cc
src/rrtext2.cc
src/reeds_shepp.cpp
src/dubins.c
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<bcar::Point> const *_points_to_check = nullptr;
- bool collide_steered();
-public:
- void set_points_to_check(std::vector<bcar::Point> const *p);
-};
-
/*! \brief Use Dubins paths-based steering procedure.
*
* \ingroup ext-steer
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
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<RRTNode *> 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<RRTNode *> &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<RRTNode *> 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
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<RRTNode *> 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<RRTNode *> 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<RRTNode *> &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<RRTNode *> nv(RRTNode &t);
-};
-
-/* \brief Use Dijkstra algorithm to find the shorter path.
-*/
-class RRTExt3 : public virtual RRTS {
- private:
- public:
- void reset();
- std::vector<RRTNode *> orig_path_;
- double orig_path_cost_ = 9999;
- std::vector<RRTNode *> 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<RRTNode *> &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<RRTNode *> &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
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 */
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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;
-}
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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;
-}
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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;
-}
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2022 Jiri Vlasak
- *
- * SPDX-License-Identifier: GPL-3.0-only
- */
-
-#include <cassert>
-#include "rrtext.hh"
-
-namespace rrts {
-
-bool
-RRTExt20::collide_steered()
-{
- assert(this->_points_to_check != nullptr);
- std::vector<bcar::Point> 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<Point> 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<bcar::Point> const *p)
-{
- this->_points_to_check = p;
-}
-
-} /* namespace rrts */
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * SPDX-License-Identifier: GPL-3.0-only
- */
-
-#include <queue>
-#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<DijkstraNode> 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<DijkstraNode>,
- 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<DijkstraNode> 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<DijkstraNode>,
- 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<std::chrono::duration<double>>(
- 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);
-}
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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<RRTNode *> RRTExt4::nv(RRTNode &t)
-{
- std::vector<RRTNode *> 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;
-}
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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;
-}
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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<RRTNode *> RRTExt7::nv(RRTNode &t)
-{
- std::vector<RRTNode *> nv;
- return nv;
-}
+++ /dev/null
-/*
- * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
- *
- * 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<RRTNode *> RRTExt9::nv(RRTNode &t)
-{
- std::vector<RRTNode *> 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;
-}
projects / hubacji1 / iamcar2.git / commitdiff