Allow using the jsoncpp library installed in non-default locations

[hubacji1/rrts.git] / api / rrts.h

diff --git a/api/rrts.h b/api/rrts.h
index 2ce7e943ee5ec06395748f8a14610edc34a93d52..42724d73740a1d7fcedd6b5c6ce59efc13a05a5a 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -3,7 +3,7 @@
 
 #include <chrono>
 #include <functional>
-#include <jsoncpp/json/json.h>
+#include <json/json.h>
 #include <random>
 #include <vector>
 #include "bcar.h"
@@ -39,7 +39,6 @@ class RRTNodeType {
 */
 class RRTNode {
         private:
-                double c_ = 0;
                 RRTNode *p_ = nullptr;
                 unsigned int t_ = 0;
                 // -- from BicycleCar
@@ -51,9 +50,15 @@ class RRTNode {
                 double sp_ = 0;
                 double st_ = 0;
         public:
+                double c_ = 0;
+                double cc = 0.0;
                 // getters, setters
                 double c() const { return this->c_; }
-                void c(double c) { this->c_ = c; }
+                void c(double c)
+                {
+                        this->c_ = c;
+                        this->cc = this->p_->cc + this->c_;
+                }
 
                 RRTNode *p() const { return this->p_; }
                 void p(RRTNode *p) { this->p_ = p; }
@@ -142,8 +147,9 @@ class RRTS {
                 std::vector<Obstacle> obstacles_;
                 std::vector<RRTNode> samples_;
                 std::vector<RRTNode> steered_;
+                std::vector<RRTNode *> path_;
                 double log_path_time_ = 0.1;
-                std::vector<double> log_path_cost_;
+                unsigned int log_path_iter_ = 20;
 
                 /*! \brief Update and return elapsed time.
                 */
@@ -215,6 +221,8 @@ class RRTS {
                 void rewire();
         public:
                 /// ---
+                std::vector<double> log_opt_time_;
+                std::vector<double> log_path_cost_;
                 struct { double x=0; double y=0; double b=0; double e=0; } entry;
                 bool entry_set = false;
                 struct { double x=0; double y=0; double h=0; } entry1;
@@ -226,12 +234,17 @@ class RRTS {
                 /*! \brief Initialize RRT algorithm if needed.
                 */
                 virtual void init();
+                virtual void reset();
                 /*! \brief Deinitialize RRT algorithm if needed.
                 */
                 virtual void deinit();
                 /*! \brief Return path found by RRT*.
                 */
-                virtual std::vector<RRTNode *> path();
+                virtual std::vector<RRTNode *>& path()
+                {
+                        return this->path_;
+                }
+                virtual void compute_path();
                 /*! \brief Return ``true`` if algorithm should stop.
 
                 Update counters (iteration, seconds, ...) and return if
@@ -306,6 +319,7 @@ class RRTS {
 
                 // getters, setters
                 unsigned int icnt() const { return this->icnt_; }
+                void icnt(unsigned int i) { this->icnt_ = i; }
                 double scnt() const { return this->scnt_; }
                 bool gf() const { return this->gf_; }
                 void gf(bool f) { this->gf_ = f; }