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 97d03bbbed51a81170cdeea73b537ad18c4bf238..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;
@@ -125,6 +138,9 @@ public:
        /*! Set pose of the bicycle car used in the planner. */
        void set_bc_pose_to(Pose const& p);
 
+       /*! Set bicycle car dimensions. */
+       void set_bc_to_become(std::string what);
+
        /*! Get goal. */
        RRTGoal const& goal(void) const;
 
@@ -149,8 +165,8 @@ public:
        /*! Return elapsed time. */
        double scnt() const;
 
-       /*! Set start. */
-       void start(double x, double y, double h);
+       /*! Set init pose. */
+       void set_init_pose_to(Pose const& p);
 
        /*! Get path. */
        std::vector<Pose> path() const;
@@ -166,13 +182,13 @@ public:
 
        /*! Set eta. */
        void eta(double e);
-
+public:
        /*! Generate JSON output. */
-       Json::Value json() const;
+       virtual Json::Value json(void) const;
 
        /*! Load JSON input. */
-       void json(Json::Value jvi);
-public:
+       virtual void json(Json::Value jvi);
+
        /*! Run next RRT* iteration. Return True if should continue. */
        virtual bool next();