Store info about if parent is cusp in node

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

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 2e85862c529e2e14fd29ca977af035124d367d17..dc146c488bd0b8cc731f9fd7f3a8f7044b7bfe63 100644 (file)
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -36,8 +36,9 @@ private:
        double _c = 0.0;
        double _cc = 0.0;
        RRTNode* _p = nullptr;
-       unsigned int _cusp = 0;
+       unsigned int _cusp_cnt = 0;
        int _segment_type = 0;  // 0 ~ straight, 1 ~ left, -1 right
+       bool _p_is_cusp = false;
 public:
        /*! Get cost to parent. */
        double c() const;
@@ -55,10 +56,10 @@ public:
        void p(RRTNode& p);
 
        /*! Get number of backward-forward direction changes. */
-       unsigned int cusp() const;
+       unsigned int cusp_cnt() const;
 
        /*! Set number of backward-forward direction changes. */
-       void cusp(RRTNode const& p);
+       void cusp_cnt(RRTNode const& p);
 
        /*! \brief Get Reeds & Shepp segment type.
         *
@@ -72,6 +73,12 @@ public:
        /*! Set Reeds & Shepp segment type. */
        void st(int st);
 
+       /*! Return true if the parent is cusp node and false otherwise. */
+       bool p_is_cusp(void) const;
+
+       /*! Set if the parent node is cusp (direction changed from parent). */
+       void p_is_cusp(bool isit);
+
        bool operator==(RRTNode const& n);
 };
 
@@ -115,6 +122,12 @@ protected:
        virtual void find_nv(RRTNode const& t);
        virtual void compute_path();
 protected:
+       /*! \brief Return nodes from f to t inclusive.
+        *
+        * The "inclusive" means that f is at the same pose (x, y, h) as
+        * this->_steered.front() and t is at the same pose (x, y, h) as
+        * this->_steered.back().
+        */
        virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
        virtual bool collide_steered() = 0;
        virtual RRTNode sample() = 0;