From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Wed, 26 Sep 2018 08:35:34 +0000 (+0200)
Subject: Fix max cusps for path from steer procedure
X-Git-Tag: v0.2.0~19
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/37bc0e780e602c2ef4c0ab75878f2fc4d673eb62

Fix max cusps for path from steer procedure
---

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 3212f6b..535e9bc 100644
--- 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));