Add reset method, remove debug print

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

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index 8077b7e8f390ff6aa05044467383f24de2fdd76a..f2d342ee476ee89e4ef68a6284918e6734046c06 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -1,6 +1,15 @@
 #include <queue>
 #include "rrtext.h"
 
+void RRTExt3::reset()
+{
+        RRTS::reset();
+        this->orig_path().clear();
+        this->first_optimized_path().clear();
+        this->orig_path_cost_ = 9999.9;
+        this->first_optimized_path_cost_ = 9999.9;
+}
+
 void RRTExt3::first_path_optimization()
 {
         if (this->orig_path().size() == 0) {