Add recompute cc method

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

diff --git a/src/rrts.cc b/src/rrts.cc
index 09e65ed9a4d92647a04872bc52554101a0f3abf3..d2c93180f99d2000e4c07d156e88520d5cffb910 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -59,6 +59,27 @@ RRTNode::operator==(RRTNode const& n)
        return this == &n;
 }
 
+void
+RRTS::recompute_cc(RRTNode* g)
+{
+       this->path_.clear();
+       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]));
+       }
+}
+
+void
+RRTS::recompute_path_cc()
+{
+       this->recompute_cc(&this->goal_);
+}
+
 double
 RRTS::min_gamma_eta() const
 {
@@ -207,7 +228,7 @@ RRTS::compute_path()
        std::reverse(this->path_.begin(), this->path_.end());
 }
 
-RRTS::RRTS() : gen_(std::random_device{}())
+RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
 {
        this->nodes_.reserve(4000000);
        this->steered_.reserve(1000);
@@ -259,13 +280,16 @@ RRTS::json(Json::Value jvi)
        this->nodes_.front().x(jvi["init"][0].asDouble());
        this->nodes_.front().y(jvi["init"][1].asDouble());
        this->nodes_.front().h(jvi["init"][2].asDouble());
-       this->goal_.x(jvi["goal"][0].asDouble());
-       this->goal_.y(jvi["goal"][1].asDouble());
-       this->goal_.b(jvi["goal"][2].asDouble());
        if (jvi["goal"].size() == 4) {
-               this->goal_.e(jvi["goal"][3].asDouble());
+               this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+                       jvi["goal"][1].asDouble(),
+                       jvi["goal"][2].asDouble(),
+                       jvi["goal"][3].asDouble());
        } else {
-               this->goal_.e(jvi["goal"][2].asDouble());
+               this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+                       jvi["goal"][1].asDouble(),
+                       jvi["goal"][2].asDouble(),
+                       jvi["goal"][2].asDouble());
        }
 }
 
@@ -277,6 +301,15 @@ 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_) {
+               rs = this->last_path_[rand() % this->last_path_.size()];
+       }
+}
+#endif
        this->find_nn(rs);
        this->steer(this->nn(), rs);
        if (this->collide_steered()) {
@@ -290,12 +323,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--;
@@ -313,32 +342,15 @@ 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 (gf) {
+               this->compute_path();
+       }
        this->time_ = this->ter_.scnt();
        return this->should_continue();
 }
@@ -346,7 +358,15 @@ RRTS::next()
 void
 RRTS::reset()
 {
-       this->goal_ = RRTGoal();
+       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());
        this->path_.clear();
        this->steered_.clear();
        this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());