Fix tmp cusps overflow

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 26c0f90296d490ae312350fc1eeb8ef8fab95572..303074196e192ca763287d1a5f6336623b3b577b 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -626,7 +626,7 @@ bool RRTBase::opt_path()
                 return false;
         std::vector<RRTNode *> cusps;
         for (unsigned int i = 0; i < tmp_cusps.size(); i++) {
-                if (tmp_cusps[i] != tmp_cusps[i + 1])
+                if (tmp_cusps[i] != tmp_cusps[(i + 1) % tmp_cusps.size()])
                         cusps.push_back(tmp_cusps[i]);
         }
         std::reverse(cusps.begin(), cusps.end());