base/rrtbase.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <cmath>
  19 #include "bcar.h"
  20 #include "rrtbase.h"
  21
  22 RRTBase::RRTBase():
  23         root_(new RRTNode()),
  24         goal_(new RRTNode())
  25 {}
  26
  27 RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
  28         root_(init),
  29         goal_(goal)
  30 {}
  31
  32 RRTNode *RRTBase::root()
  33 {
  34         return this->root_;
  35 }
  36
  37 RRTNode *RRTBase::goal()
  38 {
  39         return this->goal_;
  40 }
  41
  42 std::vector<RRTNode *> &RRTBase::nodes()
  43 {
  44         return this->nodes_;
  45 }
  46
  47 std::vector<RRTNode *> &RRTBase::samples()
  48 {
  49         return this->samples_;
  50 }
  51
  52 std::vector<std::vector<RRTNode *>> &RRTBase::tlog()
  53 {
  54         return this->tlog_;
  55 }
  56
  57 bool RRTBase::goal_found()
  58 {
  59         return this->goal_found_;
  60 }
  61
  62 float RRTBase::elapsed()
  63 {
  64         std::chrono::duration<float> dt;
  65         dt = std::chrono::duration_cast<std::chrono::duration<float>>(
  66                         this->tend_ - this->tstart_);
  67         return dt.count();
  68 }
  69
  70 bool RRTBase::tlog(std::vector<RRTNode *> t)
  71 {
  72         this->tlog_.push_back(t);
  73         return true;
  74 }
  75
  76 void RRTBase::tstart()
  77 {
  78         this->tstart_ = std::chrono::high_resolution_clock::now();
  79 }
  80
  81 void RRTBase::tend()
  82 {
  83         this->tend_ = std::chrono::high_resolution_clock::now();
  84 }
  85
  86 bool RRTBase::link_obstacles(
  87                 std::vector<CircleObstacle> *cobstacles,
  88                 std::vector<SegmentObstacle> *sobstacles)
  89 {
  90         this->cobstacles_ = cobstacles;
  91         this->sobstacles_ = sobstacles;
  92         if (!this->cobstacles_ || !this->sobstacles_) {
  93                 return false;
  94         }
  95         return true;
  96 }
  97
  98 bool RRTBase::goal_found(
  99                 RRTNode *node,
 100                 float (*cost)(RRTNode *, RRTNode* ))
 101 {
 102         if (this->goal_found_)
 103                 return false;
 104         float xx = pow(node->x() - this->goal_->x(), 2);
 105         float yy = pow(node->y() - this->goal_->y(), 2);
 106         float dh = std::abs(node->h() - this->goal_->h());
 107         if (pow(xx + yy, 0.5) < this->GOAL_FOUND_DISTANCE &&
 108                         dh < this->GOAL_FOUND_ANGLE) {
 109                 node->add_child(this->goal_, (*cost)(node, this->goal_));
 110                 if (this->collide(node, this->goal_)) {
 111                         node->children().pop_back();
 112                         return false;
 113                 }
 114                 this->goal_found_ = true;
 115                 return true;
 116         }
 117         return false;
 118 }
 119
 120 bool RRTBase::collide(RRTNode *init, RRTNode *goal)
 121 {
 122         std::vector<RRTEdge *> edges;
 123         RRTNode *tmp = goal;
 124         while (tmp != init) {
 125                 BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
 126                 for (auto &o: *this->cobstacles_) {
 127                         if (o.collide(tmp)) {
 128                                 return true;
 129                         }
 130                         // TODO collide with car frame
 131                 }
 132                 for (auto &o: *this->sobstacles_) {
 133                         for (auto &e: bc.frame()) {
 134                                 if (o.collide(e)) {
 135                                         return true;
 136                                 }
 137                         }
 138                 }
 139                 if (!tmp->parent()) {
 140                         break;
 141                 }
 142                 edges.push_back(new RRTEdge(tmp, tmp->parent()));
 143                 tmp = tmp->parent();
 144         }
 145         for (auto &e: edges) {
 146                 for (auto &o: *this->cobstacles_) {
 147                         if (o.collide(e)) {
 148                                 return true;
 149                         }
 150                 }
 151                 for (auto &o: *this->sobstacles_) {
 152                         if (o.collide(e)) {
 153                                 return true;
 154                         }
 155                 }
 156         }
 157         return false;
 158 }
 159
 160 std::vector<RRTNode *> RRTBase::findt()
 161 {
 162         std::vector<RRTNode *> nodes;
 163         RRTNode *tmp = this->goal_;
 164         while (tmp != this->root_) {
 165                 nodes.push_back(tmp);
 166                 if (!tmp->parent())
 167                         break;
 168                 tmp = tmp->parent();
 169         }
 170         nodes.push_back(this->root_);
 171         return nodes;
 172 }