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);
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
{
return this->ter_.scnt();
}
+double
+RRTS::eta() const
+{
+ return this->eta_;
+}
+
+void
+RRTS::eta(double e)
+{
+ this->eta_ = e;
+}
+
Json::Value
RRTS::json() const
{