Fix repeating cusp in opt

When two nodes are near themselves, do not use both.

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 0234fd450c562caadb46bb7ee2cf853806ca1dc6..130b1385b64753cc7bd4c685f8e1fd8554d5b2b1 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -923,7 +923,10 @@ 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) % tmp_cusps.size()])
+                if (!IS_NEAR(
+                        tmp_cusps[i],
+                        tmp_cusps[(i + 1) % tmp_cusps.size()]
+                ))
                         cusps.push_back(tmp_cusps[i]);
         }
         std::reverse(cusps.begin(), cusps.end());