Add path and path cost getters

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 7b730c1af6dbb5fc02db93781ea2293c8ab082af..e0930d565a8338c7b0f3beb136ca00c86ca69fe6 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -120,6 +120,12 @@ public:
        /*! Set start. */
        void set_start(double x, double y, double h);
 
+       /*! Get path. */
+       std::vector<Pose> get_path() const;
+
+       /*! Get path cost. */
+       double get_path_cost() const;
+
        /*! Get iterations counter. */
        unsigned int icnt() const;
 

diff --git a/src/rrts.cc b/src/rrts.cc
index 46f289eb2b08f68ae69893e70f7a68895b01409f..1d66b3cda5e04e66b2125c9c127244790887b089 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -292,6 +292,22 @@ RRTS::set_start(double x, double y, double h)
        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
 {