Consider only one goal in goal found procedure

diff --git a/src/rrts.cc b/src/rrts.cc
index d4ea90e7eb8caf5b1e923cafc4fbabb475366fc6..c48e680a31fe28f438a37e3e2836d3a88f40a430 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -2,7 +2,7 @@
 #include "rrts.h"
 
 #include "reeds_shepp.h"
-
+#include <iostream>
 template <typename T> int sgn(T val) {
         return (T(0) < val) - (val < T(0));
 }
@@ -225,19 +225,18 @@ void RRTS::join_steered(RRTNode *f)
 bool RRTS::goal_found(RRTNode &f)
 {
         bool found = false;
-        for (auto &g: this->goals()) {
-                double cost = this->cost_build(f, g);
-                double edist = sqrt(
-                        pow(f.x() - g.x(), 2)
-                        + pow(f.y() - g.y(), 2)
-                );
-                double adist = std::abs(f.h() - g.h());
-                if (edist < 0.05 && adist < M_PI / 32) {
-                        found = true;
-                        if (g.p() == nullptr || cc(f) + cost < cc(g)) {
-                                g.p(&f);
-                                g.c(cost);
-                        }
+        auto &g = this->goals().front();
+        double cost = this->cost_build(f, g);
+        double edist = sqrt(
+                pow(f.x() - g.x(), 2)
+                + pow(f.y() - g.y(), 2)
+        );
+        double adist = std::abs(f.h() - g.h());
+        if (edist < 0.05 && adist < M_PI / 32) {
+                found = true;
+                if (g.p() == nullptr || cc(f) + cost < cc(g)) {
+                        g.p(&f);
+                        g.c(cost);
                 }
         }
         return found;