Add eta getter/setter

[hubacji1/rrts.git] / incl / rrts.hh

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 0e12a5c4dc52ce82295ddcdea3a312ac68525b41..0b8e363cb3e22e322577cd70e34ef4148a92ab22 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 /*! \brief RRT* structure and default procedures.
  *
  * \file
@@ -30,6 +36,7 @@ private:
        double c_ = 0.0;
        double cc_ = 0.0;
        RRTNode* p_ = nullptr;
+       unsigned int cusp_ = 0;
 public:
        /*! Get cost to parent. */
        double c() const;
@@ -46,6 +53,12 @@ public:
        /*! Set parent node. */
        void p(RRTNode& p);
 
+       /*! Get number of backward-forward direction changes. */
+       unsigned int cusp() const;
+
+       /*! Set number of backward-forward direction changes. */
+       void cusp(RRTNode const& p);
+
        bool operator==(RRTNode const& n);
 };
 
@@ -60,6 +73,7 @@ protected:
        BicycleCar bc_;
        RRTGoal goal_;
        unsigned int icnt_ = 0;
+       unsigned int _imax = 1000;
        Ter ter_;
        std::default_random_engine gen_;
        std::vector<RRTNode> nodes_;
@@ -70,6 +84,10 @@ protected:
        double cost_ = 0.0;
        double eta_ = 0.5;
        double time_ = 0.0;
+       double last_goal_cc_ = 0.0;
+       std::vector<RRTNode> last_path_;
+       void recompute_cc(RRTNode* g);
+       void recompute_path_cc();
        double min_gamma_eta() const;
        bool should_continue() const;
        void join_steered(RRTNode* f);
@@ -90,6 +108,24 @@ protected:
 public:
        RRTS();
 
+       /*! Set internal bicycle car. */
+       BicycleCar &bc();
+
+       /*! Set maximum number of iterations before reset. */
+       void set_imax_reset(unsigned int i);
+
+       /*! Set goal. */
+       void set_goal(double x, double y, double b, double e);
+
+       /*! Set start. */
+       void set_start(double x, double y, double h);
+
+       /*! Get path. */
+       std::vector<Pose> get_path() const;
+
+       /*! Get path cost. */
+       double get_path_cost() const;
+
        /*! Get iterations counter. */
        unsigned int icnt() const;
 
@@ -99,6 +135,9 @@ public:
        /*! Return elapsed time. */
        double scnt() const;
 
+       double eta() const;
+       void eta(double e);
+
        /*! Generate JSON output. */
        Json::Value json() const;