Refactor rrt node

[hubacji1/iamcar2.git] / rrts / src / rrts.cc

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();