Log opt path time

diff --git a/api/rrts.h b/api/rrts.h
index c5aabdece25b7e5d63aa250210b5f1d4ee2aab24..ef4518e7233d754cf3cb9ed125469d1c7047bc24 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -145,6 +145,7 @@ class RRTS {
                 double log_path_time_ = 0.1;
                 unsigned int log_path_iter_ = 20;
                 std::vector<double> log_path_cost_;
+                std::vector<double> log_opt_time_;
 
                 /*! \brief Update and return elapsed time.
                 */

diff --git a/src/rrtext3.cc b/src/rrtext3.cc
index a93cfcea84da5b2d7c784b47e1769083ff3ea60b..9db9c78b93f08312b15c5871760c52769e56b431 100644 (file)
--- a/src/rrtext3.cc
+++ b/src/rrtext3.cc
@@ -225,6 +225,7 @@ std::vector<RRTNode *> RRTExt3::second_path_optimization()
 
 std::vector<RRTNode *> RRTExt3::path()
 {
+        auto tstart = std::chrono::high_resolution_clock::now();
         this->first_optimized_path_ = this->first_path_optimization();
         if (this->first_optimized_path_.size() > 0)
                 this->first_optimized_path_cost(
@@ -232,7 +233,12 @@ std::vector<RRTNode *> RRTExt3::path()
                 );
         else
                 return this->orig_path();
-        return this->second_path_optimization();
+        auto opt2 = this->second_path_optimization();
+        auto tend = std::chrono::high_resolution_clock::now();
+        auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
+            tend - tstart);
+        this->log_opt_time_.push_back(tdiff.count());
+        return opt2;
 }
 
 Json::Value RRTExt3::json()

diff --git a/src/rrts.cc b/src/rrts.cc
index 438392dabe1a54e4fe020ba30e8591cffa626b53..868f4712fafc370b924650a9adc0810c1899b2ed 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -685,6 +685,11 @@ Json::Value RRTS::json()
                 for (auto i: this->log_path_cost_)
                         jvo["log_path_cost"][cnt++] = i;
         }
+        {
+                unsigned int cnt = 0;
+                for (auto i: this->log_opt_time_)
+                        jvo["log_opt_time"][cnt++] = i;
+        }
         //{
         //        unsigned int ncnt = 0;
         //        for (auto n: this->nodes()) {