From c7c6ca281d500d4e558127d920a278354a12e62b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 16 Feb 2024 23:00:35 +0100 Subject: [PATCH] Fix the multiple chainable controller activation failure bug (Fixes #1400) --- controller_manager/src/controller_manager.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1911bd6ff88..43665c2548e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -840,6 +840,12 @@ void ControllerManager::clear_requests() { deactivate_request_.clear(); activate_request_.clear(); + // Set these interfaces are unavailable when clearing requests to avoid leaving them in available + // state without the controller being in active state + for (const auto & controller_name : to_chained_mode_request_) + { + resource_manager_->make_controller_reference_interfaces_unavailable(controller_name); + } to_chained_mode_request_.clear(); from_chained_mode_request_.clear(); activate_command_interface_request_.clear(); @@ -1429,9 +1435,6 @@ void ControllerManager::switch_chained_mode( auto controller = found_it->c; if (!is_controller_active(*controller)) { - // if it is a chainable controller, make the reference interfaces available on preactivation - // (This is needed when you activate a couple of chainable controller altogether) - resource_manager_->make_controller_reference_interfaces_available(controller_name); if (!controller->set_chained_mode(to_chained_mode)) { RCLCPP_ERROR( @@ -2368,6 +2371,10 @@ controller_interface::return_type ControllerManager::check_following_controllers if (found_it == to_chained_mode_request_.end()) { to_chained_mode_request_.push_back(following_ctrl_it->info.name); + // if it is a chainable controller, make the reference interfaces available on preactivation + // (This is needed when you activate a couple of chainable controller altogether) + resource_manager_->make_controller_reference_interfaces_available( + following_ctrl_it->info.name); RCLCPP_DEBUG( get_logger(), "Adding controller '%s' in 'to chained mode' request.", following_ctrl_it->info.name.c_str());