Update running controller goal also when we are preempting it

naturerobots · Aug 21, 2024 · fdaa96b · fdaa96b
1 parent 8e91d2b
commit fdaa96b
Showing 1 changed file with 2 additions and 1 deletion.
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;