Update running controller goal also when we are preempting it

naturerobots · Aug 27, 2024 · 367269c · 367269c
1 parent 8e91d2b
commit 367269c
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 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
@@ -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
@@ -81,9 +81,10 @@ void ControllerAction::start(
   if(slot_it != concurrency_slots_.end() && slot_it->second.in_use)
   {
     boost::lock_guard<boost::mutex> 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<int>(slot));
       update_plan = true;