Fix missing cusps points

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 0d09d642932ebb3fc6a1d7097267bb0dd79e5a37..80284b423ad40d70ff9ca9406475f3616850953a 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -595,10 +595,17 @@ bool T2::opt_path()
 {
         if (this->tlog().size() == 0)
                 return false;
-        std::vector<RRTNode *> cusps;
+        std::vector<RRTNode *> tmp_cusps;
         for (auto n: this->tlog().back()) {
-                if (n->parent() && sgn(n->s()) != sgn(n->parent()->s()))
-                                cusps.push_back(n);
+                if (n->parent() && sgn(n->s()) != sgn(n->parent()->s())) {
+                                tmp_cusps.push_back(n);
+                                tmp_cusps.push_back(n->parent());
+                }
+        }
+        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());