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