src/rrtext13.cc

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