Fix searching for cusps points

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index b39e2d55b37a08e900fcbe018600ef60e20f1810..744a662484eac47636312de41fec58732ad3aaba 100644 (file)
--- 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++)