From fdaa96b77c9e0781205d217ddb1aae7ffa07e643 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 --- mbf_abstract_nav/src/controller_action.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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;