From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 7 Oct 2019 09:58:40 +0000 (+0200)
Subject: Add time awareness to RRT
X-Git-Tag: v0.3.0~11^2~5
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/bc23a1a8d9edcecfc6c0fd000a6f3dd27bb389f9

Add time awareness to RRT
---

diff --git a/api/rrts.h b/api/rrts.h
index b6e4296..903e45e 100644
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -1,6 +1,7 @@
 #ifndef RRTS_H
 #define RRTS_H
 
+#include <chrono>
 #include <functional>
 #include <random>
 #include <vector>
@@ -55,6 +56,8 @@ class Obstacle {
 class RRTS {
         private:
                 unsigned int icnt_ = 0;
+                std::chrono::high_resolution_clock::time_point tstart_;
+                double scnt_ = 0;
 
                 std::vector<RRTNode> goals_;
                 std::vector<RRTNode> nodes_;
@@ -62,6 +65,10 @@ class RRTS {
                 std::vector<RRTNode> samples_;
                 std::vector<RRTNode> steered_;
 
+                /*! \brief Update and return elapsed time.
+                */
+                double elapsed();
+
                 // RRT procedures
                 std::tuple<bool, unsigned int, unsigned int>
                 collide(std::vector<std::tuple<double, double>> &poly);
@@ -115,6 +122,7 @@ class RRTS {
 
                 // getters, setters
                 unsigned int icnt() const { return this->icnt_; }
+                double scnt() const { return this->scnt_; }
                 std::vector<RRTNode> &goals() { return this->goals_; }
                 std::vector<RRTNode> &nodes() { return this->nodes_; }
                 std::vector<Obstacle> &obstacles() { return this->obstacles_; }
diff --git a/src/rrts.cc b/src/rrts.cc
index c09c59b..901eeef 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -21,6 +21,17 @@ Obstacle::Obstacle()
 {
 }
 
+double RRTS::elapsed()
+{
+        std::chrono::duration<double> dt;
+        dt = std::chrono::duration_cast<std::chrono::duration<double>>(
+                std::chrono::high_resolution_clock::now()
+                - this->tstart_
+        );
+        this->scnt_ = dt.count();
+        return this->scnt_;
+}
+
 // RRT procedures
 std::tuple<bool, unsigned int, unsigned int>
 RRTS::collide(std::vector<std::tuple<double, double>> &poly)
@@ -231,6 +242,8 @@ std::vector<RRTNode *> RRTS::path()
 
 bool RRTS::next()
 {
+        if (this->icnt_ == 0)
+                this->tstart_ = std::chrono::high_resolution_clock::now();
         bool next = true;
         if (++this->icnt_ > 999)
                 next = false;