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(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_build(RRTNode &f, RRTNode &t)
  79 {
  80         double q0[] = {f.x(), f.y(), f.h()};
  81         double q1[] = {t.x(), t.y(), t.h()};
  82         ReedsSheppStateSpace rsss(f.mtr());
  83         return rsss.distance(q0, q1);
  84 }
  85
  86 double RRTS::cost_search(RRTNode &f, RRTNode &t)
  87 {
  88         double cost = 0;
  89         cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
  90         double heur = std::min(
  91                 std::abs(t.h() - f.h()),
  92                 2 * M_PI - std::abs(t.h() - f.h())
  93         );
  94         heur *= f.mtr();
  95         cost = std::max(cost, heur);
  96         return cost;
  97 }
  98
  99 void RRTS::sample()
 100 {
 101         double x = this->ndx_(this->gen_);
 102         double y = this->ndy_(this->gen_);
 103         double h = this->ndh_(this->gen_);
 104         this->samples().push_back(RRTNode());
 105         this->samples().back().x(x);
 106         this->samples().back().y(y);
 107         this->samples().back().h(h);
 108 }
 109
 110 RRTNode *RRTS::nn(RRTNode &t)
 111 {
 112         RRTNode *nn = &this->nodes().front();
 113         double cost = this->cost(*nn, t);
 114         for (auto &f: this->nodes()) {
 115                 if (this->cost(f, t) < cost) {
 116                         nn = &f;
 117                         cost = this->cost(f, t);
 118                 }
 119         }
 120         return nn;
 121 }
 122
 123 std::vector<RRTNode *> RRTS::nv(RRTNode &t)
 124 {
 125         std::vector<RRTNode *> nv;
 126         double cost = std::min(GAMMA(this->nodes().size()), ETA);
 127         for (auto &f: this->nodes())
 128                 if (this->cost(f, t) < cost)
 129                         nv.push_back(&f);
 130         return nv;
 131 }
 132
 133 int cb_rs_steer(double q[3], void *user_data)
 134 {
 135         std::vector<RRTNode> *nodes = (std::vector<RRTNode> *) user_data;
 136         nodes->push_back(RRTNode());
 137         nodes->back().x(q[0]);
 138         nodes->back().y(q[1]);
 139         nodes->back().h(q[2]);
 140         return 0;
 141 }
 142
 143 void RRTS::steer(RRTNode &f, RRTNode &t)
 144 {
 145         this->steered().clear();
 146         double q0[] = {f.x(), f.y(), f.h()};
 147         double q1[] = {t.x(), t.y(), t.h()};
 148         ReedsSheppStateSpace rsss(f.mtr());
 149         rsss.sample(q0, q1, 0.5, cb_rs_steer, &this->steered());
 150 }
 151
 152 void RRTS::join_steered(RRTNode *f)
 153 {
 154         while (this->steered().size() > 0) {
 155                 this->nodes().push_back(this->steered().front());
 156                 RRTNode *t = &this->nodes().back();
 157                 t->p(f);
 158                 t->c(this->cost(*f, *t));
 159                 this->steered().erase(this->steered().begin());
 160                 f = t;
 161         }
 162 }
 163
 164 bool RRTS::goal_found(RRTNode &f)
 165 {
 166         bool found = false;
 167         for (auto &g: this->goals()) {
 168                 double cost = this->cost(f, g);
 169                 double edist = sqrt(
 170                         pow(f.x() - g.x(), 2)
 171                         + pow(f.y() - g.y(), 2)
 172                 );
 173                 double adist = std::abs(f.h() - g.h());
 174                 if (edist < 0.05 && adist < M_PI / 32) {
 175                         found = true;
 176                         if (g.p() == nullptr || cc(f) + cost < cc(g)) {
 177                                 g.p(&f);
 178                                 g.c(cost);
 179                         }
 180                 }
 181         }
 182         return found;
 183 }
 184
 185 // RRT* procedures
 186 bool RRTS::connect()
 187 {
 188         bool conn = false;
 189         RRTNode *t = &this->steered().front();
 190         RRTNode *f = this->nn(this->samples().back());
 191         double cost = this->cost(*f, *t);
 192         for (auto n: this->nv(*t)) {
 193                 if (
 194                         !std::get<0>(this->collide_two_nodes(*n, *t))
 195                         && this->cost(*n, *t) < cost
 196                 ) {
 197                         f = n;
 198                         cost = this->cost(*n, *t);
 199                 }
 200         }
 201         this->nodes().push_back(this->steered().front());
 202         t = &this->nodes().back();
 203         t->p(f);
 204         t->c(this->cost(*f, *t));
 205         conn = true;
 206         return conn;
 207 }
 208
 209 void RRTS::rewire()
 210 {
 211         RRTNode *f = &this->nodes().back();
 212         for (auto n: this->nv(*f)) {
 213                 if (
 214                         !std::get<0>(this->collide_two_nodes(*f, *n))
 215                         && cc(*f) + this->cost(*f, *n) < cc(*n)
 216                 )
 217                         n->p(f);
 218         }
 219 }
 220
 221 // API
 222 std::vector<RRTNode *> RRTS::path()
 223 {
 224         std::vector<RRTNode *> path;
 225         if (this->goals().size() == 0)
 226                 return path;
 227         RRTNode *goal = &this->goals().front();
 228         for (auto &n: this->goals()) {
 229                 if (
 230                         n.p() != nullptr
 231                         && (n.c() < goal->c() || goal->p() == nullptr)
 232                 ) {
 233                         goal = &n;
 234                 }
 235         }
 236         if (goal->p() == nullptr)
 237                 return path;
 238         while (goal != nullptr) {
 239                 path.push_back(goal);
 240                 goal = goal->p();
 241         }
 242         std::reverse(path.begin(), path.end());
 243         return path;
 244 }
 245
 246 bool RRTS::next()
 247 {
 248         bool next = true;
 249         this->icnt_++;
 250         this->sample();
 251         this->steer(
 252                 *this->nn(this->samples().back()),
 253                 this->samples().back()
 254         );
 255         if (std::get<0>(this->collide_steered_from(
 256                 *this->nn(this->samples().back())
 257         )))
 258                 return next;
 259         if (!this->connect())
 260                 return next;
 261         this->rewire();
 262         unsigned scnt = this->steered().size();
 263         this->steered().erase(this->steered().begin());
 264         this->join_steered(&this->nodes().back());
 265         RRTNode *just_added = &this->nodes().back();
 266         while (scnt > 0) {
 267                 scnt--;
 268                 for (auto &g: this->goals()) {
 269                         this->steer(*just_added, g);
 270                         if (std::get<0>(this->collide_steered_from(
 271                                 *just_added
 272                         )))
 273                                 continue;
 274                         this->join_steered(just_added);
 275                 }
 276                 next = !this->goal_found(this->nodes().back());
 277                 just_added = just_added->p();
 278         }
 279         if (this->icnt_ > 999)
 280                 next = false;
 281         return next;
 282 }
 283
 284 void RRTS::set_sample(
 285         double mx, double dx,
 286         double my, double dy,
 287         double mh, double dh
 288 )
 289 {
 290         this->ndx_ = std::normal_distribution<double>(mx, dx);
 291         this->ndy_ = std::normal_distribution<double>(my, dy);
 292         this->ndh_ = std::normal_distribution<double>(mh, dh);
 293 }
 294
 295 RRTS::RRTS()
 296         : gen_(std::random_device{}())
 297 {
 298         this->goals().reserve(100);
 299         this->nodes().reserve(4000000);
 300         this->samples().reserve(1000);
 301         this->steered().reserve(20000);
 302         this->nodes().push_back(RRTNode()); // root
 303 }
 304
 305 double cc(RRTNode &t)
 306 {
 307         RRTNode *n = &t;
 308         double cost = 0;
 309         while (n != nullptr) {
 310                 cost += n->c();
 311                 n = n->p();
 312         }
 313         return cost;
 314 }