diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 46797b0748..83179d2224 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2320,7 +2320,7 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; - std::vector failed_controllers_list; + rt_buffer_.deactivate_controllers_list.clear(); for (const auto & loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information @@ -2396,21 +2396,18 @@ controller_interface::return_type ControllerManager::update( if (controller_ret != controller_interface::return_type::OK) { - failed_controllers_list.push_back(loaded_controller.info.name); + rt_buffer_.deactivate_controllers_list.push_back(loaded_controller.info.name); ret = controller_ret; } } } } - if (!failed_controllers_list.empty()) + if (!rt_buffer_.deactivate_controllers_list.empty()) { - const auto FALLBACK_STACK_MAX_SIZE = 500; - std::vector active_controllers_using_interfaces(failed_controllers_list); - active_controllers_using_interfaces.reserve(FALLBACK_STACK_MAX_SIZE); - std::vector cumulative_fallback_controllers; - cumulative_fallback_controllers.reserve(FALLBACK_STACK_MAX_SIZE); + rt_buffer_.fallback_controllers_list.clear(); + rt_buffer_.activate_controllers_using_interfaces_list.clear(); - for (const auto & failed_ctrl : failed_controllers_list) + for (const auto & failed_ctrl : rt_buffer_.deactivate_controllers_list) { auto ctrl_it = std::find_if( rt_controller_list.begin(), rt_controller_list.end(), @@ -2419,52 +2416,36 @@ controller_interface::return_type ControllerManager::update( { for (const auto & fallback_controller : ctrl_it->info.fallback_controllers_names) { - cumulative_fallback_controllers.push_back(fallback_controller); + rt_buffer_.fallback_controllers_list.push_back(fallback_controller); get_active_controllers_using_command_interfaces_of_controller( - fallback_controller, rt_controller_list, active_controllers_using_interfaces); + fallback_controller, rt_controller_list, + rt_buffer_.activate_controllers_using_interfaces_list); } } } - std::string controllers_string; - controllers_string.reserve(500); - for (const auto & controller : failed_controllers_list) - { - controllers_string.append(controller); - controllers_string.append(" "); - } + RCLCPP_ERROR( get_logger(), "Deactivating controllers : [ %s] as their update resulted in an error!", - controllers_string.c_str()); - if (active_controllers_using_interfaces.size() > failed_controllers_list.size()) - { - controllers_string.clear(); - for (size_t i = failed_controllers_list.size(); - i < active_controllers_using_interfaces.size(); i++) - { - controllers_string.append(active_controllers_using_interfaces[i]); - controllers_string.append(" "); - } - RCLCPP_ERROR_EXPRESSION( - get_logger(), !controllers_string.empty(), - "Deactivating controllers : [ %s] using the command interfaces needed for the fallback " - "controllers to activate.", - controllers_string.c_str()); - } - if (!cumulative_fallback_controllers.empty()) - { - controllers_string.clear(); - for (const auto & controller : cumulative_fallback_controllers) - { - controllers_string.append(controller); - controllers_string.append(" "); - } - RCLCPP_ERROR( - get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str()); - } - deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); - if (!cumulative_fallback_controllers.empty()) + rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !rt_buffer_.activate_controllers_using_interfaces_list.empty(), + "Deactivating controllers : [ %s] using the command interfaces needed for the fallback " + "controllers to activate.", + rt_buffer_.get_concatenated_string(rt_buffer_.activate_controllers_using_interfaces_list) + .c_str()); + RCLCPP_ERROR_EXPRESSION( + get_logger(), !rt_buffer_.fallback_controllers_list.empty(), + "Activating fallback controllers : [ %s]", + rt_buffer_.get_concatenated_string(rt_buffer_.fallback_controllers_list).c_str()); + std::for_each( + rt_buffer_.activate_controllers_using_interfaces_list.begin(), + rt_buffer_.activate_controllers_using_interfaces_list.end(), + [this](const std::string & controller) + { add_element_to_list(rt_buffer_.deactivate_controllers_list, controller); }); + deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); + if (!rt_buffer_.fallback_controllers_list.empty()) { - activate_controllers(rt_controller_list, cumulative_fallback_controllers); + activate_controllers(rt_controller_list, rt_buffer_.fallback_controllers_list); } }