From 367269c3c2427dfee29426e5f9f136731fb3b049 Mon Sep 17 00:00:00 2001 From: corot Date: Wed, 21 Aug 2024 10:42:10 +0900 Subject: [PATCH] Update running controller goal also when we are preempting it --- .../include/mbf_abstract_nav/abstract_action_base.hpp | 2 +- mbf_abstract_nav/src/controller_action.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action_base.hpp b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action_base.hpp index 3b2e2cde..670b42e0 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action_base.hpp +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action_base.hpp @@ -143,7 +143,7 @@ class AbstractActionBase // if there is already a plugin running on the same slot, cancel it slot_it->second.execution->cancel(); - // TODO + WARNING: this will block the main thread for an arbitrary time during which we won't execute callbacks + // WARNING: this will block the main thread for an arbitrary time during which we won't execute callbacks if (slot_it->second.thread_ptr->joinable()) { slot_it->second.thread_ptr->join(); } diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index 50a33266..e7a22472 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -81,9 +81,10 @@ void ControllerAction::start( if(slot_it != concurrency_slots_.end() && slot_it->second.in_use) { boost::lock_guard goal_guard(goal_mtx_); + const auto slot_status = slot_it->second.goal_handle.getGoalStatus().status; if ((slot_it->second.execution->getName() == goal_handle.getGoal()->controller || goal_handle.getGoal()->controller.empty()) && - slot_it->second.goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::ACTIVE) + (slot_status == actionlib_msgs::GoalStatus::ACTIVE || slot_status == actionlib_msgs::GoalStatus::PREEMPTING)) { ROS_DEBUG_STREAM_NAMED(name_, "Updating running controller goal of slot " << static_cast(slot)); update_plan = true;