Merge branch 'fix/dijkstra-backward-opt'

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 69f9175b204752a28b95ad13d49ecad320518f07..6842312a2a32a31338d47fbd66b172ac4659d39a 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -104,6 +104,7 @@ private:
                RRTNode* node = nullptr;
                unsigned int i = 0;
                bool v = false;
+               double d = 0.0;
                bool vi();
                DijkstraNode(RRTNode* n);
        };
@@ -111,11 +112,16 @@ private:
        public:
                int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
        };
+       class DijkstraNodeBackwardComparator {
+       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 interesting_forward();
+       void interesting_backward();
        void dijkstra_forward();
        void dijkstra_backward();
        void compute_path();

diff --git a/incl/rrts.hh b/incl/rrts.hh
index a9ef93c7351a233aea7b288a48c22104f9705ae1..9c847ed0b075addf807dd06ce9b571eae6a4935b 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -71,6 +71,8 @@ protected:
        double eta_ = 0.5;
        double time_ = 0.0;
        double last_goal_cc_ = 0.0;
+       std::vector<RRTNode> last_path_;
+       void recompute_path_cc();
        double min_gamma_eta() const;
        bool should_continue() const;
        void join_steered(RRTNode* f);

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 602788674d2f4dc5d20d573b4abae51e00cca122..f6dd77473d014baa673fe3042ad1ab4222bdd634 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,3 +1,4 @@
+#include <algorithm>
 #include <queue>
 #include "rrtext.hh"
 
@@ -24,8 +25,15 @@ RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
        return n1.node->cc() > n2.node->cc();
 }
 
+int
+RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
+               DijkstraNode const& n2)
+{
+       return n1.d > n2.d;
+}
+
 void
-RRTExt13::pick_interesting()
+RRTExt13::interesting_forward()
 {
        this->dn_.clear();
        this->dn_.reserve(this->path_.size());
@@ -37,28 +45,72 @@ RRTExt13::pick_interesting()
        return;
 #endif
 #if 1 // cusp and 1st non-drivable path poses are interesting
+       this->dn_.push_back(DijkstraNode(this->opath_.front()));
+       this->dn_.back().i = this->dn_.size() - 1;
        for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
-       //unsigned int i = 0; { // just for cusp, comment out drivable section
                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_) {
+                       if (!this->bc_.drivable(nj)) {
+                               this->dn_.push_back(DijkstraNode(&njl));
+                               this->dn_.back().i = this->dn_.size() - 1;
+                               i = j;
+                               break;
+                       }
+                       if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
                                this->dn_.push_back(DijkstraNode(&njl));
                                this->dn_.back().i = this->dn_.size() - 1;
                        }
+               }
+       }
+       this->dn_.push_back(DijkstraNode(this->opath_.back()));
+       this->dn_.back().i = this->dn_.size() - 1;
+#endif
+}
+
+void
+RRTExt13::interesting_backward()
+{
+       this->dn_.clear();
+       this->dn_.reserve(this->path_.size());
+#if 0 // all path poses are interesting
+       for (auto n: this->opath_) {
+               this->dn_.push_back(DijkstraNode(n));
+               this->dn_.back().i = this->dn_.size() - 1;
+       }
+       std::reverse(this->dn_.begin(), this->dn_.end());
+       return;
+#endif
+#if 1 // cusp and 1st non-drivable path poses are interesting
+       double goal_cc = this->goal_.cc();
+       this->dn_.push_back(DijkstraNode(this->opath_.back()));
+       this->dn_.back().i = this->dn_.size() - 1;
+       this->dn_.back().d = goal_cc - this->opath_.back()->cc();
+       for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
+               RRTNode& ni = *this->opath_[i];
+               this->bc_.set_pose(ni);
+               for (unsigned int j = i - 1; j > 0; j--) {
+                       RRTNode& nj = *this->opath_[j];
+                       RRTNode& njl = *this->opath_[j + 1];
                        if (!this->bc_.drivable(nj)) {
                                this->dn_.push_back(DijkstraNode(&njl));
                                this->dn_.back().i = this->dn_.size() - 1;
+                               this->dn_.back().d = goal_cc - njl.cc();
                                i = j;
                                break;
                        }
+                       if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+                               this->dn_.push_back(DijkstraNode(&njl));
+                               this->dn_.back().i = this->dn_.size() - 1;
+                               this->dn_.back().d = goal_cc - njl.cc();
+                       }
                }
        }
-       this->dn_.push_back(DijkstraNode(this->opath_.back()));
+       this->dn_.push_back(DijkstraNode(this->opath_.front()));
        this->dn_.back().i = this->dn_.size() - 1;
+       this->dn_.back().d = goal_cc - this->opath_.front()->cc();
 #endif
 }
 
@@ -73,19 +125,25 @@ RRTExt13::dijkstra_forward()
                DijkstraNode fd = pq.top();
                RRTNode& f = *fd.node;
                pq.pop();
+               this->bc_.set_pose(f);
                for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
                        RRTNode& t = *this->dn_[i].node;
+                       if (!this->bc_.drivable(t)) {
+                               continue;
+                       }
                        double cost = f.cc() + this->cost_build(f, t);
                        this->steer(f, t);
                        if (this->steered_.size() == 0) {
                                break;
                        }
