Merge branch 'fix-steer'

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

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 6bab6ae71f6fadd06e2bf63e36a34a9e57933da1..cccdc44ecb2e97582438aa2d99237a79dcb834fb 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;
@@ -52,13 +53,19 @@ public:
        RRTNode* p() const;
 
        /*! Set parent node. */
+       void p(RRTNode& p, bool connecting_goal);
        void p(RRTNode& p);
 
-       /*! Get number of backward-forward direction changes. */
-       unsigned int cusp() const;
+       /*! Get number of backward-forward direction changes.
+        *
+        * The parent node may be cusp to this node, i.e. parent may have sp ==
+        * 0 or sgn(sp) differs from sgn(this->sp). In such a situation, the
+        * number of this->cusp_cnt is incremented by one.
+        */
+       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 +79,15 @@ 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);
+
+       /*! Return true if p would be cusp if set as parent of this. */
+       bool would_be_cusp_if_parent(RRTNode const& p) const;
+
        bool operator==(RRTNode const& n);
 };
 
@@ -115,6 +131,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 +147,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 +174,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;