From 137af55017fa7b4bf70166e70ac7f160a2358844 Mon Sep 17 00:00:00 2001
From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Thu, 4 Oct 2018 10:27:03 +0200
Subject: [PATCH] Fix missing cusps points

---
 decision_control/rrtplanner.cc | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index 0d09d64..80284b4 100644
--- 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());
-- 
2.39.2