Include asserts in set parent method for rrt node

diff --git a/rrts/incl/rrts.hh b/rrts/incl/rrts.hh
index a6c00f0f78745ad9c620a9e49ce2d0cad8ee9384..cccdc44ecb2e97582438aa2d99237a79dcb834fb 100644 (file)
--- a/rrts/incl/rrts.hh
+++ b/rrts/incl/rrts.hh
@@ -53,6 +53,7 @@ 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.

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index d6f8f002c2a750a8f17527bac7f944fa93e8b46e..6bcf151811721854a0d226499b0eb95eb4345016 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -55,15 +55,25 @@ 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_cnt() const
 {
@@ -541,7 +551,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;
                        }