X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/ad8fcd4781d83e876e116be55a8a8c2e60de0a4a..82c63df514da47ffb441b8cb878a5f7a2a6ad860:/rrts/incl/rrts.hh

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index 6bab6ae..cccdc44 100644
--- 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;