Fix forward dijkstra algorithm

[hubacji1/rrts.git] / src / rrtext13.cc

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 03ff7148425b03e247f9bc96a7218ce9ee8726c1..5be98881726a428952e24f8f00312102ad546597 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,7 +1,6 @@
+#include <algorithm>
 #include <queue>
 #include "rrtext.hh"
-#include <iostream>
-using namespace std;
 
 namespace rrts {
 
@@ -27,37 +26,80 @@ RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
 }
 
 void
-RRTExt13::pick_interesting()
+RRTExt13::interesting_forward()
 {
        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;
+#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;
+       }
+       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; {
                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
+       this->dn_.push_back(DijkstraNode(this->opath_.back()));
+       this->dn_.back().i = this->dn_.size() - 1;
+       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;
                                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_.push_back(DijkstraNode(this->opath_.front()));
        this->dn_.back().i = this->dn_.size() - 1;
+#endif
 }
 
 void
@@ -78,12 +120,14 @@ RRTExt13::dijkstra_forward()
                        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());
@@ -136,6 +180,15 @@ void
 RRTExt13::compute_path()
 {
        RRTS::compute_path();
+       if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
+               return;
+       }
+#if 0 // TODO 0.59 should work for sc4-1-0 only.
+       if (this->goal_.cc() * 0.59 > this->last_goal_cc_
+                       && this->last_goal_cc_ != 0.0) {
+               return;
+       }
+#endif
        bool measure_time = false;
        if (this->opath_.size() == 0) {
                this->opath_ = this->path_;
@@ -145,11 +198,19 @@ RRTExt13::compute_path()
        if (measure_time) {
                this->otime_ = -this->ter_.scnt();
        }
-       this->pick_interesting();
-       this->dijkstra_forward();
-       this->pick_interesting();
-       this->dijkstra_backward();
-       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();
        }
@@ -188,8 +249,6 @@ RRTExt13::reset()
 {
        RRTS::reset();
        this->opath_.clear();
-       this->ogoal_cc_ = 0.0;
-       this->otime_ = 0.0;
 }
 
 } // namespace rrts