Add reset method

diff --git a/api/rrts.h b/api/rrts.h
index 8fa3fb773ebf9c61547e15eea5fdb0186a6bd402..24a10c47a14b358ff8541252b63b6cd4594f061d 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -234,6 +234,7 @@ class RRTS {
                 /*! \brief Initialize RRT algorithm if needed.
                 */
                 virtual void init();
+                virtual void reset();
                 /*! \brief Deinitialize RRT algorithm if needed.
                 */
                 virtual void deinit();

diff --git a/src/rrts.cc b/src/rrts.cc
index a309bc3065daa0709d4fca22a25ade43fefcd3ed..5a40b085cf05985b6df29c96405b35098f3c9bde 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -488,6 +488,29 @@ void RRTS::init()
 {
 }
 
+void RRTS::reset()
+{
+        RRTNode init = RRTNode();
+        init.x(this->nodes().front().x());
+        init.y(this->nodes().front().y());
+        init.h(this->nodes().front().h());
+        this->nodes().clear();
+        this->store_node(RRTNode());
+        this->nodes().front().x(init.x());
+        this->nodes().front().y(init.y());
+        this->nodes().front().h(init.h());
+        this->samples().clear();
+        this->steered().clear();
+        this->log_opt_time_.clear();
+        this->path().clear();
+        this->gf(false);
+        for (auto& g: this->goals()) {
+                g.p(nullptr);
+                g.c_ = 0.0;
+                g.cc = 0.0;
+        }
+}
+
 void RRTS::deinit()
 {
         this->nodes().clear();