Fix max cusps for path from steer procedure

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 3212f6b5413f73b7a233e9d6f32b17c628d6d70c..535e9bccf98182f2377521a3f4a29487e20421cc 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -461,11 +461,11 @@ bool T2::next()
                         delete ns;
                 } else if (IS_NEAR(pn, ns)) {
                         delete ns;
-                } else if (cusps > 0) {
-                        en_add = false;
                 } else {
                         if (sgn(pn->s()) != sgn(ns->s()))
                                 cusps++;
+                        if (cusps > 0)
+                                en_add = false;
 #if NVVERSION>1
                         nvs = this->nv(
                                         this->iy_,
@@ -515,11 +515,11 @@ bool T2::next()
                                 delete ns;
                         } else if (IS_NEAR(pn, ns)) {
                                 delete ns;
-                        } else if (cusps > 0) {
-                                en_add = false;
                         } else {
                                 if (sgn(pn->s()) != sgn(ns->s()))
                                         cusps++;
+                                if (cusps > 0)
+                                        en_add = false;
                                 this->nodes().push_back(ns);
                                 this->add_iy(ns);
                                 pn->add_child(ns, this->cost(pn, ns));