rrts/src/rrtext12.cc

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