From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Sat, 6 Oct 2018 10:57:22 +0000 (+0200)
Subject: Fix searching for cusps points
X-Git-Tag: v0.2.0~1^2~2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/98e54410270ebd0e3cc983b51f8453807c69d44d

Fix searching for cusps points
---

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b39e2d5..744a662 100644
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -634,21 +634,22 @@ bool T2::opt_path()
         float oc = this->tlog().back().front()->ccost();
         std::vector<RRTNode *> tmp_cusps;
         for (auto n: this->tlog().back()) {
-                if (n->parent() && sgn(n->s()) != sgn(n->parent()->s())) {
-                                tmp_cusps.push_back(n);
-                                tmp_cusps.push_back(n->parent());
+                if (sgn(n->s()) == 0) {
+                        tmp_cusps.push_back(n);
+                } else if (n->parent() &&
+                                sgn(n->s()) != sgn(n->parent()->s())) {
+                        tmp_cusps.push_back(n);
+                        tmp_cusps.push_back(n->parent());
                 }
         }
-        if (tmp_cusps.size() == 0)
+        if (tmp_cusps.size() < 2)
                 return false;
         std::vector<RRTNode *> cusps;
         for (unsigned int i = 0; i < tmp_cusps.size() - 1; i++) {
                 if (tmp_cusps[i] != tmp_cusps[i + 1])
                         cusps.push_back(tmp_cusps[i]);
         }
-        cusps.push_back(this->root());
         std::reverse(cusps.begin(), cusps.end());
-        cusps.push_back(this->tlog().back().front());
         // Begin of Dijkstra
         std::vector<RRTNodeDijkstra> dnodes;
         for (unsigned int i = 0; i < cusps.size(); i++)