From c02460e9bc1dbfb2e82348b308cc3fa1e3d12cda Mon Sep 17 00:00:00 2001 From: Renan Salles Date: Tue, 31 Oct 2023 11:11:30 +0900 Subject: [PATCH] fix angle oscillation --- mbf_abstract_nav/src/controller_action.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index 3c602b0c..2614aff2 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -122,6 +122,9 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut double oscillation_distance; private_nh.param("oscillation_distance", oscillation_distance, 0.03); + double oscillation_angle; + private_nh.param("oscillation_angle", oscillation_angle, M_PI); + mbf_msgs::ExePathResult result; mbf_msgs::ExePathFeedback feedback; @@ -276,7 +279,8 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut if (!oscillation_timeout.isZero()) { // check if oscillating - if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance) + if (mbf_utility::distance(robot_pose_, oscillation_pose) >= oscillation_distance || + mbf_utility::angle(robot_pose_, oscillation_pose) >= oscillation_angle) { last_oscillation_reset = ros::Time::now(); oscillation_pose = robot_pose_;