Change goal found to include IS_NEAR macro

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 0bb7442f31e802db14a6986e3c363b6f1147ea00..4aafe7f388702b6e6f593fef08165b8e4e303b80 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -304,8 +304,7 @@ bool RRTBase::goal_found(
         float xx = pow(node->x() - this->goal_->x(), 2);
         float yy = pow(node->y() - this->goal_->y(), 2);
         float dh = std::abs(node->h() - this->goal_->h());
-        if (pow(xx + yy, 0.5) < this->GOAL_FOUND_DISTANCE &&
-                        dh < this->GOAL_FOUND_ANGLE) {
+        if (IS_NEAR(node, this->goal_)) {
                 if (this->goal_found_) {
                         if (node->ccost() + (*cost)(node, this->goal_) <
                                         this->goal_->ccost()) {