From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 3 May 2021 19:31:12 +0000 (+0200)
Subject: Log opt path time
X-Git-Tag: v0.7.0~12
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/76e7021fa028e514b75e4604b5cb2596407a8bc6

Log opt path time
---

diff --git a/api/rrts.h b/api/rrts.h
index c5aabde..ef4518e 100644
--- 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 a93cfce..9db9c78 100644
--- 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 438392d..868f471 100644
--- 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()) {