diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7acbe20166..82118d7bed 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -590,6 +590,38 @@ class ControllerManager : public rclcpp::Node }; SwitchParams switch_params_; + + struct RTBufferVariables + { + RTBufferVariables() + { + deactivate_controllers_list.reserve(1000); + activate_controllers_using_interfaces_list.reserve(1000); + fallback_controllers_list.reserve(1000); + concatenated_string.reserve(5000); + } + + const std::string & get_concatenated_string( + const std::vector & strings, bool clear_string = true) + { + if (clear_string) + { + concatenated_string.clear(); + } + for (const auto & str : strings) + { + concatenated_string.append(str); + concatenated_string.append(" "); + } + return concatenated_string; + } + + std::vector deactivate_controllers_list; + std::vector activate_controllers_using_interfaces_list; + std::vector fallback_controllers_list; + std::string concatenated_string; + }; + RTBufferVariables rt_buffer_; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 34566f95b3..0f323e9de3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2327,36 +2327,26 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration & if (!ok) { - std::vector stop_request = {}; - std::string failed_hardware_string; - failed_hardware_string.reserve(500); + rt_buffer_.deactivate_controllers_list.clear(); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { - failed_hardware_string.append(hardware_name); - failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); - stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); - } - std::string stop_request_string; - stop_request_string.reserve(500); - for (const auto & controller : stop_request) - { - stop_request_string.append(controller); - stop_request_string.append(" "); + rt_buffer_.deactivate_controllers_list.insert( + rt_buffer_.deactivate_controllers_list.end(), controllers.begin(), controllers.end()); } RCLCPP_ERROR( get_logger(), "Deactivating following hardware components as their read cycle resulted in an error: [ %s]", - failed_hardware_string.c_str()); + rt_buffer_.get_concatenated_string(failed_hardware_names).c_str()); RCLCPP_ERROR_EXPRESSION( - get_logger(), !stop_request_string.empty(), + get_logger(), !rt_buffer_.deactivate_controllers_list.empty(), "Deactivating following controllers as their hardware components read cycle resulted in an " "error: [ %s]", - stop_request_string.c_str()); + rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - deactivate_controllers(rt_controller_list, stop_request); + deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); // TODO(destogl): do auto-start of broadcasters } } @@ -2429,7 +2419,7 @@ controller_interface::return_type ControllerManager::update( "configuration (use_sim_time parameter) and if a valid clock source is available"); } - 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 @@ -2527,21 +2517,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(), @@ -2550,52 +2537,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); } } @@ -2614,37 +2585,27 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration if (!ok) { - std::vector stop_request = {}; - std::string failed_hardware_string; - failed_hardware_string.reserve(500); + rt_buffer_.deactivate_controllers_list.clear(); // Determine controllers to stop for (const auto & hardware_name : failed_hardware_names) { - failed_hardware_string.append(hardware_name); - failed_hardware_string.append(" "); auto controllers = resource_manager_->get_cached_controllers_to_hardware(hardware_name); - stop_request.insert(stop_request.end(), controllers.begin(), controllers.end()); - } - std::string stop_request_string; - stop_request_string.reserve(500); - for (const auto & controller : stop_request) - { - stop_request_string.append(controller); - stop_request_string.append(" "); + rt_buffer_.deactivate_controllers_list.insert( + rt_buffer_.deactivate_controllers_list.end(), controllers.begin(), controllers.end()); } RCLCPP_ERROR( get_logger(), "Deactivating following hardware components as their write cycle resulted in an error: [ " "%s]", - failed_hardware_string.c_str()); + rt_buffer_.get_concatenated_string(failed_hardware_names).c_str()); RCLCPP_ERROR_EXPRESSION( - get_logger(), !stop_request_string.empty(), + get_logger(), !rt_buffer_.deactivate_controllers_list.empty(), "Deactivating following controllers as their hardware components write cycle resulted in an " "error: [ %s]", - stop_request_string.c_str()); + rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str()); std::vector & rt_controller_list = rt_controllers_wrapper_.update_and_get_used_by_rt_list(); - deactivate_controllers(rt_controller_list, stop_request); + deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list); // TODO(destogl): do auto-start of broadcasters } }