src/rrtext3.cc

   1 #include <queue>
   2 #include "rrtext.h"
   3
   4 void RRTExt3::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 RRTExt3::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         dn.reserve(this->orig_path().size());
  60         unsigned int dncnt = 0;
  61         for (auto n: this->orig_path()) {
  62                 if (
  63                         n->t(RRTNodeType::cusp)
  64                         || n->t(RRTNodeType::connected)
  65                 ) {
  66                         dn.push_back(DijkstraNode(*n));
  67                         dn.back().cc = n->cc;
  68                         dn.back().s = &dn.back();
  69                         dn.back().n = n;
  70                         dn.back().i = dncnt++;
  71                 }
  72         }
  73         dn.push_back(DijkstraNode(*this->orig_path().back()));
  74         dn.back().cc = this->orig_path().back()->cc;
  75         dn.back().s = &dn.back();
  76         dn.back().n = this->orig_path().back();
  77         dn.back().i = dncnt++;
  78         std::priority_queue<
  79                 DijkstraNode,
  80                 std::vector<DijkstraNode>,
  81                 DijkstraNodeComparator
  82         > pq;
  83         dn.front().vi();
  84         pq.push(dn.front());
  85         while (!pq.empty()) {
  86                 DijkstraNode f = pq.top();
  87                 pq.pop();
  88                 for (unsigned int i = f.i + 1; i < dncnt; i++) {
  89                         double cost = f.cc + this->cost_search(f, dn[i]);
  90                         this->steer(f, dn[i]);
  91                         if (this->steered().size() == 0)
  92                                 break; // TODO why not continue?
  93                         if (std::get<0>(this->collide_steered_from(f)))
  94                                 continue;
  95                         if (cost < dn[i].cc) {
  96                                 dn[i].cc = cost;
  97                                 dn[i].p(f.s);
  98                                 if (!dn[i].vi())
  99                                         pq.push(dn[i]);
 100                         }
 101                 }
 102         }
 103         DijkstraNode *ln = nullptr;
 104         for (auto &n: dn) {
 105                 if (n.v)
 106                         ln = &n;
 107         }
 108         if (ln == nullptr || ln->p() == nullptr)
 109                 return;
 110         while (ln->p() != nullptr) {
 111                 RRTNode *t = ln->n;
 112                 RRTNode *f = ln->p()->n;
 113                 this->steer(*f, *t);
 114                 if (std::get<0>(this->collide_steered_from(*f)))
 115                         return;
 116                 this->join_steered(f);
 117                 t->p(&this->nodes().back());
 118                 t->c(this->cost_build(this->nodes().back(), *t));
 119                 ln = ln->p();
 120         }
 121         RRTS::compute_path();
 122 }
 123
 124 void RRTExt3::second_path_optimization()
 125 {
 126         if (this->first_optimized_path().size() == 0) {
 127                 return;
 128         }
 129         class DijkstraNode : public RRTNode {
 130                 public:
 131                         DijkstraNode *s = nullptr;
 132                         RRTNode *n = nullptr;
 133                         unsigned int i = 0;
 134                         double cc = 9999;
 135                         bool v = false;
 136                         bool vi()
 137                         {
 138                                 if (this->v)
 139                                         return true;
 140                                 this->v = true;
 141                                 return false;
 142                         }
 143                         DijkstraNode(const RRTNode& n) {
 144                             this->x(n.x());
 145                             this->y(n.y());
 146                             this->h(n.h());
 147                             this->sp(n.sp());
 148                             this->st(n.st());
 149                         }
 150                         // override
 151                         DijkstraNode *p_ = nullptr;
 152                         DijkstraNode *p() const { return this->p_; }
 153                         void p(DijkstraNode *p) { this->p_ = p; }
 154         };
 155         class DijkstraNodeComparator {
 156                 public:
 157                         int operator() (
 158                                 const DijkstraNode &n1,
 159                                 const DijkstraNode &n2
 160                         )
 161                         {
 162                                 return n1.cc > n2.cc;
 163                         }
 164         };
 165         std::vector<DijkstraNode> dn;
 166         dn.reserve(this->orig_path().size());
 167         unsigned int dncnt = 0;
 168         for (auto n: this->orig_path()) {
 169                 if (
 170                         n->t(RRTNodeType::cusp)
 171                         || n->t(RRTNodeType::connected)
 172                 ) {
 173                         dn.push_back(DijkstraNode(*n));
 174                         dn.back().cc = n->cc;
 175                         dn.back().s = &dn.back();
 176                         dn.back().n = n;
 177                         dn.back().i = dncnt++;
 178                 }
 179         }
 180         dn.push_back(DijkstraNode(*this->orig_path().back()));
 181         dn.back().cc = this->orig_path().back()->cc;
 182         dn.back().s = &dn.back();
 183         dn.back().n = this->orig_path().back();
 184         dn.back().i = dncnt++;
 185         std::priority_queue<
 186                 DijkstraNode,
 187                 std::vector<DijkstraNode>,
 188                 DijkstraNodeComparator
 189         > pq;
 190         // TODO rewrite
 191         dn.back().vi();
 192         pq.push(dn.back());
 193         while (!pq.empty()) {
 194                 DijkstraNode t = pq.top();
 195                 pq.pop();
 196                 for (unsigned int i = t.i - 1; i > 0; i--) {
 197                         double cost = dn[i].cc + this->cost_search(dn[i], t);
 198                         this->steer(dn[i], t);
 199                         if (this->steered().size() == 0)
 200                                 break; // TODO why not continue?
 201                         if (std::get<0>(this->collide_steered_from(dn[i])))
 202                                 continue;
 203                         if (cost < t.cc) {
 204                                 t.cc = cost;
 205                                 t.p(dn[i].s);
 206                                 if (!dn[i].vi())
 207                                         pq.push(dn[i]);
 208                         }
 209                 }
 210         }
 211         DijkstraNode *fn = nullptr;
 212         for (auto &n: dn) {
 213                 if (n.v) {
 214                         fn = &n;
 215                         break;
 216                 }
 217         }
 218         if (fn == nullptr || fn->p() == nullptr)
 219                 return;
 220         DijkstraNode *ln = &dn.back();
 221         while (ln->p() != fn) {
 222                 RRTNode *t = ln->n;
 223                 RRTNode *f = ln->p()->n;
 224                 this->steer(*f, *t);
 225                 if (std::get<0>(this->collide_steered_from(*f)))
 226                         return;
 227                 this->join_steered(f);
 228                 t->p(&this->nodes().back());
 229                 t->c(this->cost_build(this->nodes().back(), *t));
 230                 ln = ln->p();
 231         }
 232         RRTS::compute_path();
 233 }
 234
 235 void RRTExt3::compute_path()
 236 {
 237         RRTS::compute_path();
 238         auto tstart = std::chrono::high_resolution_clock::now();
 239         this->first_path_optimization();
 240         this->first_optimized_path_ = RRTS::path();
 241         if (this->first_optimized_path_.size() > 0) {
 242                 this->first_optimized_path_cost(
 243                         this->first_optimized_path_.back()->cc
 244                 );
 245         } else {
 246                 return;
 247         }
 248         this->second_path_optimization();
 249         auto tend = std::chrono::high_resolution_clock::now();
 250         auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
 251             tend - tstart);
 252         this->log_opt_time_.push_back(tdiff.count());
 253 }
 254
 255 Json::Value RRTExt3::json()
 256 {
 257         Json::Value jvo = RRTS::json();
 258         jvo["orig_path_cost"] = this->orig_path_cost();
 259         {
 260                 unsigned int cu = 0;
 261                 unsigned int co = 0;
 262                 unsigned int pcnt = 0;
 263                 for (auto n: this->orig_path()) {
 264                         jvo["orig_path"][pcnt][0] = n->x();
 265                         jvo["orig_path"][pcnt][1] = n->y();
 266                         jvo["orig_path"][pcnt][2] = n->h();
 267                         if (n->t(RRTNodeType::cusp))
 268                                 cu++;
 269                         if (n->t(RRTNodeType::connected))
 270                                 co++;
 271                         pcnt++;
 272                 }
 273                 jvo["orig_cusps-in-path"] = cu;
 274                 jvo["orig_connecteds-in-path"] = co;
 275         }
 276         return jvo;
 277 }
 278
 279 void RRTExt3::json(Json::Value jvi)
 280 {
 281         return RRTS::json(jvi);
 282 }