Fix asserts and speed tolerance

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 85bd8ed043210fd2558e387556911d17d7e4b2b7..0c1a663e0f7e5e5beaa9d16404bddd090240f33b 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -116,8 +116,10 @@ RRTNode::p_is_cusp(bool isit)
 bool
 RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
 {
-       if (p.sp() == 0) {
-               assert(this->sp() != 0);
+       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;
@@ -128,7 +130,7 @@ RRTNode::would_be_cusp_if_parent(RRTNode const& p) const
                        return true; // only root has no parent and it is cusp
                }
        } else {
-               if (this->sp() == 0) {
+               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;
@@ -200,11 +202,14 @@ 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);
-       assert(std::abs(t->y() - f->x()) < 1e-3);
-       assert(std::abs(t->h() - f->x()) < 1e-3);
+       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_) {