Merge branch 'fix-steer'

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

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 4f131145c63ae19c45cdf7cd5c38a7334b04ccc0..0c1a663e0f7e5e5beaa9d16404bddd090240f33b 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -55,25 +55,37 @@ RRTNode::p() const
 }
 
 void
-RRTNode::p(RRTNode& p)
+RRTNode::p(RRTNode& p, bool connecting_goal)
 {
-       if (this != &p) {
-               this->_p = &p;
+       assert(this != &p);
+       if (!connecting_goal) {
+               assert(!(std::abs(p.x() - this->x()) < 1e-3
+                       && std::abs(p.y() - this->y()) < 1e-3
+                       && std::abs(p.h() - this->h()) < 1e-3));
        }
+       this->_p = &p;
+       this->p_is_cusp(this->would_be_cusp_if_parent(p));
+       this->cusp_cnt(p);
+}
+
+void
+RRTNode::p(RRTNode& p)
+{
+       return this->p(p, false);
 }
 
 unsigned int
-RRTNode::cusp() const
+RRTNode::cusp_cnt() const
 {
-       return this->_cusp;
+       return this->_cusp_cnt;
 }
 
 void
-RRTNode::cusp(RRTNode const& p)
+RRTNode::cusp_cnt(RRTNode const& p)
 {
-       this->_cusp = p.cusp();
-       if (this->sp() != p.sp() || this->sp() == 0.0) {
-               this->_cusp++;
+       this->_cusp_cnt = p.cusp_cnt();
+       if (this->_p_is_cusp) {
+               this->_cusp_cnt++;
        }
 }
 
@@ -89,6 +101,45 @@ RRTNode::st(int st)
        this->_segment_type = st;
 }
 
+bool
+RRTNode::p_is_cusp(void) const
+{
+       return this->_p_is_cusp;
+}
+
+void
+RRTNode::p_is_cusp(bool isit)
+{
+       this->_p_is_cusp = isit;
+}
+
+bool
+RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
+{
+       if (std::abs(p.sp()) < 1e-3) {
+               // It should not be possible to have two zero speed nodes next
+               // to each other. In practice, this sometimes happens.
+               //assert(std::abs(this->sp()) > 1e-3);
+               if (p.p()) {
+                       if (sgn(p.p()->sp()) != sgn(this->sp())) {
+                               return true;
+                       } else {
+                               return false;
+                       }
+               } else {
+                       return true; // only root has no parent and it is cusp
+               }
+       } else {
+               if (std::abs(this->sp()) < 1e-3) {
+                       return false; // this is cusp, not the parent
+               } else if (sgn(p.sp()) != sgn(this->sp())) {
+                       return true;
+               } else {
+                       return false;
+               }
+       }
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -140,7 +191,6 @@ RRTS::join_steered(RRTNode* f)
                RRTNode* t = &this->_nodes.back();
                t->p(*f);
                t->c(this->cost_build(*f, *t));
-               t->cusp(*f);
                this->_steered.erase(this->_steered.begin());
                f = t;
        }
@@ -151,6 +201,15 @@ RRTS::connect()
 {
        RRTNode* f = this->_nn;
        RRTNode* t = &this->_steered.front();
+       // Require the steer method to return first node equal to nn:
+       assert(std::abs(t->x() - f->x()) < 1e-3
+               && std::abs(t->y() - f->y()) < 1e-3
+               && std::abs(t->h() - f->h()) < 1e-3);
+       this->_steered.erase(this->_steered.begin());
+       t = &this->_steered.front();
+       assert(!(std::abs(t->x() - f->x()) < 1e-3
+               && std::abs(t->y() - f->y()) < 1e-3
+               && std::abs(t->h() - f->h()) < 1e-3));
 #if USE_RRTS
        double cost = f->cc() + this->cost_build(*f, *t);
        for (auto n: this->nv_) {
@@ -172,7 +231,6 @@ RRTS::connect()
        t = &this->_nodes.back();
        t->p(*f);
        t->c(this->cost_build(*f, *t));
-       t->cusp(*f);
        this->_steered.erase(this->_steered.begin());
        return true;
 }
@@ -279,6 +337,12 @@ RRTS::set_bc_pose_to(Pose const& p)
        this->_bc.set_pose_to(p);
 }
 
+void
+RRTS::set_bc_to_become(std::string what)
+{
+       this->_bc.become(what);
+}
+
 RRTGoal const&
 RRTS::goal(void) const
 {
@@ -328,11 +392,11 @@ RRTS::scnt() const
 }
 
 void
-RRTS::start(double x, double y, double h)
+RRTS::set_init_pose_to(Pose const& p)
 {
-       this->_nodes.front().x(x);
-       this->_nodes.front().y(y);
-       this->_nodes.front().h(h);
+       this->_nodes.front().x(p.x());
+       this->_nodes.front().y(p.y());
+       this->_nodes.front().h(p.h());
 }
 
 std::vector<Pose>
@@ -386,6 +450,7 @@ RRTS::json(void) const
                        jvo["paths"][j][i][2] = n.h();
                        jvo["paths"][j][i][3] = n.sp();
                        jvo["paths"][j][i][4] = n.st();
+                       jvo["paths"][j][i][5] = n.p_is_cusp();
                        i++;
                }
                jvo["costs"][j] = path.back().cc();
@@ -398,11 +463,13 @@ RRTS::json(void) const
                jvo["paths"][j][i][2] = n->h();
                jvo["paths"][j][i][3] = n->sp();
                jvo["paths"][j][i][4] = n->st();
+               jvo["paths"][j][i][5] = n->p_is_cusp();
                jvo["path"][i][0] = n->x();
                jvo["path"][i][1] = n->y();
                jvo["path"][i][2] = n->h();
                jvo["path"][i][3] = n->sp();
                jvo["path"][i][4] = n->st();
+               jvo["path"][i][5] = n->p_is_cusp();
                i++;
        }
        jvo["costs"][j] = this->_path.back()->cc();
@@ -418,10 +485,10 @@ RRTS::json(Json::Value jvi)
 {
        assert(jvi["init"] != Json::nullValue);
        assert(jvi["goal"] != Json::nullValue);
-       this->start(
+       this->set_init_pose_to(Pose(
                jvi["init"][0].asDouble(),
                jvi["init"][1].asDouble(),
-               jvi["init"][2].asDouble());
+               jvi["init"][2].asDouble()));
        if (jvi["goal"].size() == 4) {
                this->goal(
                        jvi["goal"][0].asDouble(),
@@ -472,13 +539,15 @@ RRTS::next()
        this->join_steered(&this->_nodes.back());
        RRTNode* just_added = &this->_nodes.back();
        bool gf = false;
-       while (ss > 0 && just_added->p() != nullptr) {
+       while (ss > 0 && just_added != nullptr) {
                this->steer(*just_added, this->_goal);
                if (this->collide_steered()) {
                        ss--;
                        just_added = just_added->p();
                        continue;
                }
+               // The first of steered is the same as just_added.
+               this->_steered.erase(this->_steered.begin());
                this->join_steered(just_added);
                bool gn = this->_goal.edist(this->_nodes.back()) < this->eta();
                bool gd = this->goal_drivable_from(this->_nodes.back());
@@ -489,7 +558,7 @@ RRTS::next()
                        double ncc = this->_nodes.back().cc() + nc;
                        if (this->_goal.p() == nullptr
                                        || ncc < this->_goal.cc()) {
-                               this->_goal.p(this->_nodes.back());
+                               this->_goal.p(this->_nodes.back(), true);
                                this->_goal.c(nc);
                                gf = true;
                        }