When reset, reset bc, too

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

diff --git a/src/rrts.cc b/src/rrts.cc
index 191bf7fbfd7f3449c2ea53297946419d4ced6402..03787b133d2c2d139dc985541debe5c1d8e5481a 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -1,7 +1,17 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 #include <algorithm>
 #include <cassert>
 #include "rrts.hh"
 
+#ifndef USE_RRTS
+#define USE_RRTS 0  // TODO improve, this solution isn't clear.
+#endif
+
 namespace rrts {
 
 void
@@ -53,6 +63,21 @@ RRTNode::p(RRTNode& p)
        }
 }
 
+unsigned int
+RRTNode::cusp() const
+{
+       return this->cusp_;
+}
+
+void
+RRTNode::cusp(RRTNode const& p)
+{
+       this->cusp_ = p.cusp();
+       if (this->sp() != p.sp() || this->sp() == 0.0) {
+               this->cusp_++;
+       }
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -60,10 +85,9 @@ RRTNode::operator==(RRTNode const& n)
 }
 
 void
-RRTS::recompute_path_cc()
+RRTS::recompute_cc(RRTNode* g)
 {
        this->path_.clear();
-       RRTNode* g = &this->goal_;
        while (g != nullptr) {
                this->path_.push_back(g);
                g = g->p();
@@ -75,6 +99,12 @@ RRTS::recompute_path_cc()
        }
 }
 
+void
+RRTS::recompute_path_cc()
+{
+       this->recompute_cc(&this->goal_);
+}
+
 double
 RRTS::min_gamma_eta() const
 {
@@ -97,6 +127,7 @@ RRTS::join_steered(RRTNode* f)
                RRTNode* t = &this->nodes_.back();
                t->p(*f);
                t->c(this->cost_build(*f, *t));
+               t->cusp(*f);
                this->steered_.erase(this->steered_.begin());
                f = t;
        }
@@ -113,6 +144,7 @@ RRTS::connect()
 {
        RRTNode* f = this->nn_;
        RRTNode* t = &this->steered_.front();
+#if USE_RRTS
        double cost = f->cc() + this->cost_build(*f, *t);
        for (auto n: this->nv_) {
                double nc = n->cc() + this->cost_build(*n, *t);
@@ -128,10 +160,12 @@ RRTS::connect()
        if (!this->bc_.drivable(*t)) {
                f = this->nn_;
        }
+#endif
        this->store(this->steered_.front());
        t = &this->nodes_.back();
        t->p(*f);
        t->c(this->cost_build(*f, *t));
+       t->cusp(*f);
        this->steered_.erase(this->steered_.begin());
        return true;
 }
@@ -223,7 +257,7 @@ RRTS::compute_path()
        std::reverse(this->path_.begin(), this->path_.end());
 }
 
-RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
+RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}())
 {
        this->nodes_.reserve(4000000);
        this->steered_.reserve(1000);
@@ -232,6 +266,48 @@ RRTS::RRTS() : gen_(std::random_device{}()), goal_(0.0, 0.0, 0.0, 0.0)
        this->store(RRTNode()); // root
 }
 
+BicycleCar &
+RRTS::bc()
+{
+       return this->bc_;
+}
+
+void
+RRTS::set_imax_reset(unsigned int i)
+{
+       this->_imax = i;
+}
+
+void
+RRTS::set_goal(double x, double y, double b, double e)
+{
+       this->goal_ = RRTGoal(x, y, b, e);
+}
+
+void
+RRTS::set_start(double x, double y, double h)
+{
+       this->nodes_.front().x(x);
+       this->nodes_.front().y(y);
+       this->nodes_.front().h(h);
+}
+
+std::vector<Pose>
+RRTS::get_path() const
+{
+       std::vector<Pose> path;
+       for (auto n: this->path_) {
+               path.push_back(Pose(n->x(), n->y(), n->h()));
+       }
+       return path;
+}
+
+double
+RRTS::get_path_cost() const
+{
+       return this->goal_.cc();
+}
+
 unsigned int
 RRTS::icnt() const
 {
@@ -250,6 +326,18 @@ RRTS::scnt() const
        return this->ter_.scnt();
 }
 
+double
+RRTS::eta() const
+{
+       return this->eta_;
+}
+
+void
+RRTS::eta(double e)
+{
+       this->eta_ = e;
+}
+
 Json::Value
 RRTS::json() const
 {
@@ -271,7 +359,6 @@ RRTS::json(Json::Value jvi)
 {
        assert(jvi["init"] != Json::nullValue);
        assert(jvi["goal"] != Json::nullValue);
-       assert(jvi["obst"] != Json::nullValue);
        this->nodes_.front().x(jvi["init"][0].asDouble());
        this->nodes_.front().y(jvi["init"][1].asDouble());
        this->nodes_.front().h(jvi["init"][2].asDouble());
@@ -310,11 +397,15 @@ RRTS::next()
        if (this->collide_steered()) {
                return this->should_continue();
        }
+#if USE_RRTS
        this->find_nv(this->steered_.front());
+#endif
        if (!this->connect()) {
                return this->should_continue();
        }
+#if USE_RRTS
        this->rewire();
+#endif
        unsigned int ss = this->steered_.size();
        this->join_steered(&this->nodes_.back());
        RRTNode* just_added = &this->nodes_.back();
@@ -367,6 +458,9 @@ RRTS::reset()
        this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());
        this->nv_.clear();
        this->nn_ = nullptr;
+       this->bc_.x(0);
+       this->bc_.y(0);
+       this->bc_.h(0);
 }
 
 } // namespace rrts