Add reset method, remove debug print

diff --git a/api/rrtce.h b/api/rrtce.h
index 439b9361945e85501f4feba7937fcd81bcfb9e59..eadd1a84a1c36ac9b3d7044a48c93b42ca61abbc 100644 (file)
--- a/api/rrtce.h
+++ b/api/rrtce.h
@@ -29,6 +29,11 @@ class RRTCE35
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt13::reset();
+                }
 };
 class RRTCE34
         : public RRTExt2
@@ -49,6 +54,11 @@ class RRTCE34
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE33
         : public RRTExt2
@@ -68,6 +78,11 @@ class RRTCE33
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE32
         : public RRTExt2
@@ -87,6 +102,11 @@ class RRTCE32
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE31
         : public RRTExt2
@@ -106,6 +126,11 @@ class RRTCE31
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE30
         : public RRTExt2
@@ -125,6 +150,11 @@ class RRTCE30
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 
 class RRTCE29
@@ -418,6 +448,11 @@ class RRTCE14
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE13
         : public RRTExt2
@@ -436,6 +471,11 @@ class RRTCE13
                         RRTExt2::deinit();
                         RRTExt8::deinit();
                 }
+                void reset()
+                {
+                    RRTExt8::reset();
+                    RRTExt3::reset();
+                }
 };
 class RRTCE12
         : public RRTExt2

diff --git a/api/rrtext.h b/api/rrtext.h
index 406afbb31cdbac6535d524ace860836bec80a025..4e1f825cbef208dfc7488f04e7f6fc7ba4831db5 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -20,6 +20,7 @@
 class RRTExt13 : public virtual RRTS {
         private:
         public:
+                void reset();
                 std::vector<RRTNode *> orig_path_;
                 double orig_path_cost_ = 9999;
                 std::vector<RRTNode *> first_optimized_path_;
@@ -173,6 +174,7 @@ class RRTExt8 : public virtual RRTS {
                     this->kdroot_ = nullptr;
                 }
                 void init();
+                void reset();
                 void deinit();
                 void store_node(RRTNode n);
                 RRTNode *nn(RRTNode &t);
@@ -287,6 +289,7 @@ class RRTExt4 : public virtual RRTS {
 class RRTExt3 : public virtual RRTS {
         private:
         public:
+                void reset();
                 std::vector<RRTNode *> orig_path_;
                 double orig_path_cost_ = 9999;
                 std::vector<RRTNode *> first_optimized_path_;

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index d0db3d119d6bd5bac81354d256899e032ba549dd..563432e7116dc165c4bc002b52b9bf62998a20f4 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -1,6 +1,15 @@
 #include <queue>
 #include "rrtext.h"
-#include <iostream>
+
+void RRTExt13::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 RRTExt13::first_path_optimization()
 {
         if (this->orig_path().size() == 0) {
@@ -67,10 +76,6 @@ void RRTExt13::first_path_optimization()
                         jbc.y(op[j]->y());
                         jbc.h(op[j]->h());
                         if (!jbc.drivable(ibc)) {
-                                //std::cerr<<*op[i];
-                                //std::cerr<<" to ";
-                                //std::cerr<<*op[j];
-                                //std::cerr<<std::endl;
                                 dn.push_back(DijkstraNode(*op[j-1]));
                                 dn.back().cc = op[j-1]->cc;
                                 dn.back().s = &dn.back();
@@ -194,10 +199,6 @@ void RRTExt13::second_path_optimization()
                         jbc.y(op[j]->y());
                         jbc.h(op[j]->h());
                         if (!jbc.drivable(ibc)) {
-                                //std::cerr<<*op[i];
-                                //std::cerr<<" to ";
-                                //std::cerr<<*op[j];
-                                //std::cerr<<std::endl;
                                 dn.push_back(DijkstraNode(*op[j-1]));
                                 dn.back().cc = op[j-1]->cc;
                                 dn.back().s = &dn.back();

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) {

diff --git a/src/rrtext8.cc b/src/rrtext8.cc
index fdeb5d391116c03fa4052467997303b9f73e126b..8cd461e1cc967fa58a6ce5b80112e5bfdbe4a8ae 100644 (file)
--- a/src/rrtext8.cc
+++ b/src/rrtext8.cc
@@ -114,6 +114,12 @@ void RRTExt8::init()
 {
 }
 
+void RRTExt8::reset()
+{
+        RRTS::reset();
+        this->delete_kd_nodes();
+}
+
 void RRTExt8::deinit()
 {
         this->delete_kd_nodes(this->kdroot_);