From dea0da28157be7c03db223bc78d773cb43f56c47 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 12 Jul 2021 13:10:16 +0200
Subject: [PATCH] Add reset method

---
 api/rrts.h  |  1 +
 src/rrts.cc | 23 +++++++++++++++++++++++
 2 files changed, 24 insertions(+)

diff --git a/api/rrts.h b/api/rrts.h
index 8fa3fb7..24a10c4 100644
--- 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 a309bc3..5a40b08 100644
--- 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();
-- 
2.39.2