src/rrtext12.cc

   1 #include "rrtext.h"
   2
   3 void RRTExt12::steer1(RRTNode &f, RRTNode &t)
   4 {
   5         // assume that `t` is circle uniformly sampled
   6         auto bc = BicycleCar();
   7         bc.x(f.x());
   8         bc.y(f.y());
   9         bc.h(f.h());
  10         bc.set_max_steer();
  11         auto max_steer = bc.st();
  12         // map t.h() in -PI...+PI to -max_steer...+max_steer
  13         bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
  14         bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5);
  15         this->steered().clear();
  16         while (true) {
  17                 bc.next();
  18                 auto n = RRTNode(bc);
  19                 if (
  20                         this->steered().size() == 0
  21                         && std::get<0>(this->collide_two_nodes(f, n))
  22                 ) {
  23                     return;
  24                 } else if (
  25                         (
  26                                 this->steered().size() > 0
  27                                 && std::get<0>(this->collide_two_nodes(
  28                                         this->steered().back(),
  29                                         n
  30                                 ))
  31                         )
  32                         || this->steered().size() > 25
  33                 ) {
  34                         break;
  35                 }
  36                 this->steered().push_back(RRTNode(bc));
  37         }
  38         bc.sp(-1*bc.sp());
  39         bc.next();
  40         bc.sp(-1*bc.sp());
  41         this->samples().back().x(bc.x());
  42         this->samples().back().y(bc.y());
  43         this->samples().back().h(bc.h());
  44 }
  45 bool RRTExt12::connect()
  46 {
  47         RRTNode *t = &this->steered().front();
  48         RRTNode *f = this->use_nn;
  49         double cost = this->cost_search(*f, *t);
  50         // steer from f->t and then continue with the steered.
  51         this->tmp_steer(*f, *t);
  52         if (this->tmp_steered_.size() > 0) {
  53                 auto col = this->collide_tmp_steered_from(*f);
  54                 if (std::get<0>(col))
  55                         return false;
  56                 this->join_tmp_steered(f);
  57                 f = &this->nodes().back();
  58         }
  59         if (sqrt(
  60                 pow(f->x() - t->x(), 2)
  61                 + pow(f->y() - t->y(), 2)
  62         ) > 0.5)
  63                 return false;
  64         // cont.
  65         this->store_node(this->steered().front());
  66         t = &this->nodes().back();
  67         t->p(f);
  68         t->c(this->cost_build(*f, *t));
  69         t->set_t(RRTNodeType::connected);
  70         return true;
  71 }
  72 bool RRTExt12::next()
  73 {
  74         if (this->icnt_ == 0)
  75                 this->tstart_ = std::chrono::high_resolution_clock::now();
  76         bool next = true;
  77         if (this->icnt_ > this->log_path_iter_)
  78             this->log_path_cost();
  79         if (this->should_stop())
  80                 return false;
  81         if (this->samples().size() == 0) {
  82                 this->samples().push_back(RRTNode());
  83                 this->samples().back().x(this->goals().front().x());
  84                 this->samples().back().y(this->goals().front().y());
  85                 this->samples().back().h(this->goals().front().h());
  86                 this->use_nn = &this->nodes().front();
  87         } else {
  88                 this->sample();
  89         }
  90         this->steer1(
  91                 *this->use_nn,
  92                 this->samples().back()
  93         );
  94         if (this->steered().size() == 0)
  95                 return next;
  96         auto col = this->collide_steered_from(*this->use_nn);
  97         if (std::get<0>(col)) {
  98                 auto rcnt = this->steered().size() - std::get<1>(col);
  99                 while (rcnt-- > 0) {
 100                         this->steered().pop_back();
 101                 }
 102         }
 103         if (!this->connect())
 104                 return next;
 105         this->rewire();
 106         unsigned scnt = this->steered().size();
 107         this->join_steered(&this->nodes().back());
 108         RRTNode *just_added = &this->nodes().back();
 109         while (scnt > 0) {
 110                 // store all the steered1 nodes
 111                 this->steered1_.push_back(just_added);
 112                 scnt--;
 113                 auto &g = this->goals().front();
 114                 this->steer2(*just_added, g);
 115                 auto col = this->collide_steered_from(*just_added);
 116                 if (std::get<0>(col)) {
 117                         auto rcnt = this->steered().size() - std::get<1>(col);
 118                         while (rcnt-- > 0) {
 119                                 this->steered().pop_back();
 120                         }
 121                 }
 122                 this->join_steered(just_added);
 123                 // store all the steered2 nodes
 124                 RRTNode* jap = &this->nodes().back();
 125                 while (jap != just_added) {
 126                         this->steered2_.push_back(jap);
 127                         jap = jap->p();
 128                 }
 129                 this->gf(this->goal_found(this->nodes().back()));
 130                 just_added = just_added->p();
 131         }
 132         return next;
 133 }