Add path and path cost getters

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

diff --git a/src/rrts.cc b/src/rrts.cc
index e08fed97acef4c9c42a4a861b785fc6a5ffd5a25..1d66b3cda5e04e66b2125c9c127244790887b089 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -257,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);
@@ -266,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
 {