src/rrtext13.cc

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