Add time awareness to RRT

diff --git a/api/rrts.h b/api/rrts.h
index b6e42961575eadbccf3faeeb62ade58b4de74896..903e45e589c73351dcda12be5c47c514f1ae2df2 100644 (file)
--- 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 c09c59babb20414aa0ad9b71db048f851cc02427..901eeef0b0c85e02c491b6829ec70f3422623464 100644 (file)
--- 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;