Avoid duplicated warn log msgs when we cannot cancel a plugin (#194)

naturerobots · May 22, 2020 · c0de070 · c0de070
1 parent e2582e6
commit c0de070
Show file tree

Hide file tree

Showing 6 changed files with 11 additions and 19 deletions.
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -247,7 +247,8 @@ bool AbstractControllerExecution::isMoving()
 bool AbstractControllerExecution::reachedGoalCheck()
 {
   //if action has a specific tolerance, check goal reached with those tolerances
-  if(tolerance_from_action_){
+  if (tolerance_from_action_)
+  {
     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_);
@@ -263,7 +264,7 @@ bool AbstractControllerExecution::cancel()
 {
   // returns false if cancel is not implemented or rejected by the recovery behavior (will run until completion)
   bool ctrl_cancelled = controller_->cancel();
-  if(!ctrl_cancelled)
+  if (!ctrl_cancelled)
   {
     ROS_WARN_STREAM("Cancel controlling failed. Wait until the current control cycle finished!");
   }

diff --git a/mbf_abstract_nav/src/abstract_planner_execution.cpp b/mbf_abstract_nav/src/abstract_planner_execution.cpp
@@ -193,7 +193,7 @@ bool AbstractPlannerExecution::cancel()
   cancel_ = true; // force cancel immediately, as the call to cancel in the planner can take a while
 
   // returns false if cancel is not implemented or rejected by the planner (will run until completion)
-  if(!planner_->cancel())
+  if (!planner_->cancel())
   {
     ROS_WARN_STREAM("Cancel planning failed or is not supported by the plugin. "
         << "Wait until the current planning finished!");

diff --git a/mbf_abstract_nav/src/abstract_recovery_execution.cpp b/mbf_abstract_nav/src/abstract_recovery_execution.cpp
@@ -92,7 +92,7 @@ bool AbstractRecoveryExecution::cancel()
 {
   cancel_ = true;
   // returns false if cancel is not implemented or rejected by the recovery behavior (will run until completion)
-  if(!behavior_->cancel())
+  if (!behavior_->cancel())
   {
     ROS_WARN_STREAM("Cancel recovery behavior \"" << name_ << "\" failed or is not supported by the plugin. "
                         << "Wait until the current recovery behavior finished!");
@@ -134,10 +134,12 @@ void AbstractRecoveryExecution::run()
     ROS_WARN_STREAM("Recovery \"" << name_ << "\" interrupted!");
     setState(STOPPED);
   }
-  catch (...){
+  catch (...)
+  {
     ROS_FATAL_STREAM("Unknown error occurred in recovery behavior \"" << name_ << "\": " << boost::current_exception_diagnostic_information());
     setState(INTERNAL_ERROR);
   }
   condition_.notify_one();
 }
+
 } /* namespace mbf_abstract_nav */
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
@@ -210,14 +210,10 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution
         if (execution.isPatienceExceeded())
         {
           ROS_INFO_STREAM("Try to cancel the plugin \"" << name_ << "\" after the patience time has been exceeded!");
-          if(execution.cancel())
+          if (execution.cancel())
           {
             ROS_INFO_STREAM("Successfully canceled the plugin \"" << name_ << "\" after the patience time has been exceeded!");
           }
-          else
-          {
-            ROS_WARN_STREAM_THROTTLE(3, "Could not cancel the plugin \"" << name_ << "\" after the patience time has been exceeded!");
-          }
         }
         break;
 

diff --git a/mbf_abstract_nav/src/planner_action.cpp b/mbf_abstract_nav/src/planner_action.cpp
@@ -147,11 +147,7 @@ void PlannerAction::run(GoalHandle &goal_handle, AbstractPlannerExecution &execu
         if (execution.isPatienceExceeded())
         {
           ROS_INFO_STREAM_NAMED(name_, "Global planner patience has been exceeded! Cancel planning...");
-          if (!execution.cancel())
-          {
-            ROS_WARN_STREAM_THROTTLE_NAMED(2.0, name_, "Cancel planning failed or is not supported; "
-                "must wait until current plan finish!");
-          }
+          execution.cancel();
         }
         else
         {

diff --git a/mbf_abstract_nav/src/recovery_action.cpp b/mbf_abstract_nav/src/recovery_action.cpp
@@ -83,10 +83,7 @@ void RecoveryAction::run(GoalHandle &goal_handle, AbstractRecoveryExecution &exe
         if (execution.isPatienceExceeded())
         {
           ROS_INFO_STREAM("Recovery behavior \"" << goal.behavior << "\" patience exceeded! Cancel recovering...");
-          if (!execution.cancel())
-          {
-            ROS_WARN_STREAM("Cancel recovering \"" << goal.behavior << "\" failed or not supported; maybe wait until it is finished!");
-          }
+          execution.cancel();
         }
 
         ROS_DEBUG_STREAM_THROTTLE_NAMED(3, name_, "Recovering with: " << goal.behavior);