From 4654a5c0a7a635289d2b46eaf06fd7042f0d69cb Mon Sep 17 00:00:00 2001 From: corot Date: Thu, 16 Jul 2020 17:41:35 +0900 Subject: [PATCH] Check the absolute angle difference when checking angular tolerance --- mbf_abstract_nav/src/abstract_controller_execution.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp index 26967c57..56348f81 100644 --- a/mbf_abstract_nav/src/abstract_controller_execution.cpp +++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp @@ -252,13 +252,13 @@ bool AbstractControllerExecution::reachedGoalCheck() { return controller_->isGoalReached(action_dist_tolerance_, action_angle_tolerance_) || (mbf_tolerance_check_ && mbf_utility::distance(robot_pose_, plan_.back()) < action_dist_tolerance_ - && mbf_utility::angle(robot_pose_, plan_.back()) < action_angle_tolerance_); + && std::abs(mbf_utility::angle(robot_pose_, plan_.back())) < action_angle_tolerance_); } // Otherwise, check whether the controller plugin returns goal reached or if mbf should check for goal reached. return controller_->isGoalReached(dist_tolerance_, angle_tolerance_) || (mbf_tolerance_check_ && mbf_utility::distance(robot_pose_, plan_.back()) < dist_tolerance_ - && mbf_utility::angle(robot_pose_, plan_.back()) < angle_tolerance_); + && std::abs(mbf_utility::angle(robot_pose_, plan_.back())) < angle_tolerance_); } bool AbstractControllerExecution::cancel()