-                       if (this->collide_steered()) {
+                       unsigned int ss = this->steered_.size();
+                       if (this->collide_steered()
+                                       || ss != this->steered_.size()) {
                                continue;
                        }
                        this->bc_.set_pose(this->steered_.back());
                        bool td = this->bc_.drivable(t);
-                       bool tn = this->bc_.edist(t) < this->eta_;
+                       bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
                        if (cost < t.cc() && td && tn) {
                                this->join_steered(&f);
                                t.p(this->nodes_.back());
@@ -103,29 +161,38 @@ RRTExt13::dijkstra_backward()
 {
        std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
                DijkstraNodeComparator> pq;
-       this->dn_.back().vi();
-       pq.push(this->dn_.back());
+       this->dn_.front().vi();
+       pq.push(this->dn_.front());
        while (!pq.empty()) {
                DijkstraNode td = pq.top();
                RRTNode& t = *td.node;
                pq.pop();
-               for (unsigned int i = td.i - 1; i > 0; i--) {
+               this->bc_.set_pose(t);
+               for (unsigned int i = td.i; i > 0; i--) {
+                       DijkstraNode& fd = this->dn_[i];
                        RRTNode& f = *this->dn_[i].node;
-                       double cost = f.cc() + this->cost_build(f, t);
+                       if (!this->bc_.drivable(f)) {
+                               continue;
+                       }
+                       double cost = td.d + this->cost_build(f, t);
                        this->steer(f, t);
                        if (this->steered_.size() == 0) {
                                break;
                        }
-                       if (this->collide_steered()) {
+                       unsigned int ss = this->steered_.size();
+                       if (this->collide_steered()
+                                       || ss != this->steered_.size()) {
                                continue;
                        }
                        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) {
+                       bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
+                       if (cost < fd.d && td && tn) {
+                               fd.d = cost;
                                this->join_steered(&f);
                                t.p(this->nodes_.back());
                                t.c(this->cost_build(this->nodes_.back(), t));
+                               this->recompute_path_cc();
                                if (!this->dn_[i].vi()) {
                                        pq.push(this->dn_[i]);
                                }
@@ -156,14 +223,19 @@ RRTExt13::compute_path()
        if (measure_time) {
                this->otime_ = -this->ter_.scnt();
        }
-       this->pick_interesting();
-       this->dijkstra_forward();
-#if 0 // TODO Fix as the code does not always finish.
-       RRTS::compute_path();
-       this->pick_interesting();
-       this->dijkstra_backward();
-#endif
-       RRTS::compute_path();
+       double curr_cc = this->goal_.cc();
+       double last_cc = curr_cc + 1.0;
+       while (curr_cc < last_cc) {
+               last_cc = curr_cc;
+               RRTS::compute_path();
+               this->interesting_forward();
+               this->dijkstra_forward();
+               RRTS::compute_path();
+               this->interesting_backward();
+               this->dijkstra_backward();
+               RRTS::compute_path();
+               curr_cc = this->goal_.cc();
+       }
        if (measure_time) {
                this->otime_ += this->ter_.scnt();
        }

diff --git a/src/rrts.cc b/src/rrts.cc
index f96cef47f8854b645fc889b68544b4aa60dfe672..191bf7fbfd7f3449c2ea53297946419d4ced6402 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -59,6 +59,22 @@ RRTNode::operator==(RRTNode const& n)
        return this == &n;
 }
 
+void
+RRTS::recompute_path_cc()
+{
+       this->path_.clear();
+       RRTNode* g = &this->goal_;
+       while (g != nullptr) {
+               this->path_.push_back(g);
+               g = g->p();
+       }
+       std::reverse(this->path_.begin(), this->path_.end());
+       for (unsigned int i = 1; i < this->path_.size(); i++) {
+               this->path_[i]->c(this->cost_build(*this->path_[i - 1],
+                       *this->path_[i]));
+       }
+}
+
 double
 RRTS::min_gamma_eta() const
 {
@@ -285,8 +301,7 @@ RRTS::next()
        double d1 = this->cost_search(this->nodes_.front(), rs);
        double d2 = this->cost_search(rs, this->goal_);
        if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
-               this->icnt_ -= 1;
-               return this->should_continue();
+               rs = this->last_path_[rand() % this->last_path_.size()];
        }
 }
 #endif
@@ -328,16 +343,6 @@ RRTS::next()
                ss--;
                just_added = just_added->p();
        }
-#if 1 // test if root -> just_added can be steered directly
-       this->steer(this->nodes_.front(), *just_added);
-       ss = this->steered_.size();
-       if (!this->collide_steered() && this->steered_.size() == ss) {
-               this->join_steered(&this->nodes_.front());
-               just_added->p(this->nodes_.back());
-               just_added->c(this->cost_build(this->nodes_.back(),
-                       *just_added));
-       }
-#endif
        if (gf) {
                this->compute_path();
        }
@@ -350,6 +355,10 @@ RRTS::reset()
 {
        if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
                this->last_goal_cc_ = this->goal_.cc();
+               this->last_path_.clear();
+               for (auto n: this->path_) {
+                       this->last_path_.push_back(*n);
+               }
        }
        this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
                this->goal_.e());