Merge branch 'refactor-and-tuning'

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 0e12a5c4dc52ce82295ddcdea3a312ac68525b41..a9ef93c7351a233aea7b288a48c22104f9705ae1 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -70,6 +70,7 @@ protected:
        double cost_ = 0.0;
        double eta_ = 0.5;
        double time_ = 0.0;
+       double last_goal_cc_ = 0.0;
        double min_gamma_eta() const;
        bool should_continue() const;
        void join_steered(RRTNode* f);

diff --git a/incl/rrtsp.hh b/incl/rrtsp.hh
index f136449fbc9e888830b551b2002f98e1a8667df5..2b526dcd167cd8976b0da95464d2e7fbd3ccd9b8 100644 (file)
--- a/incl/rrtsp.hh
+++ b/incl/rrtsp.hh
@@ -14,6 +14,29 @@
 
 namespace rrts {
 
+class P39 : public RRTExt2, public RRTExt8, public RRTExt10, public RRTExt14,
+               public RRTExt15, public RRTExt16, public RRTExt17,
+               public RRTExt13 {
+public:
+       Json::Value json() const
+       {
+               auto jvo = RRTExt13::json();
+               auto json15 = RRTExt15::json();
+               jvo["log_path_cost"] = json15["log_path_cost"];
+               return jvo;
+       }
+       void json(Json::Value jvi)
+       {
+               RRTExt2::json(jvi);
+       }
+       void reset()
+       {
+               RRTExt8::reset();
+               RRTExt14::reset();
+               RRTExt13::reset();
+       }
+};
+
 class P38 : public RRTExt2, public RRTExt8, public RRTExt10, public RRTExt14,
                public RRTExt15, public RRTExt16, public RRTExt18,
                public RRTExt13 {

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index 03ff7148425b03e247f9bc96a7218ce9ee8726c1..602788674d2f4dc5d20d573b4abae51e00cca122 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,7 +1,5 @@
 #include <queue>
 #include "rrtext.hh"
-#include <iostream>
-using namespace std;
 
 namespace rrts {
 
@@ -31,13 +29,16 @@ 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;
+#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
        for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
-       //unsigned int i = 0; {
+       //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++) {
@@ -58,6 +59,7 @@ RRTExt13::pick_interesting()
        }
        this->dn_.push_back(DijkstraNode(this->opath_.back()));
        this->dn_.back().i = this->dn_.size() - 1;
+#endif
 }
 
 void
@@ -136,6 +138,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_;
@@ -147,8 +158,11 @@ RRTExt13::compute_path()
        }
        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();
        if (measure_time) {
                this->otime_ += this->ter_.scnt();
@@ -188,8 +202,6 @@ RRTExt13::reset()
 {
        RRTS::reset();
        this->opath_.clear();
-       this->ogoal_cc_ = 0.0;
-       this->otime_ = 0.0;
 }
 
 } // namespace rrts

diff --git a/src/rrts.cc b/src/rrts.cc
index 100c8cc243d770b4e979a1b5aefb3b0cc61155be..f96cef47f8854b645fc889b68544b4aa60dfe672 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -280,6 +280,16 @@ RRTS::next()
        }
        this->icnt_ += 1;
        auto rs = this->sample();
+#if 1 // anytime RRTs
+{
+       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();
+       }
+}
+#endif
        this->find_nn(rs);
        this->steer(this->nn(), rs);
        if (this->collide_steered()) {
@@ -293,12 +303,8 @@ RRTS::next()
        unsigned int ss = this->steered_.size();
        this->join_steered(&this->nodes_.back());
        RRTNode* just_added = &this->nodes_.back();
+       bool gf = false;
        while (ss > 0 && just_added->p() != nullptr) {
-               //if (!this->goal_drivable_from(*just_added)) {
-               //      ss--;
-               //      just_added = just_added->p();
-               //      continue;
-               //}
                this->steer(*just_added, this->goal_);
                if (this->collide_steered()) {
                        ss--;
@@ -316,32 +322,25 @@ RRTS::next()
                                        || ncc < this->goal_.cc()) {
                                this->goal_.p(this->nodes_.back());
                                this->goal_.c(nc);
-                               this->compute_path();
+                               gf = true;
                        }
                }
                ss--;
                just_added = just_added->p();
        }
-
-       ////if (!this->goal_drivable_from(this->nodes_.back())) {
-       ////    return this->should_continue();
-       ////}
-       //this->steer(this->nodes_.back(), this->goal_);
-       //if (this->collide_steered()) {
-       //      return this->should_continue();
-       //}
-       //this->join_steered(&this->nodes_.back());
-       //bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
-       //bool gd = this->goal_drivable_from(this->nodes_.back());
-       //if (gn && gd) {
-       //      double nc = this->cost_build(this->nodes_.back(), this->goal_);
-       //      double ncc = this->nodes_.back().cc() + nc;
-       //      if (this->goal_.p() == nullptr || ncc < this->goal_.cc()) {
-       //              this->goal_.p(this->nodes_.back());
-       //              this->goal_.c(nc);
-       //              this->compute_path();
-       //      }
-       //}
+#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();
+       }
        this->time_ = this->ter_.scnt();
        return this->should_continue();
 }
@@ -349,6 +348,9 @@ RRTS::next()
 void
 RRTS::reset()
 {
+       if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
+               this->last_goal_cc_ = this->goal_.cc();
+       }
        this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
                this->goal_.e());
        this->path_.clear();