Goal found only when near to some goal

diff --git a/src/rrts.cc b/src/rrts.cc
index 937160319eb6f6884add356e8eb8bde57e39ccc2..337b7a67ade4e1c09e19e2943106915c4f9b1d9c 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -224,7 +224,12 @@ bool RRTS::next()
         }
         for (auto &n: this->goals()) {
                 double cost = this->cost(this->nodes().back(), n);
-                if (cost < ETA) {
+                double edist = sqrt(
+                        pow(this->nodes().back().x() - n.x(), 2)
+                        + pow(this->nodes().back().y() - n.y(), 2)
+                );
+                double adist = std::abs(this->nodes().back().h() - n.h());
+                if (edist < 0.05 && adist < M_PI / 32) {
                         next = false;
                         if (
                                 n.p() == nullptr