When reset, reset bc, too

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

diff --git a/src/rrts.cc b/src/rrts.cc
index 8e175a27ff787335c10337175a3c24c498c6bd7f..03787b133d2c2d139dc985541debe5c1d8e5481a 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -278,6 +278,36 @@ 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
 {
@@ -296,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
 {
@@ -416,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