src/rrtext13.cc

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