src/rrtext13.cc

   1 #include <queue>
   2 #include "rrtext.hh"
   3 #include <iostream>
   4 using namespace std;
   5
   6 namespace rrts {
   7
   8 RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
   9 {
  10 }
  11
  12 bool
  13 RRTExt13::DijkstraNode::vi()
  14 {
  15         if (this->v) {
  16                 return true;
  17         }
  18         this->v = true;
  19         return false;
  20 }
  21
  22 int
  23 RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
  24                 DijkstraNode const& n2)
  25 {
  26         return n1.node->cc() > n2.node->cc();
  27 }
  28
  29 void
  30 RRTExt13::pick_interesting()
  31 {
  32         this->dn_.clear();
  33         this->dn_.reserve(this->path_.size());
  34         //for (auto n: this->opath_) {
  35         //      this->dn_.push_back(DijkstraNode(n));
  36         //      this->dn_.back().i = this->dn_.size() - 1;
  37         //}
  38         //return;
  39         for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
  40         //unsigned int i = 0; {
  41                 RRTNode& ni = *this->opath_[i];
  42                 this->bc_.set_pose(ni);
  43                 for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
  44                         RRTNode& nj = *this->opath_[j];
  45                         RRTNode& njl = *this->opath_[j - 1];
  46                         if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())
  47                                         || njl.edist(nj) < this->eta_) {
  48                                 this->dn_.push_back(DijkstraNode(&njl));
  49                                 this->dn_.back().i = this->dn_.size() - 1;
  50                         }
  51                         if (!this->bc_.drivable(nj)) {
  52                                 this->dn_.push_back(DijkstraNode(&njl));
  53                                 this->dn_.back().i = this->dn_.size() - 1;
  54                                 i = j;
  55                                 break;
  56                         }
  57                 }
  58         }
  59         this->dn_.push_back(DijkstraNode(this->opath_.back()));
  60         this->dn_.back().i = this->dn_.size() - 1;
  61 }
  62
  63 void
  64 RRTExt13::dijkstra_forward()
  65 {
  66         std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
  67                 DijkstraNodeComparator> pq;
  68         this->dn_.front().vi();
  69         pq.push(this->dn_.front());
  70         while (!pq.empty()) {
  71                 DijkstraNode fd = pq.top();
  72                 RRTNode& f = *fd.node;
  73                 pq.pop();
  74                 for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
  75                         RRTNode& t = *this->dn_[i].node;
  76                         double cost = f.cc() + this->cost_build(f, t);
  77                         this->steer(f, t);
  78                         if (this->steered_.size() == 0) {
  79                                 break;
  80                         }
  81                         if (this->collide_steered()) {
  82                                 continue;
  83                         }
  84                         this->bc_.set_pose(this->steered_.back());
  85                         bool td = this->bc_.drivable(t);
  86                         bool tn = this->bc_.edist(t) < this->eta_;
  87                         if (cost < t.cc() && td && tn) {
  88                                 this->join_steered(&f);
  89                                 t.p(this->nodes_.back());
  90                                 t.c(this->cost_build(this->nodes_.back(), t));
  91                                 if (!this->dn_[i].vi()) {
  92                                         pq.push(this->dn_[i]);
  93                                 }
  94                         }
  95                 }
  96         }
  97 }
  98
  99 void
 100 RRTExt13::dijkstra_backward()
 101 {
 102         std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
 103                 DijkstraNodeComparator> pq;
 104         this->dn_.back().vi();
 105         pq.push(this->dn_.back());
 106         while (!pq.empty()) {
 107                 DijkstraNode td = pq.top();
 108                 RRTNode& t = *td.node;
 109                 pq.pop();
 110                 for (unsigned int i = td.i - 1; i > 0; i--) {
 111                         RRTNode& f = *this->dn_[i].node;
 112                         double cost = f.cc() + this->cost_build(f, t);
 113                         this->steer(f, t);
 114                         if (this->steered_.size() == 0) {
 115                                 break;
 116                         }
 117                         if (this->collide_steered()) {
 118                                 continue;
 119                         }
 120                         this->bc_.set_pose(this->steered_.back());
 121                         bool td = this->bc_.drivable(t);
 122                         bool tn = this->bc_.edist(t) < this->eta_;
 123                         if (cost < t.cc() && td && tn) {
 124                                 this->join_steered(&f);
 125                                 t.p(this->nodes_.back());
 126                                 t.c(this->cost_build(this->nodes_.back(), t));
 127                                 if (!this->dn_[i].vi()) {
 128                                         pq.push(this->dn_[i]);
 129                                 }
 130                         }
 131                 }
 132         }
 133 }
 134
 135 void
 136 RRTExt13::compute_path()
 137 {
 138         RRTS::compute_path();
 139         bool measure_time = false;
 140         if (this->opath_.size() == 0) {
 141                 this->opath_ = this->path_;
 142                 this->ogoal_cc_ = this->goal_.cc();
 143                 measure_time = true;
 144         }
 145         if (measure_time) {
 146                 this->otime_ = -this->ter_.scnt();
 147         }
 148         this->pick_interesting();
 149         this->dijkstra_forward();
 150         this->pick_interesting();
 151         this->dijkstra_backward();
 152         RRTS::compute_path();
 153         if (measure_time) {
 154                 this->otime_ += this->ter_.scnt();
 155         }
 156 }
 157
 158 RRTExt13::RRTExt13()
 159 {
 160         this->opath_.reserve(10000);
 161         this->dn_.reserve(10000);
 162 }
 163
 164 Json::Value
 165 RRTExt13::json() const
 166 {
 167         auto jvo = RRTS::json();
 168         unsigned int i = 0;
 169         for (auto n: this->opath_) {
 170                 jvo["opath"][i][0] = n->x();
 171                 jvo["opath"][i][1] = n->y();
 172                 jvo["opath"][i][2] = n->h();
 173                 i++;
 174         }
 175         jvo["ogoal_cc"] = this->ogoal_cc_;
 176         jvo["otime"] = this->otime_;
 177         return jvo;
 178 }
 179
 180 void
 181 RRTExt13::json(Json::Value jvi)
 182 {
 183         RRTS::json(jvi);
 184 }
 185
 186 void
 187 RRTExt13::reset()
 188 {
 189         RRTS::reset();
 190         this->opath_.clear();
 191         this->ogoal_cc_ = 0.0;
 192         this->otime_ = 0.0;
 193 }
 194
 195 } // namespace rrts