Use interesting forward/backward methods

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 69f9175b204752a28b95ad13d49ecad320518f07..038677f7638132fceb34d64216274bd34d410dd3 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -115,7 +115,8 @@ private:
        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/src/rrtext13.cc b/src/rrtext13.cc
index 5545c95f40ad84aa466a4b052818f4d572730401..1917ddfefb6e1f396547b955755dfa145df9cf4c 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,3 +1,4 @@
+#include <algorithm>
 #include <queue>
 #include "rrtext.hh"
 
@@ -25,7 +26,7 @@ RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
 }
 
 void
-RRTExt13::pick_interesting()
+RRTExt13::interesting_forward()
 {
        this->dn_.clear();
        this->dn_.reserve(this->path_.size());
@@ -62,6 +63,45 @@ RRTExt13::pick_interesting()
 #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_.front()));
+       this->dn_.back().i = this->dn_.size() - 1;
+#endif
+}
+
 void
 RRTExt13::dijkstra_forward()
 {