Refactor rrt node

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index a9d8fdc5343be0a1ef20ebc90e226343a0d4ecd9..4d5014534a96ef6fd1131af4ba234b921cbab905 100644 (file)
--- 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 c2235ad16b8ef455b45c1eed44036b3b0de52f98..d5953dbe799d352657c6a405e2747332b260585b 100644 (file)
--- 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 485ee6c9d4fbf998cc8f7b020205f64654fd586f..b55022444657221f8889f86d844c3d12e022b427 100644 (file)
--- 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();