From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 14 Mar 2023 17:57:39 +0000 (+0100)
Subject: Refactor rrt node
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/commitdiff_plain/9066c783342c64176432334ca421ca6cf59df371

Refactor rrt node
---

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index a9d8fdc..4d50145 100644
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -24,7 +24,7 @@ using namespace bcar;
 /*! Compute elapsed time class. */
 class Ter {
 private:
-	std::chrono::high_resolution_clock::time_point tstart_;
+	std::chrono::high_resolution_clock::time_point _tstart;
 public:
 	void start();
 	double scnt() const;
@@ -33,10 +33,11 @@ public:
 /*! Store RRT node. */
 class RRTNode : public virtual Pose, public virtual CarMove {
 private:
-	double c_ = 0.0;
-	double cc_ = 0.0;
-	RRTNode* p_ = nullptr;
-	unsigned int cusp_ = 0;
+	double _c = 0.0;
+	double _cc = 0.0;
+	RRTNode* _p;
+	unsigned int _cusp = 0;
+	int _segment_type = 0;  // 0 ~ straight, 1 ~ left, -1 right
 public:
 	/*! Get cost to parent. */
 	double c() const;
@@ -59,9 +60,19 @@ public:
 	/*! Set number of backward-forward direction changes. */
 	void cusp(RRTNode const& p);
 
-	bool operator==(RRTNode const& n);
+	/*! \brief Get Reeds & Shepp segment type.
+	 *
+	 * Segment type is:
+	 * - 0 for straight,
+	 * - 1 for turning left,
+	 * - and -1 for turning right.
+	 */
+	int st(void);
+
+	/*! Set Reeds & Shepp segment type. */
+	void st(int st);
 
-	int segment_type = 0;
+	bool operator==(RRTNode const& n);
 };
 
 class RRTGoal : public virtual RRTNode, public virtual PoseRange {
diff --git a/rrts/src/rrtext16.cc b/rrts/src/rrtext16.cc
index c2235ad..d5953db 100644
--- a/rrts/src/rrtext16.cc
+++ b/rrts/src/rrtext16.cc
@@ -18,7 +18,7 @@ cb_steer(double q[4], void *w)
 	st->back().y(q[1]);
 	st->back().h(q[2]);
 	st->back().sp(q[3]);
-	st->back().segment_type = q[4];
+	st->back().st(q[4]);
 	return 0;
 }
 
diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 485ee6c..b550224 100644
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -17,67 +17,78 @@ namespace rrts {
 void
 Ter::start()
 {
-	this->tstart_ = std::chrono::high_resolution_clock::now();
+	this->_tstart = std::chrono::high_resolution_clock::now();
 }
 
 double
 Ter::scnt() const
 {
-	using namespace std::chrono;
-	auto t = high_resolution_clock::now() - this->tstart_;
-	auto d = duration_cast<duration<double>>(t);
+	auto t = std::chrono::high_resolution_clock::now() - this->_tstart;
+	auto d = std::chrono::duration_cast<std::chrono::seconds>(t);
 	return d.count();
 }
 
 double
 RRTNode::c() const
 {
-	return this->c_;
+	return this->_c;
 }
 
 void
 RRTNode::c(double c)
 {
-	assert(this->p_ != nullptr);
-	this->c_ = c;
-	this->cc_ = this->p_->cc() + c;
+	assert(this->_p != nullptr);
+	this->_c = c;
+	this->_cc = this->_p->cc() + c;
 }
 
 double
 RRTNode::cc() const
 {
-	return this->cc_;
+	return this->_cc;
 }
 
 RRTNode*
 RRTNode::p() const
 {
-	return this->p_;
+	return this->_p;
 }
 
 void
 RRTNode::p(RRTNode& p)
 {
 	if (this != &p) {
-		this->p_ = &p;
+		this->_p = &p;
 	}
 }
 
 unsigned int
 RRTNode::cusp() const
 {
-	return this->cusp_;
+	return this->_cusp;
 }
 
 void
 RRTNode::cusp(RRTNode const& p)
 {
-	this->cusp_ = p.cusp();
+	this->_cusp = p.cusp();
 	if (this->sp() != p.sp() || this->sp() == 0.0) {
-		this->cusp_++;
+		this->_cusp++;
 	}
 }
 
+int
+RRTNode::st(void)
+{
+	return this->_segment_type;
+}
+
+void
+RRTNode::st(int st)
+{
+	this->_segment_type = st;
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -348,7 +359,7 @@ RRTS::json() const
 		jvo["path"][i][1] = n->y();
 		jvo["path"][i][2] = n->h();
 		jvo["path"][i][3] = n->sp();
-		jvo["path"][i][4] = n->segment_type;
+		jvo["path"][i][4] = n->st();
 		i++;
 	}
 	jvo["goal_cc"] = this->goal_.cc();