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 f5e297841597434dd53c64fb0e2b0206a97866fc..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; }
@@ -129,7 +134,7 @@ class Obstacle {
 normal, 1 - uniform, 2 - uniform circle)
 */
 class RRTS {
-        private:
+        protected:
                 unsigned int icnt_ = 0;
                 std::chrono::high_resolution_clock::time_point tstart_;
                 double scnt_ = 0;
@@ -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.
                 */
@@ -168,7 +174,7 @@ class RRTS {
                 /*! \brief Set uniform circle distribution for sampling.
                 */
                 void set_sample_uniform_circle();
-        protected:
+                RRTNode* use_nn; // Used for RRTExt12.
                 std::vector<RRTNode> tmp_steered_;
                 bool finishit = false;
                 double path_cost_before_opt_ = 9999;
@@ -184,6 +190,8 @@ class RRTS {
                 virtual std::tuple<bool, unsigned int, unsigned int>
                 collide_steered_from(RRTNode &f);
                 virtual std::tuple<bool, unsigned int, unsigned int>
+                collide_tmp_steered_from(RRTNode &f);
+                virtual std::tuple<bool, unsigned int, unsigned int>
                 collide_two_nodes(RRTNode &f, RRTNode &t);
                 void sample();
                         std::default_random_engine gen_;
@@ -209,24 +217,34 @@ class RRTS {
                 void join_tmp_steered(RRTNode *f);
                 virtual bool goal_found(RRTNode &f);
                 // RRT* procedures
-                bool connect();
+                virtual bool connect();
                 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;
+                struct { double x=0; double y=0; double h=0; } entry2;
+                bool entries_set = false;
                 std::vector<RRTNode *> steered1_;
                 std::vector<RRTNode *> steered2_;
                 /// ---
                 /*! \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
@@ -252,7 +270,7 @@ class RRTS {
                 bool should_continue();
                 /*! \brief Run next RRT* iteration.
                 */
-                bool next();
+                virtual bool next();
                 /*! \brief Set sampling info.
 
                 Based on `sample_dist_type`, set proper distribution
@@ -301,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; }