Rewrite ext13

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 30f9759a3f474c693cac228696e6db917d212ee1..f981dfe2e19aa6dfae6803e3834f8296cbe9c1aa 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,6 +18,7 @@ link_libraries(jsoncpp_lib)
 add_library(rrts STATIC
        src/rrts.cc
        src/rrtext14.cc
+       src/rrtext13.cc
        src/rrtext10.cc
        src/rrtext8.cc
        src/rrtext2.cc

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 299f6202c7583756029decbb51d590118e736b8d..3469c08edcd48e05a8c997db00690ea2f54d5160 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -37,36 +37,32 @@ public:
 
 /*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
 class RRTExt13 : public virtual RRTS {
-       private:
+private:
+       class DijkstraNode {
        public:
-               void reset();
-               std::vector<RRTNode *> orig_path_;
-               double orig_path_cost_ = 9999;
-               std::vector<RRTNode *> first_optimized_path_;
-               double first_optimized_path_cost_ = 9999;
-               void first_path_optimization();
-               void second_path_optimization();
-               void compute_path();
-               Json::Value json();
-               void json(Json::Value jvi);
-
-               // getter, setter
-               std::vector<RRTNode *> &orig_path()
-               {
-                       return this->orig_path_;
-               };
-               double &orig_path_cost() { return this->orig_path_cost_; }
-               void orig_path_cost(double c) { this->orig_path_cost_ = c; }
-               std::vector<RRTNode *> &first_optimized_path()
-               {
-                       return this->first_optimized_path_;
-               };
-               double &first_optimized_path_cost() {
-                       return this->first_optimized_path_cost_;
-               }
-               void first_optimized_path_cost(double c) {
-                       this->first_optimized_path_cost_ = c;
-               }
+               RRTNode* node = nullptr;
+               unsigned int i = 0;
+               bool v = false;
+               bool vi();
+               DijkstraNode(RRTNode* n);
+       };
+       class DijkstraNodeComparator {
+       public:
+               int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+       };
+       std::vector<RRTNode*> opath_;
+       double ogoal_cc_ = 0.0;
+       double otime_ = 0.0;
+       std::vector<DijkstraNode> dn_;
+       void pick_interesting();
+       void dijkstra_forward();
+       void dijkstra_backward();
+       void compute_path();
+public:
+       RRTExt13();
+       Json::Value json() const;
+       void json(Json::Value jvi);
+       void reset();
 };
 
 /*! \brief Different `steer` procedures.

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 172ef6a9f46ebfc0601e7e8aa33207c803d699a1..03ff7148425b03e247f9bc96a7218ce9ee8726c1 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,314 +1,195 @@
 #include <queue>
-#include "rrtext.h"
+#include "rrtext.hh"
+#include <iostream>
+using namespace std;
 
-void RRTExt13::reset()
+namespace rrts {
+
+RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
 {
-       RRTS::reset();
-       this->orig_path().clear();
-       this->first_optimized_path().clear();
-       this->orig_path_cost_ = 9999.9;
-       this->first_optimized_path_cost_ = 9999.9;
 }
 
-void RRTExt13::first_path_optimization()
+bool
+RRTExt13::DijkstraNode::vi()
 {
-       if (this->orig_path().size() == 0) {
-               this->orig_path_ = RRTS::path();
-               if (this->orig_path().size() == 0)
-                       return;
-               else
-                       this->orig_path_cost(this->orig_path().back()->cc);
+       if (this->v) {
+               return true;
        }
-       class DijkstraNode : public RRTNode {
-               public:
-                       DijkstraNode *s = nullptr;
-                       RRTNode *n = nullptr;
-                       unsigned int i = 0;
-                       double cc = 9999;
-                       bool v = false;
-                       bool vi()
-                       {
-                               if (this->v)
-                                       return true;
-                               this->v = true;
-                               return false;
-                       }
-                       DijkstraNode(const RRTNode& n) {
-                           this->x(n.x());
-                           this->y(n.y());
-                           this->h(n.h());
-                           this->sp(n.sp());
-                           this->st(n.st());
-                       }
-                       // override
-                       DijkstraNode *p_ = nullptr;
-                       DijkstraNode *p() const { return this->p_; }
-                       void p(DijkstraNode *p) { this->p_ = p; }
-       };
-       class DijkstraNodeComparator {
-               public:
-                       int operator() (
-                               const DijkstraNode &n1,
-                               const DijkstraNode &n2
-                       )
-                       {
-                               return n1.cc > n2.cc;
+       this->v = true;
+       return false;
+}
+
+int
+RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
+               DijkstraNode const& n2)
+{
+       return n1.node->cc() > n2.node->cc();
+}
+
+void
+RRTExt13::pick_interesting()
+{
+       this->dn_.clear();
+       this->dn_.reserve(this->path_.size());
+       //for (auto n: this->opath_) {
+       //      this->dn_.push_back(DijkstraNode(n));
+       //      this->dn_.back().i = this->dn_.size() - 1;
+       //}
+       //return;
+       for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
+       //unsigned int i = 0; {
+               RRTNode& ni = *this->opath_[i];
+               this->bc_.set_pose(ni);
+               for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
+                       RRTNode& nj = *this->opath_[j];
+                       RRTNode& njl = *this->opath_[j - 1];
+                       if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())
+                                       || njl.edist(nj) < this->eta_) {
+                               this->dn_.push_back(DijkstraNode(&njl));
+                               this->dn_.back().i = this->dn_.size() - 1;
                        }
-       };
-       std::vector<DijkstraNode> dn;
-       unsigned int ops = this->orig_path().size();
-       auto op = this->orig_path();
-       dn.reserve(ops);
-       unsigned int dncnt = 0;
-       dn.push_back(DijkstraNode(*this->orig_path().front()));
-       dn.back().cc = this->orig_path().front()->cc;
-       dn.back().s = &dn.back();
-       dn.back().n = this->orig_path().front();
-       dn.back().i = dncnt++;
-       for (unsigned int i = 0; i < ops - 1; i++) {
-               auto ibc = BicycleCar();
-               ibc.x(op[i]->x());
-               ibc.y(op[i]->y());
-               ibc.h(op[i]->h());
-               for (unsigned int j = i + 1; j < ops; j++) {
-                       auto jbc = BicycleCar();
-                       jbc.x(op[j]->x());
-                       jbc.y(op[j]->y());
-                       jbc.h(op[j]->h());
-                       if (!jbc.drivable(ibc)) {
-                               dn.push_back(DijkstraNode(*op[j-1]));
-                               dn.back().cc = op[j-1]->cc;
-                               dn.back().s = &dn.back();
-                               dn.back().n = op[j-1];
-                               dn.back().i = dncnt++;
+                       if (!this->bc_.drivable(nj)) {
+                               this->dn_.push_back(DijkstraNode(&njl));
+                               this->dn_.back().i = this->dn_.size() - 1;
                                i = j;
                                break;
                        }
                }
        }
-       dn.push_back(DijkstraNode(*this->orig_path().back()));
-       dn.back().cc = this->orig_path().back()->cc;
-       dn.back().s = &dn.back();
-       dn.back().n = this->orig_path().back();
-       dn.back().i = dncnt++;
-       std::priority_queue<
-               DijkstraNode,
-               std::vector<DijkstraNode>,
-               DijkstraNodeComparator
-       > pq;
-       dn.front().vi();
-       pq.push(dn.front());
+       this->dn_.push_back(DijkstraNode(this->opath_.back()));
+       this->dn_.back().i = this->dn_.size() - 1;
+}
+
+void
+RRTExt13::dijkstra_forward()
+{
+       std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
+               DijkstraNodeComparator> pq;
+       this->dn_.front().vi();
+       pq.push(this->dn_.front());
        while (!pq.empty()) {
-               DijkstraNode f = pq.top();
+               DijkstraNode fd = pq.top();
+               RRTNode& f = *fd.node;
                pq.pop();
-               for (unsigned int i = f.i + 1; i < dncnt; i++) {
-                       double cost = f.cc + this->cost_search(f, dn[i]);
-                       this->steer(f, dn[i]);
-                       if (this->steered().size() == 0)
-                               break; // TODO why not continue?
-                       if (std::get<0>(this->collide_steered_from(f)))
+               for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
+                       RRTNode& t = *this->dn_[i].node;
+                       double cost = f.cc() + this->cost_build(f, t);
+                       this->steer(f, t);
+                       if (this->steered_.size() == 0) {
+                               break;
+                       }
+                       if (this->collide_steered()) {
                                continue;
-                       if (cost < dn[i].cc) {
-                               dn[i].cc = cost;
-                               dn[i].p(f.s);
-                               if (!dn[i].vi())
-                                       pq.push(dn[i]);
+                       }
+                       this->bc_.set_pose(this->steered_.back());
+                       bool td = this->bc_.drivable(t);
+                       bool tn = this->bc_.edist(t) < this->eta_;
+                       if (cost < t.cc() && td && tn) {
+                               this->join_steered(&f);
+                               t.p(this->nodes_.back());
+                               t.c(this->cost_build(this->nodes_.back(), t));
+                               if (!this->dn_[i].vi()) {
+                                       pq.push(this->dn_[i]);
+                               }
                        }
                }
        }
-       DijkstraNode *ln = nullptr;
-       for (auto &n: dn) {
-               if (n.v)
-                       ln = &n;
-       }
-       if (ln == nullptr || ln->p() == nullptr)
-               return;
-       while (ln->p() != nullptr) {
-               RRTNode *t = ln->n;
-               RRTNode *f = ln->p()->n;
-               this->steer(*f, *t);
-               if (std::get<0>(this->collide_steered_from(*f)))
-                       return;
-               this->join_steered(f);
-               t->p(&this->nodes().back());
-               t->c(this->cost_build(this->nodes().back(), *t));
-               ln = ln->p();
-       }
-       RRTS::compute_path();
 }
 
-void RRTExt13::second_path_optimization()
+void
+RRTExt13::dijkstra_backward()
 {
-       if (this->first_optimized_path().size() == 0) {
-               return;
-       }
-       class DijkstraNode : public RRTNode {
-               public:
-                       DijkstraNode *s = nullptr;
-                       RRTNode *n = nullptr;
-                       unsigned int i = 0;
-                       double cc = 9999;
-                       bool v = false;
-                       bool vi()
-                       {
-                               if (this->v)
-                                       return true;
-                               this->v = true;
-                               return false;
-                       }
-                       DijkstraNode(const RRTNode& n) {
-                           this->x(n.x());
-                           this->y(n.y());
-                           this->h(n.h());
-                           this->sp(n.sp());
-                           this->st(n.st());
-                       }
-                       // override
-                       DijkstraNode *p_ = nullptr;
-                       DijkstraNode *p() const { return this->p_; }
-                       void p(DijkstraNode *p) { this->p_ = p; }
-       };
-       class DijkstraNodeComparator {
-               public:
-                       int operator() (
-                               const DijkstraNode &n1,
-                               const DijkstraNode &n2
-                       )
-                       {
-                               return n1.cc > n2.cc;
-                       }
-       };
-       std::vector<DijkstraNode> dn;
-       unsigned int ops = this->orig_path().size();
-       auto op = this->orig_path();
-       dn.reserve(ops);
-       unsigned int dncnt = 0;
-       dn.push_back(DijkstraNode(*this->orig_path().front()));
-       dn.back().cc = this->orig_path().front()->cc;
-       dn.back().s = &dn.back();
-       dn.back().n = this->orig_path().front();
-       dn.back().i = dncnt++;
-       for (unsigned int i = ops - 1; i > 1; i--) {
-               auto ibc = BicycleCar();
-               ibc.x(op[i]->x());
-               ibc.y(op[i]->y());
-               ibc.h(op[i]->h());
-               for (unsigned int j = i - 1; j > 0; j--) {
-                       auto jbc = BicycleCar();
-                       jbc.x(op[j]->x());
-                       jbc.y(op[j]->y());
-                       jbc.h(op[j]->h());
-                       if (!jbc.drivable(ibc)) {
-                               dn.push_back(DijkstraNode(*op[j-1]));
-                               dn.back().cc = op[j-1]->cc;
-                               dn.back().s = &dn.back();
-                               dn.back().n = op[j-1];
-                               dn.back().i = dncnt++;
-                               i = j;
-                               break;
-                       }
-               }
-       }
-       dn.push_back(DijkstraNode(*this->orig_path().back()));
-       dn.back().cc = this->orig_path().back()->cc;
-       dn.back().s = &dn.back();
-       dn.back().n = this->orig_path().back();
-       dn.back().i = dncnt++;
-       std::priority_queue<
-               DijkstraNode,
-               std::vector<DijkstraNode>,
-               DijkstraNodeComparator
-       > pq;
-       // TODO rewrite
-       dn.back().vi();
-       pq.push(dn.back());
+       std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
+               DijkstraNodeComparator> pq;
+       this->dn_.back().vi();
+       pq.push(this->dn_.back());
        while (!pq.empty()) {
-               DijkstraNode t = pq.top();
+               DijkstraNode td = pq.top();
+               RRTNode& t = *td.node;
                pq.pop();
-               for (unsigned int i = t.i - 1; i > 0; i--) {
-                       double cost = dn[i].cc + this->cost_search(dn[i], t);
-                       this->steer(dn[i], t);
-                       if (this->steered().size() == 0)
-                               break; // TODO why not continue?
-                       if (std::get<0>(this->collide_steered_from(dn[i])))
+               for (unsigned int i = td.i - 1; i > 0; i--) {
+                       RRTNode& f = *this->dn_[i].node;
+                       double cost = f.cc() + this->cost_build(f, t);
+                       this->steer(f, t);
+                       if (this->steered_.size() == 0) {
+                               break;
+                       }
+                       if (this->collide_steered()) {
                                continue;
-                       if (cost < t.cc) {
-                               t.cc = cost;
-                               t.p(dn[i].s);
-                               if (!dn[i].vi())
-                                       pq.push(dn[i]);
+                       }
+                       this->bc_.set_pose(this->steered_.back());
+                       bool td = this->bc_.drivable(t);
+                       bool tn = this->bc_.edist(t) < this->eta_;
+                       if (cost < t.cc() && td && tn) {
+                               this->join_steered(&f);
+                               t.p(this->nodes_.back());
+                               t.c(this->cost_build(this->nodes_.back(), t));
+                               if (!this->dn_[i].vi()) {
+                                       pq.push(this->dn_[i]);
+                               }
                        }
                }
        }
-       DijkstraNode *fn = nullptr;
-       for (auto &n: dn) {
-               if (n.v) {
-                       fn = &n;
-                       break;
-               }
+}
+
+void
+RRTExt13::compute_path()
+{
+       RRTS::compute_path();
+       bool measure_time = false;
+       if (this->opath_.size() == 0) {
+               this->opath_ = this->path_;
+               this->ogoal_cc_ = this->goal_.cc();
+               measure_time = true;
        }
-       if (fn == nullptr || fn->p() == nullptr)
-               return;
-       DijkstraNode *ln = &dn.back();
-       while (ln->p() != fn) {
-               RRTNode *t = ln->n;
-               RRTNode *f = ln->p()->n;
-               this->steer(*f, *t);
-               if (std::get<0>(this->collide_steered_from(*f)))
-                       return;
-               this->join_steered(f);
-               t->p(&this->nodes().back());
-               t->c(this->cost_build(this->nodes().back(), *t));
-               ln = ln->p();
+       if (measure_time) {
+               this->otime_ = -this->ter_.scnt();
        }
+       this->pick_interesting();
+       this->dijkstra_forward();
+       this->pick_interesting();
+       this->dijkstra_backward();
        RRTS::compute_path();
+       if (measure_time) {
+               this->otime_ += this->ter_.scnt();
+       }
 }
 
-void RRTExt13::compute_path()
+RRTExt13::RRTExt13()
 {
-       RRTS::compute_path();
-       auto tstart = std::chrono::high_resolution_clock::now();
-       this->first_path_optimization();
-       this->first_optimized_path_ = RRTS::path();
-       if (this->first_optimized_path_.size() > 0) {
-               this->first_optimized_path_cost(
-                       this->first_optimized_path_.back()->cc
-               );
-       } else {
-               return;
-       }
-       this->second_path_optimization();
-       auto tend = std::chrono::high_resolution_clock::now();
-       auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
-           tend - tstart);
-       this->log_opt_time_.push_back(tdiff.count());
+       this->opath_.reserve(10000);
+       this->dn_.reserve(10000);
 }
 
-Json::Value RRTExt13::json()
+Json::Value
+RRTExt13::json() const
 {
-       Json::Value jvo = RRTS::json();
-       jvo["orig_path_cost"] = this->orig_path_cost();
-       {
-               unsigned int cu = 0;
-               unsigned int co = 0;
-               unsigned int pcnt = 0;
-               for (auto n: this->orig_path()) {
-                       jvo["orig_path"][pcnt][0] = n->x();
-                       jvo["orig_path"][pcnt][1] = n->y();
-                       jvo["orig_path"][pcnt][2] = n->h();
-                       if (n->t(RRTNodeType::cusp))
-                               cu++;
-                       if (n->t(RRTNodeType::connected))
-                               co++;
-                       pcnt++;
-               }
-               jvo["orig_cusps-in-path"] = cu;
-               jvo["orig_connecteds-in-path"] = co;
+       auto jvo = RRTS::json();
+       unsigned int i = 0;
+       for (auto n: this->opath_) {
+               jvo["opath"][i][0] = n->x();
+               jvo["opath"][i][1] = n->y();
+               jvo["opath"][i][2] = n->h();
+               i++;
        }
+       jvo["ogoal_cc"] = this->ogoal_cc_;
+       jvo["otime"] = this->otime_;
        return jvo;
 }
 
-void RRTExt13::json(Json::Value jvi)
+void
+RRTExt13::json(Json::Value jvi)
 {
-       return RRTS::json(jvi);
+       RRTS::json(jvi);
 }
+
+void
+RRTExt13::reset()
+{
+       RRTS::reset();
+       this->opath_.clear();
+       this->ogoal_cc_ = 0.0;
+       this->otime_ = 0.0;
+}
+
+} // namespace rrts