Add reset method, remove debug print

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

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();