src/rrtext13.cc

   1 #include <queue>
   2 #include "rrtext.h"
   3 #include <iostream>
   4 void RRTExt13::first_path_optimization()
   5 {
   6         if (this->orig_path().size() == 0) {
   7                 this->orig_path_ = RRTS::path();
   8                 if (this->orig_path().size() == 0)
   9                         return;
  10                 else
  11                         this->orig_path_cost(this->orig_path().back()->cc);
  12         }
  13         class DijkstraNode : public RRTNode {
  14                 public:
  15                         DijkstraNode *s = nullptr;
  16                         RRTNode *n = nullptr;
  17                         unsigned int i = 0;
  18                         double cc = 9999;
  19                         bool v = false;
  20                         bool vi()
  21                         {
  22                                 if (this->v)
  23                                         return true;
  24                                 this->v = true;
  25                                 return false;
  26                         }
  27                         DijkstraNode(const RRTNode& n) {
  28                             this->x(n.x());
  29                             this->y(n.y());
  30                             this->h(n.h());
  31                             this->sp(n.sp());
  32                             this->st(n.st());
  33                         }
  34                         // override
  35                         DijkstraNode *p_ = nullptr;
  36                         DijkstraNode *p() const { return this->p_; }
  37                         void p(DijkstraNode *p) { this->p_ = p; }
  38         };
  39         class DijkstraNodeComparator {
  40                 public:
  41                         int operator() (
  42                                 const DijkstraNode &n1,
  43                                 const DijkstraNode &n2
  44                         )
  45                         {
  46                                 return n1.cc > n2.cc;
  47                         }
  48         };
  49         std::vector<DijkstraNode> dn;
  50         unsigned int ops = this->orig_path().size();
  51         auto op = this->orig_path();
  52         dn.reserve(ops);
  53         unsigned int dncnt = 0;
  54         dn.push_back(DijkstraNode(*this->orig_path().front()));
  55         dn.back().cc = this->orig_path().front()->cc;
  56         dn.back().s = &dn.back();
  57         dn.back().n = this->orig_path().front();
  58         dn.back().i = dncnt++;
  59         for (unsigned int i = 0; i < ops - 1; i++) {
  60                 auto ibc = BicycleCar();
  61                 ibc.x(op[i]->x());
  62                 ibc.y(op[i]->y());
  63                 ibc.h(op[i]->h());
  64                 for (unsigned int j = i + 1; j < ops; j++) {
  65                         auto jbc = BicycleCar();
  66                         jbc.x(op[j]->x());
  67                         jbc.y(op[j]->y());
  68                         jbc.h(op[j]->h());
  69                         if (!jbc.drivable(ibc)) {
  70                                 //std::cerr<<*op[i];
  71                                 //std::cerr<<" to ";
  72                                 //std::cerr<<*op[j];
  73                                 //std::cerr<<std::endl;
  74                                 dn.push_back(DijkstraNode(*op[j-1]));
  75                                 dn.back().cc = op[j-1]->cc;
  76                                 dn.back().s = &dn.back();
  77                                 dn.back().n = op[j-1];
  78                                 dn.back().i = dncnt++;
  79                                 i = j;
  80                                 break;
  81                         }
  82                 }
  83         }
  84         dn.push_back(DijkstraNode(*this->orig_path().back()));
  85         dn.back().cc = this->orig_path().back()->cc;
  86         dn.back().s = &dn.back();
  87         dn.back().n = this->orig_path().back();
  88         dn.back().i = dncnt++;
  89         std::priority_queue<
  90                 DijkstraNode,
  91                 std::vector<DijkstraNode>,
  92                 DijkstraNodeComparator
  93         > pq;
  94         dn.front().vi();
  95         pq.push(dn.front());
  96         while (!pq.empty()) {
  97                 DijkstraNode f = pq.top();
  98                 pq.pop();
  99                 for (unsigned int i = f.i + 1; i < dncnt; i++) {
 100                         double cost = f.cc + this->cost_search(f, dn[i]);
 101                         this->steer(f, dn[i]);
 102                         if (this->steered().size() == 0)
 103                                 break; // TODO why not continue?
 104                         if (std::get<0>(this->collide_steered_from(f)))
 105                                 continue;
 106                         if (cost < dn[i].cc) {
 107                                 dn[i].cc = cost;
 108                                 dn[i].p(f.s);
 109                                 if (!dn[i].vi())
 110                                         pq.push(dn[i]);
 111                         }
 112                 }
 113         }
 114         DijkstraNode *ln = nullptr;
 115         for (auto &n: dn) {
 116                 if (n.v)
 117                         ln = &n;
 118         }
 119         if (ln == nullptr || ln->p() == nullptr)
 120                 return;
 121         while (ln->p() != nullptr) {
 122                 RRTNode *t = ln->n;
 123                 RRTNode *f = ln->p()->n;
 124                 this->steer(*f, *t);
 125                 if (std::get<0>(this->collide_steered_from(*f)))
 126                         return;
 127                 this->join_steered(f);
 128                 t->p(&this->nodes().back());
 129                 t->c(this->cost_build(this->nodes().back(), *t));
 130                 ln = ln->p();
 131         }
 132         RRTS::compute_path();
 133 }
 134
 135 void RRTExt13::second_path_optimization()
 136 {
 137         if (this->first_optimized_path().size() == 0) {
 138                 return;
 139         }
 140         class DijkstraNode : public RRTNode {
 141                 public:
 142                         DijkstraNode *s = nullptr;
 143                         RRTNode *n = nullptr;
 144                         unsigned int i = 0;
 145                         double cc = 9999;
 146                         bool v = false;
 147                         bool vi()
 148                         {
 149                                 if (this->v)
 150                                         return true;
 151                                 this->v = true;
 152                                 return false;
 153                         }
 154                         DijkstraNode(const RRTNode& n) {
 155                             this->x(n.x());
 156                             this->y(n.y());
 157                             this->h(n.h());
 158                             this->sp(n.sp());
 159                             this->st(n.st());
 160                         }
 161                         // override
 162                         DijkstraNode *p_ = nullptr;
 163                         DijkstraNode *p() const { return this->p_; }
 164                         void p(DijkstraNode *p) { this->p_ = p; }
 165         };
 166         class DijkstraNodeComparator {
 167                 public:
 168                         int operator() (
 169                                 const DijkstraNode &n1,
 170                                 const DijkstraNode &n2
 171                         )
 172                         {
 173                                 return n1.cc > n2.cc;
 174                         }
 175         };
 176         std::vector<DijkstraNode> dn;
 177         unsigned int ops = this->orig_path().size();
 178         auto op = this->orig_path();
 179         dn.reserve(ops);
 180         unsigned int dncnt = 0;
 181         dn.push_back(DijkstraNode(*this->orig_path().front()));
 182         dn.back().cc = this->orig_path().front()->cc;
 183         dn.back().s = &dn.back();
 184         dn.back().n = this->orig_path().front();
 185         dn.back().i = dncnt++;
 186         for (unsigned int i = ops - 1; i > 1; i--) {
 187                 auto ibc = BicycleCar();
 188                 ibc.x(op[i]->x());
 189                 ibc.y(op[i]->y());
 190                 ibc.h(op[i]->h());
 191                 for (unsigned int j = i - 1; j > 0; j--) {
 192                         auto jbc = BicycleCar();
 193                         jbc.x(op[j]->x());
 194                         jbc.y(op[j]->y());
 195                         jbc.h(op[j]->h());
 196                         if (!jbc.drivable(ibc)) {
 197                                 //std::cerr<<*op[i];
 198                                 //std::cerr<<" to ";
 199                                 //std::cerr<<*op[j];
 200                                 //std::cerr<<std::endl;
 201                                 dn.push_back(DijkstraNode(*op[j-1]));
 202                                 dn.back().cc = op[j-1]->cc;
 203                                 dn.back().s = &dn.back();
 204                                 dn.back().n = op[j-1];
 205                                 dn.back().i = dncnt++;
 206                                 i = j;
 207                                 break;
 208                         }
 209                 }
 210         }
 211         dn.push_back(DijkstraNode(*this->orig_path().back()));
 212         dn.back().cc = this->orig_path().back()->cc;
 213         dn.back().s = &dn.back();
 214         dn.back().n = this->orig_path().back();
 215         dn.back().i = dncnt++;
 216         std::priority_queue<
 217                 DijkstraNode,
 218                 std::vector<DijkstraNode>,
 219                 DijkstraNodeComparator
 220         > pq;
 221         // TODO rewrite
 222         dn.back().vi();
 223         pq.push(dn.back());
 224         while (!pq.empty()) {
 225                 DijkstraNode t = pq.top();
 226                 pq.pop();
 227                 for (unsigned int i = t.i - 1; i > 0; i--) {
 228                         double cost = dn[i].cc + this->cost_search(dn[i], t);
 229                         this->steer(dn[i], t);
 230                         if (this->steered().size() == 0)
 231                                 break; // TODO why not continue?
 232                         if (std::get<0>(this->collide_steered_from(dn[i])))
 233                                 continue;
 234                         if (cost < t.cc) {
 235                                 t.cc = cost;
 236                                 t.p(dn[i].s);
 237                                 if (!dn[i].vi())
 238                                         pq.push(dn[i]);
 239                         }
 240                 }
 241         }
 242         DijkstraNode *fn = nullptr;
 243         for (auto &n: dn) {
 244                 if (n.v) {
 245                         fn = &n;
 246                         break;
 247                 }
 248         }
 249         if (fn == nullptr || fn->p() == nullptr)
 250                 return;
 251         DijkstraNode *ln = &dn.back();
 252         while (ln->p() != fn) {
 253                 RRTNode *t = ln->n;
 254                 RRTNode *f = ln->p()->n;
 255                 this->steer(*f, *t);
 256                 if (std::get<0>(this->collide_steered_from(*f)))
 257                         return;
 258                 this->join_steered(f);
 259                 t->p(&this->nodes().back());
 260                 t->c(this->cost_build(this->nodes().back(), *t));
 261                 ln = ln->p();
 262         }
 263         RRTS::compute_path();
 264 }
 265
 266 void RRTExt13::compute_path()
 267 {
 268         RRTS::compute_path();
 269         auto tstart = std::chrono::high_resolution_clock::now();
 270         this->first_path_optimization();
 271         this->first_optimized_path_ = RRTS::path();
 272         if (this->first_optimized_path_.size() > 0) {
 273                 this->first_optimized_path_cost(
 274                         this->first_optimized_path_.back()->cc
 275                 );
 276         } else {
 277                 return;
 278         }
 279         this->second_path_optimization();
 280         auto tend = std::chrono::high_resolution_clock::now();
 281         auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
 282             tend - tstart);
 283         this->log_opt_time_.push_back(tdiff.count());
 284 }
 285
 286 Json::Value RRTExt13::json()
 287 {
 288         Json::Value jvo = RRTS::json();
 289         jvo["orig_path_cost"] = this->orig_path_cost();
 290         {
 291                 unsigned int cu = 0;
 292                 unsigned int co = 0;
 293                 unsigned int pcnt = 0;
 294                 for (auto n: this->orig_path()) {
 295                         jvo["orig_path"][pcnt][0] = n->x();
 296                         jvo["orig_path"][pcnt][1] = n->y();
 297                         jvo["orig_path"][pcnt][2] = n->h();
 298                         if (n->t(RRTNodeType::cusp))
 299                                 cu++;
 300                         if (n->t(RRTNodeType::connected))
 301                                 co++;
 302                         pcnt++;
 303                 }
 304                 jvo["orig_cusps-in-path"] = cu;
 305                 jvo["orig_connecteds-in-path"] = co;
 306         }
 307         return jvo;
 308 }
 309
 310 void RRTExt13::json(Json::Value jvi)
 311 {
 312         return RRTS::json(jvi);
 313 }