src/rrts.cc

   1 #include <algorithm>
   2 #include "rrts.h"
   3
   4 #include "reeds_shepp.h"
   5
   6 #define ETA 1.0 // for steer, nv
   7 #define GAMMA(cV) ({ \
   8         __typeof__ (cV) _cV = (cV); \
   9         pow(log(_cV) / _cV, 1.0 / 3.0); \
  10 })
  11
  12 RRTNode::RRTNode()
  13 {
  14 }
  15
  16 RRTNode::RRTNode(const BicycleCar &bc) : BicycleCar(bc)
  17 {
  18 }
  19
  20 Obstacle::Obstacle()
  21 {
  22 }
  23
  24 // RRT procedures
  25 std::tuple<bool, unsigned int, unsigned int>
  26 RRTS::collide(std::vector<std::tuple<double, double>> &poly)
  27 {
  28         for (auto &o: this->obstacles())
  29                 if (std::get<0>(::collide(poly, o.poly())))
  30                         return ::collide(poly, o.poly());
  31         return std::make_tuple(false, 0, 0);
  32 }
  33
  34 std::tuple<bool, unsigned int, unsigned int>
  35 RRTS::collide_steered_from(RRTNode &f)
  36 {
  37         std::vector<std::tuple<double, double>> s;
  38         s.push_back(std::make_tuple(f.x(), f.y()));
  39         for (auto &n: this->steered()) {
  40                 s.push_back(std::make_tuple(n.lfx(), n.lfy()));
  41                 s.push_back(std::make_tuple(n.lrx(), n.lry()));
  42                 s.push_back(std::make_tuple(n.rrx(), n.rry()));
  43                 s.push_back(std::make_tuple(n.rfx(), n.rfy()));
  44         }
  45         auto col = this->collide(s);
  46         auto strip_from = this->steered().size() - std::get<1>(col) / 4;
  47         if (std::get<0>(col) && strip_from > 0) {
  48                 while (strip_from-- > 0) {
  49                         this->steered().pop_back();
  50                 }
  51                 return this->collide_steered_from(f);
  52         }
  53         return col;
  54 }
  55
  56 std::tuple<bool, unsigned int, unsigned int>
  57 RRTS::collide_two_nodes(RRTNode &f, RRTNode &t)
  58 {
  59         std::vector<std::tuple<double, double>> p;
  60         p.push_back(std::make_tuple(f.lfx(), f.lfy()));
  61         p.push_back(std::make_tuple(f.lrx(), f.lry()));
  62         p.push_back(std::make_tuple(f.rrx(), f.rry()));
  63         p.push_back(std::make_tuple(f.rfx(), f.rfy()));
  64         p.push_back(std::make_tuple(t.lfx(), t.lfy()));
  65         p.push_back(std::make_tuple(t.lrx(), t.lry()));
  66         p.push_back(std::make_tuple(t.rrx(), t.rry()));
  67         p.push_back(std::make_tuple(t.rfx(), t.rfy()));
  68         return this->collide(p);
  69 }
  70
  71 double RRTS::cost_build(RRTNode &f, RRTNode &t)
  72 {
  73         double cost = 0;
  74         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
  75         return cost;
  76 }
  77
  78 double RRTS::cost_search(RRTNode &f, RRTNode &t)
  79 {
  80         double cost = 0;
  81         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
  82         return cost;
  83 }
  84
  85 void RRTS::sample()
  86 {
  87         double x = this->ndx_(this->gen_);
  88         double y = this->ndy_(this->gen_);
  89         double h = this->ndh_(this->gen_);
  90         this->samples().push_back(RRTNode());
  91         this->samples().back().x(x);
  92         this->samples().back().y(y);
  93         this->samples().back().h(h);
  94 }
  95
  96 RRTNode *RRTS::nn(RRTNode &t)
  97 {
  98         RRTNode *nn = &this->nodes().front();
  99         double cost = this->cost_search(*nn, t);
 100         for (auto &f: this->nodes()) {
 101                 if (this->cost_search(f, t) < cost) {
 102                         nn = &f;
 103                         cost = this->cost_search(f, t);
 104                 }
 105         }
 106         return nn;
 107 }
 108
 109 std::vector<RRTNode *> RRTS::nv(RRTNode &t)
 110 {
 111         std::vector<RRTNode *> nv;
 112         double cost = std::min(GAMMA(this->nodes().size()), ETA);
 113         for (auto &f: this->nodes())
 114                 if (this->cost_search(f, t) < cost)
 115                         nv.push_back(&f);
 116         return nv;
 117 }
 118
 119 int cb_rs_steer(double q[3], void *user_data)
 120 {
 121         std::vector<RRTNode> *nodes = (std::vector<RRTNode> *) user_data;
 122         nodes->push_back(RRTNode());
 123         nodes->back().x(q[0]);
 124         nodes->back().y(q[1]);
 125         nodes->back().h(q[2]);
 126         return 0;
 127 }
 128
 129 void RRTS::steer(RRTNode &f, RRTNode &t)
 130 {
 131         this->steered().clear();
 132         double q0[] = {f.x(), f.y(), f.h()};
 133         double q1[] = {t.x(), t.y(), t.h()};
 134         ReedsSheppStateSpace rsss(f.mtr());
 135         rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->steered());
 136 }
 137
 138 void RRTS::join_steered(RRTNode *f)
 139 {
 140         while (this->steered().size() > 0) {
 141                 this->nodes().push_back(this->steered().front());
 142                 RRTNode *t = &this->nodes().back();
 143                 t->p(f);
 144                 t->c(this->cost_build(*f, *t));
 145                 this->steered().erase(this->steered().begin());
 146                 f = t;
 147         }
 148 }
 149
 150 bool RRTS::goal_found(RRTNode &f)
 151 {
 152         bool found = false;
 153         for (auto &g: this->goals()) {
 154                 double cost = this->cost_build(f, g);
 155                 double edist = sqrt(
 156                         pow(f.x() - g.x(), 2)
 157                         + pow(f.y() - g.y(), 2)
 158                 );
 159                 double adist = std::abs(f.h() - g.h());
 160                 if (edist < 0.05 && adist < M_PI / 32) {
 161                         found = true;
 162                         if (g.p() == nullptr || cc(f) + cost < cc(g)) {
 163                                 g.p(&f);
 164                                 g.c(cost);
 165                         }
 166                 }
 167         }
 168         return found;
 169 }
 170
 171 // RRT* procedures
 172 bool RRTS::connect()
 173 {
 174         RRTNode *t = &this->steered().front();
 175         RRTNode *f = this->nn(this->samples().back());
 176         double cost = this->cost_search(*f, *t);
 177         for (auto n: this->nv(*t)) {
 178                 if (
 179                         !std::get<0>(this->collide_two_nodes(*n, *t))
 180                         && this->cost_search(*n, *t) < cost
 181                 ) {
 182                         f = n;
 183                         cost = this->cost_search(*n, *t);
 184                 }
 185         }
 186         this->nodes().push_back(this->steered().front());
 187         t = &this->nodes().back();
 188         t->p(f);
 189         t->c(this->cost_build(*f, *t));
 190         return true;
 191 }
 192
 193 void RRTS::rewire()
 194 {
 195         RRTNode *f = &this->nodes().back();
 196         for (auto n: this->nv(*f)) {
 197                 if (
 198                         !std::get<0>(this->collide_two_nodes(*f, *n))
 199                         && cc(*f) + this->cost_search(*f, *n) < cc(*n)
 200                 ) {
 201                         n->p(f);
 202                         n->c(this->cost_build(*f, *n));
 203                 }
 204         }
 205 }
 206
 207 // API
 208 std::vector<RRTNode *> RRTS::path()
 209 {
 210         std::vector<RRTNode *> path;
 211         if (this->goals().size() == 0)
 212                 return path;
 213         RRTNode *goal = &this->goals().front();
 214         for (auto &n: this->goals()) {
 215                 if (
 216                         n.p() != nullptr
 217                         && (n.c() < goal->c() || goal->p() == nullptr)
 218                 ) {
 219                         goal = &n;
 220                 }
 221         }
 222         if (goal->p() == nullptr)
 223                 return path;
 224         while (goal != nullptr) {
 225                 path.push_back(goal);
 226                 goal = goal->p();
 227         }
 228         std::reverse(path.begin(), path.end());
 229         return path;
 230 }
 231
 232 bool RRTS::next()
 233 {
 234         bool next = true;
 235         if (++this->icnt_ > 999)
 236                 next = false;
 237         this->sample();
 238         this->steer(
 239                 *this->nn(this->samples().back()),
 240                 this->samples().back()
 241         );
 242         if (std::get<0>(this->collide_steered_from(
 243                 *this->nn(this->samples().back())
 244         )))
 245                 return next;
 246         if (!this->connect())
 247                 return next;
 248         this->rewire();
 249         unsigned scnt = this->steered().size();
 250         this->steered().erase(this->steered().begin());
 251         this->join_steered(&this->nodes().back());
 252         RRTNode *just_added = &this->nodes().back();
 253         while (scnt > 0) {
 254                 scnt--;
 255                 for (auto &g: this->goals()) {
 256                         this->steer(*just_added, g);
 257                         if (std::get<0>(this->collide_steered_from(
 258                                 *just_added
 259                         )))
 260                                 continue;
 261                         this->join_steered(just_added);
 262                 }
 263                 next = !this->goal_found(this->nodes().back());
 264                 just_added = just_added->p();
 265         }
 266         return next;
 267 }
 268
 269 void RRTS::set_sample(
 270         double mx, double dx,
 271         double my, double dy,
 272         double mh, double dh
 273 )
 274 {
 275         this->ndx_ = std::normal_distribution<double>(mx, dx);
 276         this->ndy_ = std::normal_distribution<double>(my, dy);
 277         this->ndh_ = std::normal_distribution<double>(mh, dh);
 278 }
 279
 280 RRTS::RRTS()
 281         : gen_(std::random_device{}())
 282 {
 283         this->goals().reserve(100);
 284         this->nodes().reserve(4000000);
 285         this->samples().reserve(1000);
 286         this->steered().reserve(20000);
 287         this->nodes().push_back(RRTNode()); // root
 288 }
 289
 290 double cc(RRTNode &t)
 291 {
 292         RRTNode *n = &t;
 293         double cost = 0;
 294         while (n != nullptr) {
 295                 cost += n->c();
 296                 n = n->p();
 297         }
 298         return cost;
 299 }