- RRT node types (currently `cusp` and `connected`).
- Compound extensions.
- Dijkstra algorithm for path optimization (`rrtext3.cc`).
+- Grid based `nn` and `nv` methods (`rrtext4.cc`).
[cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
add_library(rrts SHARED
src/rrts.cc
+ src/rrtext4.cc
src/rrtext3.cc
src/rrtext2.cc
src/rrtext1.cc
## Implemented extensions
There is a list of implemented extensions and what they include:
+- `rrtext4.cc`: store RRT nodes to grid,
- `rrtext3.cc`: Dijkstra algorithm for path optimization,
- `rrtext2.cc`: [cute c2][] for collision detection,
- `rrtext1.cc`: different cost for building and searching.
// ext2
#include "cute_c2.h"
+/*! \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_[100][100]; // [0, 0] is bottom left
+ unsigned int x_min_ = 0;
+ unsigned int x_max_ = 0;
+ unsigned int y_min_ = 0;
+ unsigned int y_max_ = 0;
+
+ unsigned int xi(RRTNode n);
+ unsigned int yi(RRTNode n);
+ public:
+ void init();
+ 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 {
#include <vector>
#include "bcar.h"
+#define ETA 1.0 // for steer, nv
+#define GAMMA(cV) ({ \
+ __typeof__ (cV) _cV = (cV); \
+ pow(log(_cV) / _cV, 1.0 / 3.0); \
+})
+
/*! \brief Possible type of RRT node.
\param cusp The node that is cusp (change in direction).
the current iteration should be the last one.
*/
bool should_stop();
+ protected:
/*! \brief Store RRT node to tree data structure.
*/
- void store_node(RRTNode n);
- protected:
+ virtual void store_node(RRTNode n);
+
// RRT procedures
std::tuple<bool, unsigned int, unsigned int>
collide(std::vector<std::tuple<double, double>> &poly);
collide_steered_from(RRTNode &f);
virtual std::tuple<bool, unsigned int, unsigned int>
collide_two_nodes(RRTNode &f, RRTNode &t);
- virtual double cost_build(RRTNode &f, RRTNode &t);
- virtual double cost_search(RRTNode &f, RRTNode &t);
void sample();
std::default_random_engine gen_;
std::normal_distribution<double> ndx_;
std::normal_distribution<double> ndy_;
std::normal_distribution<double> ndh_;
- RRTNode *nn(RRTNode &t);
- std::vector<RRTNode *> nv(RRTNode &t);
+ virtual RRTNode *nn(RRTNode &t);
+ virtual std::vector<RRTNode *> nv(RRTNode &t);
void steer(RRTNode &f, RRTNode &t);
/*! \brief Join steered nodes to RRT data structure
double mh, double dh
);
+ // RRT procedures
+ virtual double cost_build(RRTNode &f, RRTNode &t);
+ virtual double cost_search(RRTNode &f, RRTNode &t);
+
// getters, setters
unsigned int icnt() const { return this->icnt_; }
double scnt() const { return this->scnt_; }
--- /dev/null
+#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 99;
+ if (n.x() <= this->x_min_)
+ return 0;
+ return (unsigned int) (floor(n.x() - this->x_min_) / ETA);
+}
+
+unsigned int RRTExt4::yi(RRTNode n)
+{
+ if (n.y() > this->y_max_)
+ return 99;
+ if (n.y() <= this->y_min_)
+ return 0;
+ return (unsigned int) (floor(n.y() - this->y_min_) / ETA);
+}
+
+// API
+void RRTExt4::init()
+{
+ this->x_min_ = this->nodes().back().x() - 50 * ETA;
+ this->x_max_ = this->nodes().back().x() + 50 * ETA;
+ this->y_min_ = this->nodes().back().y() - 50 * ETA;
+ this->y_max_ = this->nodes().back().y() + 50 * ETA;
+}
+
+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 * ETA) {
+ int xi_min = txi - l;
+ if (xi_min < 0)
+ xi_min = 0;
+ int xi_max = txi + l;
+ if (xi_max > 99)
+ xi_max = 99;
+ int yi_min = tyi - l;
+ if (yi_min < 0)
+ yi_min = 0;
+ int yi_max = tyi + l;
+ if (yi_max > 99)
+ yi_max = 99;
+ 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;
+}
#include "reeds_shepp.h"
-#define ETA 1.0 // for steer, nv
-#define GAMMA(cV) ({ \
- __typeof__ (cV) _cV = (cV); \
- pow(log(_cV) / _cV, 1.0 / 3.0); \
-})
-
template <typename T> int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
projects / hubacji1 / rrts.git / commitdiff