From 5deeaf6af3285f8f9865ab9fb9196bfc61437548 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 2 Sep 2020 14:07:57 +0000 Subject: [PATCH 01/27] Add controller_manager_msgs package --- controller_manager_msgs/CMakeLists.txt | 32 +++++++++++++++++++ controller_manager_msgs/package.xml | 27 ++++++++++++++++ .../srv/SwitchController.srv | 30 +++++++++++++++++ 3 files changed, 89 insertions(+) create mode 100644 controller_manager_msgs/CMakeLists.txt create mode 100644 controller_manager_msgs/package.xml create mode 100644 controller_manager_msgs/srv/SwitchController.srv diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt new file mode 100644 index 0000000000..fa491f5d7e --- /dev/null +++ b/controller_manager_msgs/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) + +project(controller_manager_msgs) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +set(srv_files + srv/SwitchController.srv +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces(${PROJECT_NAME} + ${srv_files} + DEPENDENCIES builtin_interfaces + ADD_LINTER_TESTS +) +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml new file mode 100644 index 0000000000..5b37d4f3a5 --- /dev/null +++ b/controller_manager_msgs/package.xml @@ -0,0 +1,27 @@ + + + + controller_manager_msgs + 0.0.0 + Messages and services for the controller manager. + Bence Magyar + BSD + http://ros.org/wiki/controller_manager_msgs + Stuart Glaser + + ament_cmake + rosidl_default_generators + + builtin_interfaces + + builtin_interfaces + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + ament_cmake + + diff --git a/controller_manager_msgs/srv/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv new file mode 100644 index 0000000000..8bfce99daa --- /dev/null +++ b/controller_manager_msgs/srv/SwitchController.srv @@ -0,0 +1,30 @@ +# The SwitchController service allows you stop a number of controllers +# and start a number of controllers, all in one single timestep of the +# controller_manager control loop. + +# To switch controllers, specify +# * the list of controller names to start, +# * the list of controller names to stop, and +# * the strictness (BEST_EFFORT or STRICT) +# * STRICT means that switching will fail if anything goes wrong (an invalid +# controller name, a controller that failed to start, etc. ) +# * BEST_EFFORT means that even when something goes wrong with on controller, +# the service will still try to start/stop the remaining controllers +# * start the controllers as soon as their hardware dependencies are ready, will +# wait for all interfaces to be ready otherwise +# * the timeout in seconds before aborting pending controllers. Zero for infinite + +# The return value "ok" indicates if the controllers were switched +# successfully or not. The meaning of success depends on the +# specified strictness. + + +string[] start_controllers +string[] stop_controllers +int32 strictness +int32 BEST_EFFORT=1 +int32 STRICT=2 +bool start_asap +float64 timeout +--- +bool ok From f00ecec63a8b2ef0388eb64a0d712b7039f6b925 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 2 Sep 2020 14:08:14 +0000 Subject: [PATCH 02/27] Add switch and unload controller functionality --- controller_manager/CMakeLists.txt | 2 + .../controller_manager/controller_manager.hpp | 59 +- controller_manager/package.xml | 1 + controller_manager/src/controller_manager.cpp | 522 +++++++++++++++++- .../test/test_controller_manager.cpp | 8 +- .../test/test_load_controller.cpp | 438 +++++++++++++++ 6 files changed, 1017 insertions(+), 13 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index db1796da8c..af41468b4e 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake) find_package(ament_cmake_core REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(controller_interface REQUIRED) +find_package(controller_manager_msgs REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -23,6 +24,7 @@ target_include_directories(controller_manager PRIVATE include) ament_target_dependencies(controller_manager ament_index_cpp controller_interface + controller_manager_msgs hardware_interface pluginlib rclcpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index be7d4bdc7c..87460bc57a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -52,6 +52,10 @@ class ControllerManager : public rclcpp::Node const std::string & controller_name, const std::string & controller_type); + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type unload_controller( + const std::string & controller_name); + CONTROLLER_MANAGER_PUBLIC std::vector> get_loaded_controllers() const; @@ -75,6 +79,13 @@ class ControllerManager : public rclcpp::Node return add_controller_impl(controller, controller_name); } + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type + switch_controller( + const std::vector & start_controllers, + const std::vector & stop_controllers, + int strictness, bool start_asap, const rclcpp::Duration & timeout); + CONTROLLER_MANAGER_PUBLIC controller_interface::return_type update(); @@ -102,11 +113,57 @@ class ControllerManager : public rclcpp::Node std::shared_ptr controller, const std::string & controller_name); + CONTROLLER_MANAGER_PUBLIC + controller_interface::ControllerInterface * get_controller_by_name(const std::string & name); + + CONTROLLER_MANAGER_PUBLIC + void manage_switch(); + + CONTROLLER_MANAGER_PUBLIC + void stop_controllers(); + + CONTROLLER_MANAGER_PUBLIC + void start_controllers(); + + CONTROLLER_MANAGER_PUBLIC + void start_controllers_asap(); + private: std::shared_ptr hw_; std::shared_ptr executor_; std::vector loaders_; - std::vector> loaded_controllers_; + + /** \name Controllers List + * The controllers list is double-buffered to avoid needing to lock the + * real-time thread when switching controllers in the non-real-time thread. + *\{*/ + /// Mutex protecting the current controllers list + std::recursive_mutex controllers_lock_; + std::vector> controllers_lists_[2]; + /// The index of the current controllers list + int current_controllers_list_ = {0}; + /// The index of the controllers list being used in the real-time thread. + int used_by_realtime_ = {-1}; + + + std::vector start_request_, stop_request_; +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING +// std::list switch_start_list_, switch_stop_list_; +#endif + + struct SwitchParams + { + bool do_switch = {false}; + bool started = {false}; + rclcpp::Time init_time = {rclcpp::Time::max()}; + + // Switch options + int strictness = {0}; + bool start_asap = {false}; + rclcpp::Duration timeout = rclcpp::Duration{0, 0}; + }; + + SwitchParams switch_params_; }; } // namespace controller_manager diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 7189108384..ea8081722a 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -11,6 +11,7 @@ ament_index_cpp controller_interface + controller_manager_msgs hardware_interface pluginlib rclcpp diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 88a722e1ec..a2bdc4e492 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -14,6 +14,7 @@ #include "controller_manager/controller_manager.hpp" +#include #include #include #include @@ -21,6 +22,7 @@ #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_loader_pluginlib.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -29,6 +31,11 @@ namespace controller_manager { +bool isControllerRunning(controller_interface::ControllerInterface & controller) +{ + return controller.get_lifecycle_node()->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; +} ControllerManager::ControllerManager( std::shared_ptr hw, std::shared_ptr executor, @@ -65,10 +72,75 @@ ControllerManager::load_controller( return add_controller_impl(controller, controller_name); } +controller_interface::return_type ControllerManager::unload_controller( + const std::string & controller_name) +{ + // get reference to controller list + int free_controllers_list = (current_controllers_list_ + 1) % 2; + while (rclcpp::ok() && free_controllers_list == used_by_realtime_) { + if (!rclcpp::ok()) { + return controller_interface::return_type::ERROR; + } + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } + std::vector> + & from = controllers_lists_[current_controllers_list_], + & to = controllers_lists_[free_controllers_list]; + to.clear(); + + // Transfers the running controllers over, skipping the one to be removed and the running ones. + bool removed = false; + for (const auto & controller : from) { + if (controller->get_lifecycle_node()->get_name() == controller_name) { + if (isControllerRunning(*controller)) { + to.clear(); + RCLCPP_ERROR( + get_logger(), + "Could not unload controller with name '%s' because it is still running", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + controller->get_lifecycle_node()->cleanup(); + removed = true; + } else { + to.push_back(controller); + } + } + + // Fails if we could not remove the controllers + if (!removed) { + to.clear(); + RCLCPP_ERROR( + get_logger(), + "Could not unload controller with name '%s' because no controller with this name exists", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + + + // Destroys the old controllers list when the realtime thread is finished with it. + RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list"); + int former_current_controllers_list_ = current_controllers_list_; + current_controllers_list_ = free_controllers_list; + while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) { + if (!rclcpp::ok()) { + return controller_interface::return_type::ERROR; + } + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } + RCLCPP_DEBUG(get_logger(), "Destruct controller"); + from.clear(); + RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); + + RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); + return controller_interface::return_type::SUCCESS; +} + std::vector> ControllerManager::get_loaded_controllers() const { - return loaded_controllers_; + return controllers_lists_[current_controllers_list_]; } void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) @@ -76,29 +148,457 @@ void ControllerManager::register_controller_loader(ControllerLoaderInterfaceShar loaders_.push_back(loader); } +controller_interface::return_type ControllerManager::switch_controller( + const std::vector & start_controllers, + const std::vector & stop_controllers, int strictness, bool start_asap, + const rclcpp::Duration & timeout) +{ + switch_params_ = SwitchParams(); + + if (!stop_request_.empty() || !start_request_.empty()) { + RCLCPP_FATAL( + get_logger(), + "The internal stop and start request lists are not empty at the beginning of the " + "switchController() call. This should not happen."); + } + + if (strictness == 0) { + RCLCPP_WARN( + get_logger(), "Controller Manager: To switch controllers you need to specify a " + "strictness level of controller_manager_msgs::SwitchController::STRICT " + "(%d) or ::BEST_EFFORT (%d). Defaulting to ::BEST_EFFORT.", + controller_manager_msgs::srv::SwitchController::Request::STRICT, + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT); + strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + } + + RCLCPP_DEBUG(get_logger(), "switching controllers:"); + for (const auto & controller : start_controllers) { + RCLCPP_DEBUG(get_logger(), "- starting controller '%s'", controller.c_str()); + } + for (const auto & controller : stop_controllers) { + RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str()); + } + + // lock controllers + std::lock_guard guard(controllers_lock_); + + controller_interface::ControllerInterface * ct; + // list all controllers to stop + for (const auto & controller : stop_controllers) { + ct = get_controller_by_name(controller); + if (ct == nullptr) { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + RCLCPP_ERROR( + get_logger(), + "Could not stop controller with name '%s' because no controller with this name exists", + controller.c_str()); + stop_request_.clear(); + return controller_interface::return_type::ERROR; + } else { + RCLCPP_DEBUG( + get_logger(), + "Could not stop controller with name '%s' because no controller with this name exists", + controller.c_str()); + } + } else { + RCLCPP_DEBUG( + get_logger(), + "Found controller '%s' that needs to be stopped in list of controllers", + controller.c_str()); + stop_request_.push_back(ct); + } + } + RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size()); + + // list all controllers to start + for (const auto & controller : start_controllers) { + ct = get_controller_by_name(controller); + if (ct == nullptr) { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + RCLCPP_ERROR( + get_logger(), + "Could not start controller with name '%s' because no controller with this name exists", + controller.c_str()); + stop_request_.clear(); + start_request_.clear(); + return controller_interface::return_type::ERROR; + } else { + RCLCPP_DEBUG( + get_logger(), + "Could not start controller with name '%s' because no controller with this name exists", + controller.c_str()); + } + } else { + RCLCPP_DEBUG( + get_logger(), "Found controller '%s' that needs to be started in list of controllers", + controller.c_str()); + start_request_.push_back(ct); + } + } + RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size()); + +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + // Do the resource management checking + std::list info_list; + switch_start_list_.clear(); + switch_stop_list_.clear(); +#endif + + const auto & controllers = controllers_lists_[current_controllers_list_]; + for (const auto & controller : controllers) { + bool in_stop_list = false; + for (const auto & request : stop_request_) { + if (request == controller.get()) { + in_stop_list = true; + break; + } + } + + bool in_start_list = false; + for (const auto & request : start_request_) { + if (request == controller.get()) { + in_start_list = true; + break; + } + } + + + const bool is_running = isControllerRunning(*controller); + + if (!is_running && in_stop_list) { // check for double stop + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Could not stop controller '" << controller->get_lifecycle_node()->get_name() << + "' since it is not running"); + stop_request_.clear(); + start_request_.clear(); + return controller_interface::return_type::ERROR; + } else { + RCLCPP_DEBUG_STREAM( + get_logger(), + "Could not stop controller '" << controller->get_lifecycle_node()->get_name() << + "' since it is not running"); + in_stop_list = false; + stop_request_.erase( + std::remove( + stop_request_.begin(), stop_request_.end(), + controller.get()), stop_request_.end()); + } + } + + if (is_running && !in_stop_list && in_start_list) { // check for doubled start + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + RCLCPP_ERROR_STREAM( + get_logger(), + "Controller '" << controller->get_lifecycle_node()->get_name() << + "' is already running"); + stop_request_.clear(); + start_request_.clear(); + return controller_interface::return_type::ERROR; + } else { + RCLCPP_DEBUG_STREAM( + get_logger(), + "Could not start controller '" << controller->get_lifecycle_node()->get_name() << + "' since it is already running "); + in_start_list = false; + start_request_.erase( + std::remove( + start_request_.begin(), start_request_.end(), + controller.get()), start_request_.end()); + } + } + +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + if (is_running && in_stop_list && !in_start_list) { // running and real stop + switch_stop_list_.push_back(info); + } else if (!is_running && !in_stop_list && in_start_list) { // start, but no restart + switch_start_list_.push_back(info); + } + + bool add_to_list = is_running; + if (in_stop_list) { + add_to_list = false; + } + if (in_start_list) { + add_to_list = true; + } + + if (add_to_list) { + info_list.push_back(info); + } +#endif + } + +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + bool in_conflict = robot_hw_->checkForConflict(info_list); + if (in_conflict) { + RCLCPP_ERROR(get_logger(), "Could not switch controllers, due to resource conflict"); + stop_request_.clear(); + start_request_.clear(); + return controller_interface::return_type::ERROR; + } + + if (!robot_hw_->prepareSwitch(switch_start_list_, switch_stop_list_)) { + RCLCPP_ERROR( + get_logger(), + "Could not switch controllers. The hardware interface combination " + "for the requested controllers is unfeasible."); + stop_request_.clear(); + start_request_.clear(); + return controller_interface::return_type::ERROR; + } +#endif + + if (start_request_.empty() && stop_request_.empty()) { + RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch"); + return controller_interface::return_type::SUCCESS; + } + + // start the atomic controller switching + switch_params_.strictness = strictness; + switch_params_.start_asap = start_asap; + switch_params_.init_time = rclcpp::Clock().now(); + switch_params_.timeout = timeout; + switch_params_.do_switch = true; + + + // wait until switch is finished + RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop"); + while (rclcpp::ok() && switch_params_.do_switch) { + if (!rclcpp::ok()) { + return controller_interface::return_type::ERROR; + } + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + start_request_.clear(); + stop_request_.clear(); + + RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); + return controller_interface::return_type::SUCCESS; +} + std::shared_ptr ControllerManager::add_controller_impl( std::shared_ptr controller, const std::string & controller_name) { + // get reference to controller list + int free_controllers_list = (current_controllers_list_ + 1) % 2; + while (rclcpp::ok() && free_controllers_list == used_by_realtime_) { + if (!rclcpp::ok()) { + return nullptr; + } + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } + std::vector> + & from = controllers_lists_[current_controllers_list_], + & to = controllers_lists_[free_controllers_list]; + to.clear(); + + // Copy all controllers from the 'from' list to the 'to' list + for (const auto & controller : from) { + to.push_back(controller); + } + + // Checks that we're not duplicating controllers + for (const auto & controller : to) { + if (controller->get_lifecycle_node()->get_name() == controller_name) { + to.clear(); + RCLCPP_ERROR( + get_logger(), + "A controller named '%s' was already loaded inside the controller manager", + controller_name.c_str()); + return nullptr; + } + } + controller->init(hw_, controller_name); + + // TODO(v-lopez) this should only be done if controller_manager is configured. + // Probably the whole load_controller part should fail if the controller_manager + // is not configured, should it implement a LifecycleNodeInterface + controller->get_lifecycle_node()->configure(); executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface()); + to.emplace_back(controller); + + // Destroys the old controllers list when the realtime thread is finished with it. + int former_current_controllers_list_ = current_controllers_list_; + current_controllers_list_ = free_controllers_list; + while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) { + if (!rclcpp::ok()) { + return nullptr; + } + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } + from.clear(); - loaded_controllers_.emplace_back(controller); - return loaded_controllers_.back(); + return to.back(); +} + +controller_interface::ControllerInterface * ControllerManager::get_controller_by_name( + const std::string & name) +{ + // Lock recursive mutex in this context + std::lock_guard guard(controllers_lock_); + + for (const auto & controller : controllers_lists_[current_controllers_list_]) { + if (controller->get_lifecycle_node()->get_name() == name) { + return controller.get(); + } + } + return nullptr; +} + +void ControllerManager::manage_switch() +{ +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + // switch hardware interfaces (if any) + if (!switch_params_.started) { + robot_hw_->doSwitch(switch_start_list_, switch_stop_list_); + switch_params_.started = true; + } +#endif + + stop_controllers(); + + // start controllers once the switch is fully complete + if (!switch_params_.start_asap) { + start_controllers(); + } else { + // start controllers as soon as their required joints are done switching + start_controllers_asap(); + } +} + +void ControllerManager::stop_controllers() +{ + // stop controllers + for (const auto & request : stop_request_) { + if (isControllerRunning(*request)) { + const auto new_state = request->get_lifecycle_node()->deactivate(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR( + get_logger(), + "After deactivating, controller %s is in state %s, expected Inactive", + request->get_lifecycle_node()->get_name(), + new_state.label().c_str()); + } + } + } +} + +void ControllerManager::start_controllers() +{ +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + // start controllers + if (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::DONE) { + for (const auto & request : start_request_) { + request->startRequest(time); + } + + switch_params_.do_switch = false; + } else if (// NOLINT + (robot_hw_->switchResult() == hardware_interface::RobotHW::SwitchState::ERROR) || + (switch_params_.timeout > 0.0 && + (time - switch_params_.init_time).toSec() > switch_params_.timeout)) + { + // abort controllers in case of error or timeout (if set) + for (const auto & request : start_request_) { + request->abortRequest(time); + } + + switch_params_.do_switch = false; + } else { + // wait controllers + for (const auto & request : start_request_) { + request->waitRequest(time); + } + } +#else + // Dummy implementation, replace with the code above when migrated + for (const auto & request : start_request_) { + const auto new_state = request->get_lifecycle_node()->activate(); + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_ERROR( + get_logger(), + "After activating, controller %s is in state %s, expected Active", + request->get_lifecycle_node()->get_name(), + new_state.label().c_str()); + } + } + // All controllers started, switching done + switch_params_.do_switch = false; +#endif +} + +void ControllerManager::start_controllers_asap() +{ +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + // start controllers if possible + for (const auto & request : start_request_) { + if (!isControllerRunning(*request)) { + // find the info from this controller + for (const auto & controller : controllers_lists_[current_controllers_list_]) { + if (request == controller.c.get()) { + // ready to start + if (robot_hw_->switchResult(controller.info) == + hardware_interface::RobotHW::SwitchState::DONE) + { + request->startRequest(time); + } else if ((robot_hw_->switchResult(controller.info) == // NOLINT + hardware_interface::RobotHW::SwitchState::ERROR) || + (switch_params_.timeout > 0.0 && + (time - switch_params_.init_time).toSec() > switch_params_.timeout)) // NOLINT + { + // abort on error or timeout (if set) + request->abortRequest(time); + } else { + // controller is waiting + request->waitRequest(time); + } + } + continue; + } + } + } + + // all needed controllers started or aborted, switch done + if (std::all_of( + start_request_.begin(), start_request_.end(), + [](controller_interface::ControllerBase * request) { + return request->isRunning() || request->isAborted(); + })) + { + switch_params_.do_switch = false; + } +#else + // Dummy implementation, replace with the code above when migrated + start_controllers(); +#endif } controller_interface::return_type ControllerManager::update() { + used_by_realtime_ = current_controllers_list_; + auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { - auto controller_ret = loaded_controller->update(); - if (controller_ret != controller_interface::return_type::SUCCESS) { - ret = controller_ret; + for (auto loaded_controller : controllers_lists_[used_by_realtime_]) { + // TODO(v-lopez) we could cache this information + if (isControllerRunning(*loaded_controller)) { + auto controller_ret = loaded_controller->update(); + if (controller_ret != controller_interface::return_type::SUCCESS) { + ret = controller_ret; + } } } + // there are controllers to start/stop + if (switch_params_.do_switch) { + manage_switch(); + } return ret; } @@ -106,7 +606,7 @@ controller_interface::return_type ControllerManager::configure() const { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { + for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { auto controller_state = loaded_controller->get_lifecycle_node()->configure(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { ret = controller_interface::return_type::ERROR; @@ -120,7 +620,7 @@ controller_interface::return_type ControllerManager::activate() const { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { + for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { auto controller_state = loaded_controller->get_lifecycle_node()->activate(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { ret = controller_interface::return_type::ERROR; @@ -134,7 +634,7 @@ controller_interface::return_type ControllerManager::deactivate() const { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { + for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { auto controller_state = loaded_controller->get_lifecycle_node()->deactivate(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { ret = controller_interface::return_type::ERROR; @@ -148,7 +648,7 @@ controller_interface::return_type ControllerManager::cleanup() const { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { + for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { auto controller_state = loaded_controller->get_lifecycle_node()->cleanup(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { ret = controller_interface::return_type::ERROR; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 89e2081873..dd7230d945 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -54,7 +54,10 @@ TEST_F(TestControllerManager, controller_lifecycle) { EXPECT_EQ(1u, cm.get_loaded_controllers().size()); EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.update()); - EXPECT_EQ(1u, test_controller->internal_counter); + EXPECT_EQ( + 0u, + test_controller->internal_counter) << + "Update should not reached an unconfigured and deactivated controller"; EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.configure()); EXPECT_EQ( @@ -66,6 +69,9 @@ TEST_F(TestControllerManager, controller_lifecycle) { lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.update()); + EXPECT_EQ(1u, test_controller->internal_counter); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.deactivate()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index b38b3eebee..581b18b03a 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -16,12 +16,15 @@ #include #include #include +#include #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_loader_interface.hpp" #include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" + #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" @@ -33,6 +36,9 @@ using ::testing::_; using ::testing::Return; +constexpr auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; +constexpr auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + class TestControllerManager : public ::testing::Test { public: @@ -49,6 +55,7 @@ class TestControllerManager : public ::testing::Test executor = std::make_shared(); } + std::shared_ptr robot; std::shared_ptr executor; }; @@ -168,3 +175,434 @@ TEST_F(TestControllerManager, register_controller_loader) lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller->get_lifecycle_node()->get_current_state().id()); } + +TEST_F(TestControllerManager, switch_controller_empty) +{ + auto cm = std::make_shared( + robot, executor, + "test_controller_manager"); + std::string controller_type = "test_controller"; + + // load the controller with name1 + std::string controller_name1 = "test_controller1"; + ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); + EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + + const auto UNSPECIFIED = 0; + + std::vector start_controllers = {}; + std::vector stop_controllers = {}; + + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)) + ) << "Switch with no controllers specified"; + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + BEST_EFFORT, true, rclcpp::Duration(0, 0)) + ) << "Switch with no controllers specified"; + + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + UNSPECIFIED, true, rclcpp::Duration(0, 0)) + ) << "Switch with no controllers specified, unspecified strictness defaults to BEST_EFFORT"; + + + start_controllers = {"nonexistent_controller"}; + stop_controllers = {}; + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm->switch_controller( + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)) + ) << "STRICT switch with nonexistent controller specified"; + + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + BEST_EFFORT, true, rclcpp::Duration(0, 0)) + ) << "BEST_EFFORT switch with nonexistent controller specified"; + + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + UNSPECIFIED, true, rclcpp::Duration(0, 0)) + ) << "Unspecified switch with nonexistent controller specified, defaults to BEST_EFFORT"; + + // From now on will only test STRICT and BEST_EFFORT + + + start_controllers = {}; + stop_controllers = {"nonexistent_controller"}; + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm->switch_controller( + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)) + ) << "STRICT switch with nonexistent controller specified"; + + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + BEST_EFFORT, true, rclcpp::Duration(0, 0)) + ) << "BEST_EFFORT switch with nonexistent controller specified"; + + + start_controllers = {"nonexistent_controller"}; + stop_controllers = {"nonexistent_controller"}; + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm->switch_controller( + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)) + ) << "STRICT switch with nonexistent controller specified"; + + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + BEST_EFFORT, true, rclcpp::Duration(0, 0)) + ) << "BEST_EFFORT switch with nonexistent controller specified"; + + + auto switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); +} + +TEST_F(TestControllerManager, switch_controller) +{ + auto cm = std::make_shared( + robot, executor, + "test_controller_manager"); + cm->configure(); + std::string controller_type = "test_controller"; + + // load the controller with name1 + std::string controller_name1 = "test_controller1"; + ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); + EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + std::shared_ptr abstract_test_controller1 = + cm->get_loaded_controllers()[0]; + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + + + { // Test stopping an stopped controller + std::vector start_controllers = {}; + std::vector stop_controllers = {controller_name1}; + + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm->switch_controller( + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)) + ) << "STRICT switch with stopped controller specified"; + + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + cm->switch_controller( + start_controllers, stop_controllers, + BEST_EFFORT, true, rclcpp::Duration(0, 0)) + ) << "BEST_EFFORT switch stopped controller specified"; + } + + + { // STRICT Combination of valid controller + invalid controller + std::vector start_controllers = {controller_name1, "nonexistent_controller"}; + std::vector stop_controllers = {}; + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm->switch_controller( + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)) + ) << "STRICT switch with nonexistent controller specified"; + + start_controllers = {controller_name1}; + stop_controllers = {"nonexistent_controller"}; + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm->switch_controller( + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)) + ) << "STRICT switch with nonexistent controller specified"; + } + + // Only testing with STRICT now for simplicity + { // Test starting an stopped controller, and stopping afterwards + RCLCPP_INFO( + cm->get_logger(), + "Starting stopped controller"); + std::vector start_controllers = {controller_name1}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + + + // Stop controller + start_controllers = {}; + stop_controllers = {controller_name1}; + RCLCPP_INFO( + cm->get_logger(), + "Stopping started controller"); + switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + } +} + +TEST_F(TestControllerManager, switch_multiple_controllers) +{ + auto cm = std::make_shared( + robot, executor, + "test_controller_manager"); + cm->configure(); + std::string controller_type = "test_controller"; + + // load the controller with name1 + std::string controller_name1 = "test_controller1"; + std::string controller_name2 = "test_controller2"; + ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); + ASSERT_NO_THROW(cm->load_controller(controller_name2, controller_type)); + EXPECT_EQ(2u, cm->get_loaded_controllers().size()); + std::shared_ptr abstract_test_controller1 = + cm->get_loaded_controllers()[0]; + std::shared_ptr abstract_test_controller2 = + cm->get_loaded_controllers()[1]; + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + + // Only testing with STRICT now for simplicity + { // Test starting an stopped controller, and stopping afterwards + RCLCPP_INFO( + cm->get_logger(), + "Starting stopped controller #1"); + std::vector start_controllers = {controller_name1}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + + // Stop controller 1, start controller 2 + start_controllers = {controller_name2}; + stop_controllers = {controller_name1}; + RCLCPP_INFO( + cm->get_logger(), + "Stopping controller #1, starting controller #2"); + switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + + start_controllers = {}; + stop_controllers = {controller_name2}; + RCLCPP_INFO( + cm->get_logger(), + "Stopping controller #1, starting controller #2"); + switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + } +} + +TEST_F(TestControllerManager, controller_lifecycle_states) +{ + auto cm = std::make_shared( + robot, executor, + "test_controller_manager"); + cm->configure(); + std::string controller_type = "test_controller"; + + // load the controller with name1 + std::string controller_name1 = "test_controller1"; + ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); + EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + std::shared_ptr abstract_test_controller1 = + cm->get_loaded_controllers()[0]; + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + + RCLCPP_INFO( + cm->get_logger(), + "Starting stopped controller"); + std::vector start_controllers = {controller_name1}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + + + // Stop controller + start_controllers = {}; + stop_controllers = {controller_name1}; + RCLCPP_INFO( + cm->get_logger(), + "Stopping started controller"); + switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + + + RCLCPP_INFO( + cm->get_logger(), + "Unloading controller"); + EXPECT_EQ(2, abstract_test_controller1.use_count()); + + auto unload_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::unload_controller, cm, + controller_name1); + + ASSERT_EQ( + std::future_status::timeout, + unload_future.wait_for(std::chrono::milliseconds(100))) << + "unload_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + unload_future.get() + ); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + EXPECT_EQ(1, abstract_test_controller1.use_count()); +} From 91e1767e0d2cb3a218be5e3844b1238bd8a7fbdf Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 2 Sep 2020 14:08:14 +0000 Subject: [PATCH 03/27] Use ControllerSpec for storing controller type and name --- controller_manager/CMakeLists.txt | 1 + .../controller_manager/controller_manager.hpp | 38 +++++---- .../controller_manager/controller_spec.hpp | 47 +++++++++++ controller_manager/src/controller_manager.cpp | 81 ++++++++++--------- .../test/test_controller_manager.cpp | 4 +- .../test/test_load_controller.cpp | 76 ++++++++--------- .../hardware_interface/controller_info.hpp | 57 +++++++++++++ 7 files changed, 209 insertions(+), 95 deletions(-) create mode 100644 controller_manager/include/controller_manager/controller_spec.hpp create mode 100644 hardware_interface/include/hardware_interface/controller_info.hpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index af41468b4e..e8613768e0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -102,6 +102,7 @@ ament_export_include_directories( ) ament_export_dependencies( controller_interface + controller_manager_msgs pluginlib ) ament_package() diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 87460bc57a..578fdd9a03 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -23,7 +23,9 @@ #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_loader_interface.hpp" +#include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" +#include "controller_manager_msgs/srv/list_controllers.hpp" #include "hardware_interface/robot_hardware.hpp" @@ -47,7 +49,7 @@ class ControllerManager : public rclcpp::Node ~ControllerManager() = default; CONTROLLER_MANAGER_PUBLIC - std::shared_ptr + controller_interface::ControllerInterfaceSharedPtr load_controller( const std::string & controller_name, const std::string & controller_type); @@ -57,14 +59,7 @@ class ControllerManager : public rclcpp::Node const std::string & controller_name); CONTROLLER_MANAGER_PUBLIC - std::vector> - get_loaded_controllers() const; - - [[deprecated( - "get_loaded_controller is deprecated, it has been renamed to get_loaded_controllers")]] - CONTROLLER_MANAGER_PUBLIC - std::vector> - get_loaded_controller() const; + std::vector get_loaded_controllers() const; CONTROLLER_MANAGER_PUBLIC void register_controller_loader(ControllerLoaderInterfaceSharedPtr loader); @@ -73,10 +68,16 @@ class ControllerManager : public rclcpp::Node typename T, typename std::enable_if::value, T>::type * = nullptr> - std::shared_ptr - add_controller(std::shared_ptr controller, std::string controller_name) + controller_interface::ControllerInterfaceSharedPtr + add_controller( + std::shared_ptr controller, std::string controller_name, + std::string controller_type) { - return add_controller_impl(controller, controller_name); + ControllerSpec controller_spec; + controller_spec.c = controller; + controller_spec.info.name = controller_name; + controller_spec.info.type = controller_type; + return add_controller_impl(controller_spec); } CONTROLLER_MANAGER_PUBLIC @@ -108,10 +109,8 @@ class ControllerManager : public rclcpp::Node protected: CONTROLLER_MANAGER_PUBLIC - std::shared_ptr - add_controller_impl( - std::shared_ptr controller, - const std::string & controller_name); + controller_interface::ControllerInterfaceSharedPtr + add_controller_impl(const ControllerSpec & controller); CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterface * get_controller_by_name(const std::string & name); @@ -128,6 +127,11 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void start_controllers_asap(); + CONTROLLER_MANAGER_PUBLIC + void list_controllers_srv_cb( + const std::shared_ptr request, + std::shared_ptr response); + private: std::shared_ptr hw_; std::shared_ptr executor_; @@ -139,7 +143,7 @@ class ControllerManager : public rclcpp::Node *\{*/ /// Mutex protecting the current controllers list std::recursive_mutex controllers_lock_; - std::vector> controllers_lists_[2]; + std::vector controllers_lists_[2]; /// The index of the current controllers list int current_controllers_list_ = {0}; /// The index of the controllers list being used in the real-time thread. diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp new file mode 100644 index 0000000000..af830bbe9d --- /dev/null +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -0,0 +1,47 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +/* + * Author: Wim Meeussen + */ + +#ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ +#define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ +#pragma GCC diagnostic ignored "-Wextra" + + +#include +#include +#include +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/controller_info.hpp" + +namespace controller_manager +{ + +/** \brief Controller Specification + * + * This struct contains both a pointer to a given controller, \ref c, as well + * as information about the controller, \ref info. + * + */ +struct ControllerSpec +{ + hardware_interface::ControllerInfo info; + controller_interface::ControllerInterfaceSharedPtr c; +}; + +} // namespace controller_manager +#endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a2bdc4e492..eed5d063bc 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -48,8 +48,7 @@ ControllerManager::ControllerManager( { } -std::shared_ptr -ControllerManager::load_controller( +controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { @@ -60,7 +59,7 @@ ControllerManager::load_controller( [&](auto loader) {return loader->is_available(controller_type);}); - std::shared_ptr controller(nullptr); + controller_interface::ControllerInterfaceSharedPtr controller(nullptr); if (it != loaders_.cend()) { controller = (*it)->create(controller_type); } else { @@ -68,8 +67,12 @@ ControllerManager::load_controller( RCLCPP_ERROR(get_logger(), "%s", error_msg.c_str()); throw std::runtime_error(error_msg); } + ControllerSpec controller_spec; + controller_spec.c = controller; + controller_spec.info.name = controller_name; + controller_spec.info.type = controller_type; - return add_controller_impl(controller, controller_name); + return add_controller_impl(controller_spec); } controller_interface::return_type ControllerManager::unload_controller( @@ -83,7 +86,7 @@ controller_interface::return_type ControllerManager::unload_controller( } std::this_thread::sleep_for(std::chrono::microseconds(200)); } - std::vector> + std::vector & from = controllers_lists_[current_controllers_list_], & to = controllers_lists_[free_controllers_list]; to.clear(); @@ -91,8 +94,8 @@ controller_interface::return_type ControllerManager::unload_controller( // Transfers the running controllers over, skipping the one to be removed and the running ones. bool removed = false; for (const auto & controller : from) { - if (controller->get_lifecycle_node()->get_name() == controller_name) { - if (isControllerRunning(*controller)) { + if (controller.info.name == controller_name) { + if (isControllerRunning(*controller.c)) { to.clear(); RCLCPP_ERROR( get_logger(), @@ -101,7 +104,7 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::ERROR; } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); - controller->get_lifecycle_node()->cleanup(); + controller.c->get_lifecycle_node()->cleanup(); removed = true; } else { to.push_back(controller); @@ -137,8 +140,7 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::SUCCESS; } -std::vector> -ControllerManager::get_loaded_controllers() const +std::vector ControllerManager::get_loaded_controllers() const { return controllers_lists_[current_controllers_list_]; } @@ -249,7 +251,7 @@ controller_interface::return_type ControllerManager::switch_controller( for (const auto & controller : controllers) { bool in_stop_list = false; for (const auto & request : stop_request_) { - if (request == controller.get()) { + if (request == controller.c.get()) { in_stop_list = true; break; } @@ -257,20 +259,20 @@ controller_interface::return_type ControllerManager::switch_controller( bool in_start_list = false; for (const auto & request : start_request_) { - if (request == controller.get()) { + if (request == controller.c.get()) { in_start_list = true; break; } } - const bool is_running = isControllerRunning(*controller); + const bool is_running = isControllerRunning(*controller.c); if (!is_running && in_stop_list) { // check for double stop if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR_STREAM( get_logger(), - "Could not stop controller '" << controller->get_lifecycle_node()->get_name() << + "Could not stop controller '" << controller.info.name << "' since it is not running"); stop_request_.clear(); start_request_.clear(); @@ -278,13 +280,13 @@ controller_interface::return_type ControllerManager::switch_controller( } else { RCLCPP_DEBUG_STREAM( get_logger(), - "Could not stop controller '" << controller->get_lifecycle_node()->get_name() << + "Could not stop controller '" << controller.info.name << "' since it is not running"); in_stop_list = false; stop_request_.erase( std::remove( stop_request_.begin(), stop_request_.end(), - controller.get()), stop_request_.end()); + controller.c.get()), stop_request_.end()); } } @@ -292,7 +294,7 @@ controller_interface::return_type ControllerManager::switch_controller( if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR_STREAM( get_logger(), - "Controller '" << controller->get_lifecycle_node()->get_name() << + "Controller '" << controller.info.name << "' is already running"); stop_request_.clear(); start_request_.clear(); @@ -300,13 +302,13 @@ controller_interface::return_type ControllerManager::switch_controller( } else { RCLCPP_DEBUG_STREAM( get_logger(), - "Could not start controller '" << controller->get_lifecycle_node()->get_name() << + "Could not start controller '" << controller.info.name << "' since it is already running "); in_start_list = false; start_request_.erase( std::remove( start_request_.begin(), start_request_.end(), - controller.get()), start_request_.end()); + controller.c.get()), start_request_.end()); } } @@ -379,10 +381,9 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::SUCCESS; } -std::shared_ptr +controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl( - std::shared_ptr controller, - const std::string & controller_name) + const ControllerSpec & controller) { // get reference to controller list int free_controllers_list = (current_controllers_list_ + 1) % 2; @@ -392,35 +393,35 @@ ControllerManager::add_controller_impl( } std::this_thread::sleep_for(std::chrono::microseconds(200)); } - std::vector> + std::vector & from = controllers_lists_[current_controllers_list_], & to = controllers_lists_[free_controllers_list]; to.clear(); // Copy all controllers from the 'from' list to the 'to' list - for (const auto & controller : from) { - to.push_back(controller); + for (const auto & from_controller : from) { + to.push_back(from_controller); } // Checks that we're not duplicating controllers - for (const auto & controller : to) { - if (controller->get_lifecycle_node()->get_name() == controller_name) { + for (const auto & to_controller : to) { + if (controller.info.name == to_controller.info.name) { to.clear(); RCLCPP_ERROR( get_logger(), "A controller named '%s' was already loaded inside the controller manager", - controller_name.c_str()); + controller.info.name.c_str()); return nullptr; } } - controller->init(hw_, controller_name); + controller.c->init(hw_, controller.info.name); // TODO(v-lopez) this should only be done if controller_manager is configured. // Probably the whole load_controller part should fail if the controller_manager // is not configured, should it implement a LifecycleNodeInterface - controller->get_lifecycle_node()->configure(); - executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface()); + controller.c->get_lifecycle_node()->configure(); + executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface()); to.emplace_back(controller); // Destroys the old controllers list when the realtime thread is finished with it. @@ -434,7 +435,7 @@ ControllerManager::add_controller_impl( } from.clear(); - return to.back(); + return to.back().c; } controller_interface::ControllerInterface * ControllerManager::get_controller_by_name( @@ -444,8 +445,8 @@ controller_interface::ControllerInterface * ControllerManager::get_controller_by std::lock_guard guard(controllers_lock_); for (const auto & controller : controllers_lists_[current_controllers_list_]) { - if (controller->get_lifecycle_node()->get_name() == name) { - return controller.get(); + if (controller.info.name == name) { + return controller.c.get(); } } return nullptr; @@ -587,8 +588,8 @@ ControllerManager::update() auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[used_by_realtime_]) { // TODO(v-lopez) we could cache this information - if (isControllerRunning(*loaded_controller)) { - auto controller_ret = loaded_controller->update(); + if (isControllerRunning(*loaded_controller.c)) { + auto controller_ret = loaded_controller.c->update(); if (controller_ret != controller_interface::return_type::SUCCESS) { ret = controller_ret; } @@ -607,7 +608,7 @@ ControllerManager::configure() const { auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { - auto controller_state = loaded_controller->get_lifecycle_node()->configure(); + auto controller_state = loaded_controller.c->get_lifecycle_node()->configure(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { ret = controller_interface::return_type::ERROR; } @@ -621,7 +622,7 @@ ControllerManager::activate() const { auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { - auto controller_state = loaded_controller->get_lifecycle_node()->activate(); + auto controller_state = loaded_controller.c->get_lifecycle_node()->activate(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { ret = controller_interface::return_type::ERROR; } @@ -635,7 +636,7 @@ ControllerManager::deactivate() const { auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { - auto controller_state = loaded_controller->get_lifecycle_node()->deactivate(); + auto controller_state = loaded_controller.c->get_lifecycle_node()->deactivate(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { ret = controller_interface::return_type::ERROR; } @@ -649,7 +650,7 @@ ControllerManager::cleanup() const { auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { - auto controller_state = loaded_controller->get_lifecycle_node()->cleanup(); + auto controller_state = loaded_controller.c->get_lifecycle_node()->cleanup(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { ret = controller_interface::return_type::ERROR; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index dd7230d945..f42d4c05ec 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -50,7 +50,9 @@ TEST_F(TestControllerManager, controller_lifecycle) { controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); auto test_controller = std::make_shared(); - auto abstract_test_controller = cm.add_controller(test_controller, "test_controller"); + auto abstract_test_controller = cm.add_controller( + test_controller, "test_controller", + "TestControllerType"); EXPECT_EQ(1u, cm.get_loaded_controllers().size()); EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.update()); diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 581b18b03a..45f159f7ee 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -86,14 +86,14 @@ TEST_F(TestControllerManager, load1_known_controller) ASSERT_NO_THROW(cm.load_controller("test_controller_01", "test_controller")); EXPECT_EQ(1u, cm.get_loaded_controllers().size()); - std::shared_ptr abstract_test_controller = + controller_manager::ControllerSpec abstract_test_controller = cm.get_loaded_controllers()[0]; - auto lifecycle_node = abstract_test_controller->get_lifecycle_node(); + auto lifecycle_node = abstract_test_controller.c->get_lifecycle_node(); lifecycle_node->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller.c->get_lifecycle_node()->get_current_state().id()); } TEST_F(TestControllerManager, load2_known_controller) @@ -105,27 +105,29 @@ TEST_F(TestControllerManager, load2_known_controller) std::string controller_name1 = "test_controller1"; ASSERT_NO_THROW(cm.load_controller(controller_name1, controller_type)); EXPECT_EQ(1u, cm.get_loaded_controllers().size()); - std::shared_ptr abstract_test_controller1 = + controller_manager::ControllerSpec abstract_test_controller1 = cm.get_loaded_controllers()[0]; EXPECT_STREQ( - controller_name1.c_str(), abstract_test_controller1->get_lifecycle_node()->get_name()); - abstract_test_controller1->get_lifecycle_node()->configure(); + controller_name1.c_str(), abstract_test_controller1.c->get_lifecycle_node()->get_name()); + abstract_test_controller1.c->get_lifecycle_node()->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); // load the same controller again with a different name std::string controller_name2 = "test_controller2"; ASSERT_NO_THROW(cm.load_controller(controller_name2, controller_type)); EXPECT_EQ(2u, cm.get_loaded_controllers().size()); - std::shared_ptr abstract_test_controller2 = + controller_manager::ControllerSpec abstract_test_controller2 = cm.get_loaded_controllers()[1]; EXPECT_STREQ( - controller_name2.c_str(), abstract_test_controller2->get_lifecycle_node()->get_name()); - abstract_test_controller2->get_lifecycle_node()->configure(); + controller_name2.c_str(), abstract_test_controller2.c->get_lifecycle_node()->get_name()); + EXPECT_STREQ( + controller_name2.c_str(), abstract_test_controller2.info.name.c_str()); + abstract_test_controller2.c->get_lifecycle_node()->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } TEST_F(TestControllerManager, update) @@ -133,14 +135,14 @@ TEST_F(TestControllerManager, update) controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); ASSERT_NO_THROW(cm.load_controller("test_controller_01", "test_controller")); - std::shared_ptr abstract_test_controller = + controller_manager::ControllerSpec abstract_test_controller = cm.get_loaded_controllers()[0]; - auto lifecycle_node = abstract_test_controller->get_lifecycle_node(); + auto lifecycle_node = abstract_test_controller.c->get_lifecycle_node(); lifecycle_node->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller.c->get_lifecycle_node()->get_current_state().id()); } TEST_F(TestControllerManager, register_controller_loader) @@ -166,14 +168,14 @@ TEST_F(TestControllerManager, register_controller_loader) ASSERT_NO_THROW(cm.load_controller(mock_controller_name, mock_controller_type)); EXPECT_EQ(1u, cm.get_loaded_controllers().size()); - std::shared_ptr abstract_test_controller = + controller_manager::ControllerSpec abstract_test_controller = cm.get_loaded_controllers()[0]; - auto lifecycle_node = abstract_test_controller->get_lifecycle_node(); + auto lifecycle_node = abstract_test_controller.c->get_lifecycle_node(); lifecycle_node->configure(); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller.c->get_lifecycle_node()->get_current_state().id()); } TEST_F(TestControllerManager, switch_controller_empty) @@ -293,12 +295,12 @@ TEST_F(TestControllerManager, switch_controller) std::string controller_name1 = "test_controller1"; ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); EXPECT_EQ(1u, cm->get_loaded_controllers().size()); - std::shared_ptr abstract_test_controller1 = + controller_manager::ControllerSpec abstract_test_controller1 = cm->get_loaded_controllers()[0]; ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); { // Test stopping an stopped controller @@ -366,7 +368,7 @@ TEST_F(TestControllerManager, switch_controller) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); // Stop controller @@ -393,7 +395,7 @@ TEST_F(TestControllerManager, switch_controller) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); } } @@ -411,17 +413,17 @@ TEST_F(TestControllerManager, switch_multiple_controllers) ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); ASSERT_NO_THROW(cm->load_controller(controller_name2, controller_type)); EXPECT_EQ(2u, cm->get_loaded_controllers().size()); - std::shared_ptr abstract_test_controller1 = + controller_manager::ControllerSpec abstract_test_controller1 = cm->get_loaded_controllers()[0]; - std::shared_ptr abstract_test_controller2 = + controller_manager::ControllerSpec abstract_test_controller2 = cm->get_loaded_controllers()[1]; ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); // Only testing with STRICT now for simplicity { // Test starting an stopped controller, and stopping afterwards @@ -448,10 +450,10 @@ TEST_F(TestControllerManager, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); // Stop controller 1, start controller 2 start_controllers = {controller_name2}; @@ -477,10 +479,10 @@ TEST_F(TestControllerManager, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); start_controllers = {}; stop_controllers = {controller_name2}; @@ -506,7 +508,7 @@ TEST_F(TestControllerManager, switch_multiple_controllers) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } } @@ -522,12 +524,12 @@ TEST_F(TestControllerManager, controller_lifecycle_states) std::string controller_name1 = "test_controller1"; ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); EXPECT_EQ(1u, cm->get_loaded_controllers().size()); - std::shared_ptr abstract_test_controller1 = + controller_manager::ControllerSpec abstract_test_controller1 = cm->get_loaded_controllers()[0]; ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); RCLCPP_INFO( cm->get_logger(), @@ -552,7 +554,7 @@ TEST_F(TestControllerManager, controller_lifecycle_states) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); // Stop controller @@ -579,13 +581,13 @@ TEST_F(TestControllerManager, controller_lifecycle_states) ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); RCLCPP_INFO( cm->get_logger(), "Unloading controller"); - EXPECT_EQ(2, abstract_test_controller1.use_count()); + EXPECT_EQ(2, abstract_test_controller1.c.use_count()); auto unload_future = std::async( std::launch::async, @@ -603,6 +605,6 @@ TEST_F(TestControllerManager, controller_lifecycle_states) ); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - abstract_test_controller1->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(1, abstract_test_controller1.use_count()); + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); + EXPECT_EQ(1, abstract_test_controller1.c.use_count()); } diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp new file mode 100644 index 0000000000..076bcbb7af --- /dev/null +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -0,0 +1,57 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2012, hiDOF, INC and Willow Garage, Inc +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// * Neither the name of Willow Garage Inc, hiDOF Inc, nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +////////////////////////////////////////////////////////////////////////////// + +#pragma once + + +#include +// TODO(v-lopez) +//#include +//#include + +namespace hardware_interface +{ + +/** \brief Controller Information + * + * This struct contains information about a given controller. + * + */ +struct ControllerInfo +{ + /** Controller name. */ + std::string name; + + /** Controller type. */ + std::string type; + + // TODO(v-lopez) + /** Claimed resources, grouped by the hardware interface they belong to. */ +// std::vector claimed_resources; +}; + +} From bef08e23b378b09d89c4612a2385a9060373e6df Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 9 Sep 2020 14:00:15 +0200 Subject: [PATCH 04/27] Add more messages and services --- controller_manager_msgs/CMakeLists.txt | 9 +++++++++ controller_manager_msgs/msg/ControllerState.msg | 5 +++++ controller_manager_msgs/srv/ListControllerTypes.srv | 6 ++++++ controller_manager_msgs/srv/ListControllers.srv | 5 +++++ controller_manager_msgs/srv/LoadController.srv | 10 ++++++++++ .../srv/ReloadControllerLibraries.srv | 11 +++++++++++ controller_manager_msgs/srv/UnloadController.srv | 10 ++++++++++ 7 files changed, 56 insertions(+) create mode 100644 controller_manager_msgs/msg/ControllerState.msg create mode 100644 controller_manager_msgs/srv/ListControllerTypes.srv create mode 100644 controller_manager_msgs/srv/ListControllers.srv create mode 100644 controller_manager_msgs/srv/LoadController.srv create mode 100644 controller_manager_msgs/srv/ReloadControllerLibraries.srv create mode 100644 controller_manager_msgs/srv/UnloadController.srv diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt index fa491f5d7e..dba0452ecc 100644 --- a/controller_manager_msgs/CMakeLists.txt +++ b/controller_manager_msgs/CMakeLists.txt @@ -14,8 +14,16 @@ find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) +set(msg_files + msg/ControllerState.msg +) set(srv_files + srv/ListControllers.srv + srv/ListControllerTypes.srv + srv/LoadController.srv + srv/ReloadControllerLibraries.srv srv/SwitchController.srv + srv/UnloadController.srv ) if(BUILD_TESTING) @@ -24,6 +32,7 @@ if(BUILD_TESTING) endif() rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} ${srv_files} DEPENDENCIES builtin_interfaces ADD_LINTER_TESTS diff --git a/controller_manager_msgs/msg/ControllerState.msg b/controller_manager_msgs/msg/ControllerState.msg new file mode 100644 index 0000000000..230d6247ea --- /dev/null +++ b/controller_manager_msgs/msg/ControllerState.msg @@ -0,0 +1,5 @@ +string name +string state +string type +# To be implemented +# controller_manager_msgs/HardwareInterfaceResources[] claimed_resources diff --git a/controller_manager_msgs/srv/ListControllerTypes.srv b/controller_manager_msgs/srv/ListControllerTypes.srv new file mode 100644 index 0000000000..059efba57b --- /dev/null +++ b/controller_manager_msgs/srv/ListControllerTypes.srv @@ -0,0 +1,6 @@ +# The ListControllers service returns a list of controller types that are known +# to the controller manager plugin mechanism. + +--- +string[] types +string[] base_classes diff --git a/controller_manager_msgs/srv/ListControllers.srv b/controller_manager_msgs/srv/ListControllers.srv new file mode 100644 index 0000000000..5236ad2448 --- /dev/null +++ b/controller_manager_msgs/srv/ListControllers.srv @@ -0,0 +1,5 @@ +# The ListControllers service returns a list of controller names/states/types of the +# controllers that are loaded inside the controller_manager. + +--- +ControllerState[] controller diff --git a/controller_manager_msgs/srv/LoadController.srv b/controller_manager_msgs/srv/LoadController.srv new file mode 100644 index 0000000000..106b288142 --- /dev/null +++ b/controller_manager_msgs/srv/LoadController.srv @@ -0,0 +1,10 @@ +# The LoadController service allows you to load a single controller +# inside controller_manager + +# To load a controller, specify the "name" of the controller. +# The return value "ok" indicates if the controller was successfully +# constructed and initialized or not. + +string name +--- +bool ok \ No newline at end of file diff --git a/controller_manager_msgs/srv/ReloadControllerLibraries.srv b/controller_manager_msgs/srv/ReloadControllerLibraries.srv new file mode 100644 index 0000000000..01483856fb --- /dev/null +++ b/controller_manager_msgs/srv/ReloadControllerLibraries.srv @@ -0,0 +1,11 @@ +# The ReloadControllerLibraries service will reload all controllers that are available in +# the system as plugins + + +# Reloading libraries only works if there are no controllers loaded. If there +# are still some controllers loaded, the reloading will fail. +# If this bool is set to true, all loaded controllers will get +# killed automatically, and the reloading can succeed. +bool force_kill +--- +bool ok diff --git a/controller_manager_msgs/srv/UnloadController.srv b/controller_manager_msgs/srv/UnloadController.srv new file mode 100644 index 0000000000..ecb4a88447 --- /dev/null +++ b/controller_manager_msgs/srv/UnloadController.srv @@ -0,0 +1,10 @@ +# The UnloadController service allows you to unload a single controller +# from controller_manager + +# To unload a controller, specify the "name" of the controller. +# The return value "ok" indicates if the controller was successfully +# unloaded or not + +string name +--- +bool ok \ No newline at end of file From 27206eabb01a0d989a1489606a3cb57e3b2847e0 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 9 Sep 2020 15:07:29 +0200 Subject: [PATCH 05/27] Add list_controllers service --- .../controller_manager/controller_manager.hpp | 7 +- controller_manager/src/controller_manager.cpp | 47 ++++++- .../test/test_controller_manager.cpp | 115 ++++++++++++++++-- 3 files changed, 157 insertions(+), 12 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 578fdd9a03..7a4d87fb76 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -93,7 +93,7 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC controller_interface::return_type - configure() const; + configure(); CONTROLLER_MANAGER_PUBLIC controller_interface::return_type @@ -149,6 +149,11 @@ class ControllerManager : public rclcpp::Node /// The index of the controllers list being used in the real-time thread. int used_by_realtime_ = {-1}; + /// mutex copied from ROS1 Control, protects service callbacks + /// not needed if we're guaranteed that the callbacks don't come from multiple threads + std::mutex services_lock_; + rclcpp::Service::SharedPtr + list_controllers_service_; std::vector start_request_, stop_request_; #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index eed5d063bc..8bc7debd3a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -580,6 +580,45 @@ void ControllerManager::start_controllers_asap() #endif } +void ControllerManager::list_controllers_srv_cb( + const std::shared_ptr, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG(get_logger(), "list controller service called"); + std::lock_guard services_guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "list controller service locked"); + + // lock controllers to get all names/types/states + std::lock_guard controller_guard(controllers_lock_); + auto & controllers = controllers_lists_[current_controllers_list_]; + response->controller.resize(controllers.size()); + + for (size_t i = 0; i < controllers.size(); ++i) { + controller_manager_msgs::msg::ControllerState & cs = response->controller[i]; + cs.name = controllers[i].info.name; + cs.type = controllers[i].info.type; + cs.state = controllers[i].c->get_lifecycle_node()->get_current_state().label(); + +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + cs.claimed_resources.clear(); + typedef std::vector ClaimedResVec; + typedef ClaimedResVec::const_iterator ClaimedResIt; + const ClaimedResVec & c_resources = controllers[i].info.claimed_resources; + for (const auto & c_resource : c_resources) { + controller_manager_msgs::HardwareInterfaceResources iface_res; + iface_res.hardware_interface = c_resource.hardware_interface; + std::copy( + c_resource.resources.begin(), c_resource.resources.end(), + std::back_inserter(iface_res.resources)); + cs.claimed_resources.push_back(iface_res); + } +#endif + } + + RCLCPP_DEBUG(get_logger(), "list controller service finished"); +} + controller_interface::return_type ControllerManager::update() { @@ -604,8 +643,14 @@ ControllerManager::update() } controller_interface::return_type -ControllerManager::configure() const +ControllerManager::configure() { + using namespace std::placeholders; + list_controllers_service_ = create_service( + "list_controllers", std::bind( + &ControllerManager::list_controllers_srv_cb, this, _1, + _2)); + auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { auto controller_state = loaded_controller.c->get_lifecycle_node()->configure(); diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index f42d4c05ec..c5dc6cb188 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -17,7 +17,8 @@ #include #include "controller_manager/controller_manager.hpp" - +#include "controller_manager_msgs/srv/list_controllers.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/utilities.hpp" @@ -47,40 +48,134 @@ class TestControllerManager : public ::testing::Test }; TEST_F(TestControllerManager, controller_lifecycle) { - controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + auto cm = std::make_shared( + robot, executor, + "test_controller_manager"); + auto test_controller = std::make_shared(); - auto abstract_test_controller = cm.add_controller( + auto abstract_test_controller = cm->add_controller( test_controller, "test_controller", "TestControllerType"); - EXPECT_EQ(1u, cm.get_loaded_controllers().size()); + EXPECT_EQ(1u, cm->get_loaded_controllers().size()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.update()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); EXPECT_EQ( 0u, test_controller->internal_counter) << "Update should not reached an unconfigured and deactivated controller"; - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.configure()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->configure()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.activate()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->activate()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.update()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); EXPECT_EQ(1u, test_controller->internal_counter); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.deactivate()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->deactivate()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.cleanup()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->cleanup()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_lifecycle_node()->get_current_state().id()); } + +std::shared_ptr call_service_and_wait( + rclcpp::Client & client, + std::shared_ptr request, + rclcpp::Executor & service_executor) +{ + EXPECT_TRUE(client.wait_for_service(std::chrono::milliseconds(500))); + auto result = client.async_send_request(request); + // Wait for the result. + EXPECT_EQ( + service_executor.spin_until_future_complete(result), + rclcpp::FutureReturnCode::SUCCESS); + return result.get(); +} + +TEST_F(TestControllerManager, list_controllers_srv) { + auto cm = std::make_shared( + robot, executor, + "test_controller_manager"); + + // periodic calls to update + auto timer = cm->create_wall_timer( + std::chrono::milliseconds(10), + std::bind(&controller_manager::ControllerManager::update, cm.get())); + + executor->add_node(cm); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->configure()); + + auto future_handle = std::async( + std::launch::async, [this]() -> void { + executor->spin(); + }); + + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("list_controllers"); + auto request = std::make_shared(); + + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 0u, + result->controller.size()); + + + auto test_controller = std::make_shared(); + static const std::string CONTROLLER_NAME = "test_controller"; + static const std::string CONTROLLER_TYPE = "TestControllerType"; + auto abstract_test_controller = cm->add_controller( + test_controller, "test_controller", + "TestControllerType"); + EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 1u, + result->controller.size()); + ASSERT_EQ(CONTROLLER_NAME, result->controller[0].name); + ASSERT_EQ(CONTROLLER_TYPE, result->controller[0].type); + ASSERT_EQ("inactive", result->controller[0].state); + + cm->switch_controller( + {CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 1u, + result->controller.size()); + ASSERT_EQ("active", result->controller[0].state); + + + cm->switch_controller( + {}, {CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 1u, + result->controller.size()); + ASSERT_EQ("inactive", result->controller[0].state); + + cm->unload_controller(CONTROLLER_NAME); + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 0u, + result->controller.size()); + executor->cancel(); +} From 47b170f6a5de3cd9b90df7157e26d3b14b96b221 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 14 Sep 2020 18:18:28 +0200 Subject: [PATCH 06/27] Implement load, unload, switch and reload services --- controller_manager/CMakeLists.txt | 14 +- .../controller_loader_interface.hpp | 16 +- .../controller_loader_pluginlib.hpp | 7 + .../controller_manager/controller_manager.hpp | 61 +++- .../controller_manager/controller_spec.hpp | 2 - .../src/controller_loader_pluginlib.cpp | 17 +- controller_manager/src/controller_manager.cpp | 194 +++++++++- .../test/controller_manager_test_common.hpp | 83 +++++ .../test/test_controller/test_controller.hpp | 2 + .../test/test_controller_manager.cpp | 123 +------ .../test/test_controller_manager_srvs.cpp | 333 ++++++++++++++++++ .../test/test_load_controller.cpp | 85 +---- .../srv/SwitchController.srv | 4 +- .../hardware_interface/controller_info.hpp | 46 +-- 14 files changed, 761 insertions(+), 226 deletions(-) create mode 100644 controller_manager/test/controller_manager_test_common.hpp create mode 100644 controller_manager/test/test_controller_manager_srvs.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e8613768e0..59b20dbeaf 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -64,7 +64,7 @@ if(BUILD_TESTING) target_link_libraries(test_controller controller_manager) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") - ament_add_gtest( + ament_add_gmock( test_controller_manager test/test_controller_manager.cpp ) @@ -87,6 +87,18 @@ if(BUILD_TESTING) test_robot_hardware ) + ament_add_gmock( + test_controller_manager_srvs + test/test_controller_manager_srvs.cpp + APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ + ) + target_include_directories(test_controller_manager_srvs PRIVATE include) + target_link_libraries(test_controller_manager_srvs controller_manager test_controller) + ament_target_dependencies( + test_controller_manager_srvs + test_robot_hardware + ) + pluginlib_export_plugin_description_file(controller_interface test/test_controller.xml) install(TARGETS test_controller diff --git a/controller_manager/include/controller_manager/controller_loader_interface.hpp b/controller_manager/include/controller_manager/controller_loader_interface.hpp index d0d1b82594..91eb670983 100644 --- a/controller_manager/include/controller_manager/controller_loader_interface.hpp +++ b/controller_manager/include/controller_manager/controller_loader_interface.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "controller_interface/controller_interface.hpp" @@ -29,7 +30,8 @@ class ControllerLoaderInterface { public: CONTROLLER_MANAGER_PUBLIC - ControllerLoaderInterface() = default; + explicit ControllerLoaderInterface(const std::string & name) + : name_(name) {} CONTROLLER_MANAGER_PUBLIC virtual ~ControllerLoaderInterface() = default; @@ -40,6 +42,18 @@ class ControllerLoaderInterface CONTROLLER_MANAGER_PUBLIC virtual bool is_available(const std::string & controller_type) const = 0; + + CONTROLLER_MANAGER_PUBLIC + const std::string & getName() const {return name_;} + + CONTROLLER_MANAGER_PUBLIC + virtual std::vector getDeclaredClasses() = 0; + + CONTROLLER_MANAGER_PUBLIC + virtual void reload() = 0; + +private: + const std::string name_; }; using ControllerLoaderInterfaceSharedPtr = std::shared_ptr; diff --git a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp index 79779a78ab..d4fa10a071 100644 --- a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp +++ b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "pluginlib/class_loader.hpp" @@ -40,6 +41,12 @@ class ControllerLoaderPluginlib : public ControllerLoaderInterface CONTROLLER_MANAGER_PUBLIC bool is_available(const std::string & controller_type) const; + CONTROLLER_MANAGER_PUBLIC + std::vector getDeclaredClasses() override; + + CONTROLLER_MANAGER_PUBLIC + void reload() override; + private: std::shared_ptr> loader_; }; diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 7a4d87fb76..b9ec60293b 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -26,6 +26,11 @@ #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" #include "controller_manager_msgs/srv/list_controllers.hpp" +#include "controller_manager_msgs/srv/list_controller_types.hpp" +#include "controller_manager_msgs/srv/load_controller.hpp" +#include "controller_manager_msgs/srv/reload_controller_libraries.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "controller_manager_msgs/srv/unload_controller.hpp" #include "hardware_interface/robot_hardware.hpp" @@ -38,6 +43,9 @@ namespace controller_manager class ControllerManager : public rclcpp::Node { public: + static constexpr bool WAIT_FOR_ALL_RESOURCES = false; + static constexpr double INFINITE_TIMEOUT = 0.0; + CONTROLLER_MANAGER_PUBLIC ControllerManager( std::shared_ptr hw, @@ -54,6 +62,14 @@ class ControllerManager : public rclcpp::Node const std::string & controller_name, const std::string & controller_type); + /** + * @brief load_controller loads a controller by name, the type must be defined in the parameter server + */ + CONTROLLER_MANAGER_PUBLIC + controller_interface::ControllerInterfaceSharedPtr + load_controller( + const std::string & controller_name); + CONTROLLER_MANAGER_PUBLIC controller_interface::return_type unload_controller( const std::string & controller_name); @@ -80,12 +96,17 @@ class ControllerManager : public rclcpp::Node return add_controller_impl(controller_spec); } + /** + * @brief switch_controller Stops some controllers and others. + * @see Documentation in controller_manager_msgs/SwitchController.srv + */ CONTROLLER_MANAGER_PUBLIC controller_interface::return_type switch_controller( const std::vector & start_controllers, const std::vector & stop_controllers, - int strictness, bool start_asap, const rclcpp::Duration & timeout); + int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES, + const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT)); CONTROLLER_MANAGER_PUBLIC controller_interface::return_type @@ -132,7 +153,35 @@ class ControllerManager : public rclcpp::Node const std::shared_ptr request, std::shared_ptr response); + CONTROLLER_MANAGER_PUBLIC + void list_controller_types_srv_cb( + const std::shared_ptr request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void load_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void reload_controller_libraries_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void switch_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + + CONTROLLER_MANAGER_PUBLIC + void unload_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response); + private: + void get_controller_names(std::vector & names); + + std::shared_ptr hw_; std::shared_ptr executor_; std::vector loaders_; @@ -154,6 +203,16 @@ class ControllerManager : public rclcpp::Node std::mutex services_lock_; rclcpp::Service::SharedPtr list_controllers_service_; + rclcpp::Service::SharedPtr + list_controller_types_service_; + rclcpp::Service::SharedPtr + load_controller_service_; + rclcpp::Service::SharedPtr + reload_controller_libraries_service_; + rclcpp::Service::SharedPtr + switch_controller_service_; + rclcpp::Service::SharedPtr + unload_controller_service_; std::vector start_request_, stop_request_; #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index af830bbe9d..dcc2c513c9 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -19,8 +19,6 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ -#pragma GCC diagnostic ignored "-Wextra" - #include #include diff --git a/controller_manager/src/controller_loader_pluginlib.cpp b/controller_manager/src/controller_loader_pluginlib.cpp index 8e4a63ebf5..911d25e133 100644 --- a/controller_manager/src/controller_loader_pluginlib.cpp +++ b/controller_manager/src/controller_loader_pluginlib.cpp @@ -16,15 +16,15 @@ #include #include +#include namespace controller_manager { ControllerLoaderPluginlib::ControllerLoaderPluginlib() -: ControllerLoaderInterface(), - loader_(std::make_shared>( - "controller_interface", "controller_interface::ControllerInterface")) +: ControllerLoaderInterface("controller_interface::ControllerInterface") { + reload(); } controller_interface::ControllerInterfaceSharedPtr ControllerLoaderPluginlib::create( @@ -38,4 +38,15 @@ bool ControllerLoaderPluginlib::is_available(const std::string & controller_type return loader_->isClassAvailable(controller_type); } +std::vector ControllerLoaderPluginlib::getDeclaredClasses() +{ + return loader_->getDeclaredClasses(); +} + +void ControllerLoaderPluginlib::reload() +{ + loader_ = std::make_shared>( + "controller_interface", "controller_interface::ControllerInterface"); +} + } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8bc7debd3a..f7a498de33 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -75,6 +75,21 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_contr return add_controller_impl(controller_spec); } +controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( + const std::string & controller_name) +{ + const std::string param_name = controller_name + ".type"; + std::string controller_type; + if (!has_parameter(param_name)) { + declare_parameter(param_name, rclcpp::ParameterValue()); + } + if (!get_parameter(param_name, controller_type)) { + RCLCPP_ERROR(get_logger(), "'type' param not defined for %s", controller_name.c_str()); + return nullptr; + } + return load_controller(controller_name, controller_type); +} + controller_interface::return_type ControllerManager::unload_controller( const std::string & controller_name) { @@ -105,6 +120,7 @@ controller_interface::return_type ControllerManager::unload_controller( } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); controller.c->get_lifecycle_node()->cleanup(); + executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface()); removed = true; } else { to.push_back(controller); @@ -619,6 +635,158 @@ void ControllerManager::list_controllers_srv_cb( RCLCPP_DEBUG(get_logger(), "list controller service finished"); } +void ControllerManager::list_controller_types_srv_cb( + const std::shared_ptr, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG(get_logger(), "list types service called"); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "list types service locked"); + + for (const auto & controller_loader : loaders_) { + std::vector cur_types = controller_loader->getDeclaredClasses(); + for (const auto & cur_type : cur_types) { + response->types.push_back(cur_type); + response->base_classes.push_back(controller_loader->getName()); + RCLCPP_INFO(get_logger(), cur_type); + } + } + + RCLCPP_DEBUG(get_logger(), "list types service finished"); +} + +void ControllerManager::load_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG(get_logger(), "loading service called for controller '%s' ", request->name.c_str()); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "loading service locked"); + + response->ok = load_controller(request->name).get(); + + RCLCPP_DEBUG( + get_logger(), "loading service finished for controller '%s' ", + request->name.c_str()); +} + +void ControllerManager::reload_controller_libraries_service_cb( + const std::shared_ptr request, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG(get_logger(), "reload libraries service called"); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "reload libraries service locked"); + + // only reload libraries if no controllers are running + std::vector loaded_controllers, running_controllers; + get_controller_names(loaded_controllers); + { + std::lock_guard guard(controllers_lock_); + for (const auto & controller : controllers_lists_[current_controllers_list_]) { + if (isControllerRunning(*controller.c)) { + running_controllers.push_back(controller.info.name); + } + } + } + if (!running_controllers.empty() && !request->force_kill) { + RCLCPP_ERROR( + get_logger(), "Controller manager: Cannot reload controller libraries because" + " there are still %i controllers running", + (int)running_controllers.size()); + response->ok = false; + return; + } + + // stop running controllers if requested + if (!loaded_controllers.empty()) { + RCLCPP_INFO(get_logger(), "Controller manager: Stopping all running controllers"); + std::vector empty; + if (switch_controller( + empty, running_controllers, + controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) != + controller_interface::return_type::SUCCESS) + { + RCLCPP_ERROR( + get_logger(), + "Controller manager: Cannot reload controller libraries because failed to stop " + "running controllers"); + response->ok = false; + return; + } + for (const auto & controller : loaded_controllers) { + if (unload_controller(controller) != controller_interface::return_type::SUCCESS) { + RCLCPP_ERROR( + get_logger(), "Controller manager: Cannot reload controller libraries because " + "failed to unload controller '%s'", + controller.c_str()); + response->ok = false; + return; + } + } + get_controller_names(loaded_controllers); + } + assert(loaded_controllers.empty()); + + // Force a reload on all the PluginLoaders (internally, this recreates the plugin loaders) + for (const auto & controller_loader : loaders_) { + controller_loader->reload(); + RCLCPP_INFO( + get_logger(), "Controller manager: reloaded controller libraries for '%s'", + controller_loader->getName().c_str()); + } + + response->ok = true; + + RCLCPP_DEBUG(get_logger(), "reload libraries service finished"); +} + +void ControllerManager::switch_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG(get_logger(), "switching service called"); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "switching service locked"); + + response->ok = switch_controller( + request->start_controllers, request->stop_controllers, request->strictness, + request->start_asap, request->timeout) == controller_interface::return_type::SUCCESS; + + RCLCPP_DEBUG(get_logger(), "switching service finished"); +} + +void ControllerManager::unload_controller_service_cb( + const std::shared_ptr request, + std::shared_ptr response) +{ + // lock services + RCLCPP_DEBUG( + get_logger(), "unloading service called for controller '%s' ", + request->name.c_str()); + std::lock_guard guard(services_lock_); + RCLCPP_DEBUG(get_logger(), "unloading service locked"); + + response->ok = unload_controller(request->name) == controller_interface::return_type::SUCCESS; + + RCLCPP_DEBUG( + get_logger(), "unloading service finished for controller '%s' ", + request->name.c_str()); +} + +void ControllerManager::get_controller_names(std::vector & names) +{ + std::lock_guard guard(controllers_lock_); + names.clear(); + for (const auto & controller : controllers_lists_[current_controllers_list_]) { + names.push_back(controller.info.name); + } +} + controller_interface::return_type ControllerManager::update() { @@ -645,13 +813,37 @@ ControllerManager::update() controller_interface::return_type ControllerManager::configure() { + auto ret = controller_interface::return_type::SUCCESS; + + using namespace std::placeholders; list_controllers_service_ = create_service( "list_controllers", std::bind( &ControllerManager::list_controllers_srv_cb, this, _1, _2)); + list_controller_types_service_ = + create_service( + "list_controller_types", std::bind( + &ControllerManager::list_controller_types_srv_cb, this, _1, + _2)); + load_controller_service_ = create_service( + "load_controller", std::bind( + &ControllerManager::load_controller_service_cb, this, _1, + _2)); + reload_controller_libraries_service_ = + create_service( + "reload_controller_libraries", std::bind( + &ControllerManager::reload_controller_libraries_service_cb, this, _1, + _2)); + switch_controller_service_ = create_service( + "switch_controller", std::bind( + &ControllerManager::switch_controller_service_cb, this, _1, + _2)); + unload_controller_service_ = create_service( + "unload_controller", std::bind( + &ControllerManager::unload_controller_service_cb, this, _1, + _2)); - auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { auto controller_state = loaded_controller.c->get_lifecycle_node()->configure(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp new file mode 100644 index 0000000000..9768aed773 --- /dev/null +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -0,0 +1,83 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROLLER_MANAGER_TEST_COMMON_HPP_ +#define CONTROLLER_MANAGER_TEST_COMMON_HPP_ + +#include +#include + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" + +#include "controller_manager/controller_loader_interface.hpp" +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" + +#include "rclcpp/utilities.hpp" +#include "test_controller/test_controller.hpp" +#include "test_robot_hardware/test_robot_hardware.hpp" + + +constexpr auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; +constexpr auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + +class TestControllerManager : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + robot_ = std::make_shared(); + robot_->init(); + + executor_ = std::make_shared(); + } + + std::shared_ptr robot_; + std::shared_ptr executor_; +}; + +class ControllerMock : public controller_interface::ControllerInterface +{ +public: + MOCK_METHOD0(update, controller_interface::return_type(void)); +}; + +constexpr char MOCK_TEST_CONTROLLER_NAME[] = "mock_test_controller"; +constexpr char MOCK_TEST_CONTROLLER_TYPE[] = "ControllerMock"; + +class ControllerLoaderMock : public controller_manager::ControllerLoaderInterface +{ +public: + ControllerLoaderMock() + : controller_manager::ControllerLoaderInterface("controller_interface::MockControllerInterface") + {} + MOCK_METHOD1(create, controller_interface::ControllerInterfaceSharedPtr(const std::string &)); + MOCK_CONST_METHOD1(is_available, bool(const std::string &)); + std::vector getDeclaredClasses() override + { + return {MOCK_TEST_CONTROLLER_NAME}; + } + MOCK_METHOD0(reload, void()); +}; + +#endif // CONTROLLER_MANAGER_TEST_COMMON_HPP_ diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index f372d2d646..0c8bf86e40 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -24,6 +24,8 @@ namespace test_controller { +constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name"; +constexpr char TEST_CONTROLLER_TYPE[] = "test_controller"; class TestController : public controller_interface::ControllerInterface { public: diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index c5dc6cb188..6bf57c4876 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -18,45 +18,21 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/srv/list_controllers.hpp" -#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" - -#include "rclcpp/utilities.hpp" - #include "test_robot_hardware/test_robot_hardware.hpp" - #include "./test_controller/test_controller.hpp" -class TestControllerManager : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - robot = std::make_shared(); - robot->init(); - - executor = std::make_shared(); - } - - std::shared_ptr robot; - std::shared_ptr executor; -}; - TEST_F(TestControllerManager, controller_lifecycle) { auto cm = std::make_shared( - robot, executor, + robot_, executor_, "test_controller_manager"); auto test_controller = std::make_shared(); auto abstract_test_controller = cm->add_controller( - test_controller, "test_controller", - "TestControllerType"); + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_TYPE); EXPECT_EQ(1u, cm->get_loaded_controllers().size()); EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); @@ -88,94 +64,3 @@ TEST_F(TestControllerManager, controller_lifecycle) { lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_lifecycle_node()->get_current_state().id()); } - -std::shared_ptr call_service_and_wait( - rclcpp::Client & client, - std::shared_ptr request, - rclcpp::Executor & service_executor) -{ - EXPECT_TRUE(client.wait_for_service(std::chrono::milliseconds(500))); - auto result = client.async_send_request(request); - // Wait for the result. - EXPECT_EQ( - service_executor.spin_until_future_complete(result), - rclcpp::FutureReturnCode::SUCCESS); - return result.get(); -} - -TEST_F(TestControllerManager, list_controllers_srv) { - auto cm = std::make_shared( - robot, executor, - "test_controller_manager"); - - // periodic calls to update - auto timer = cm->create_wall_timer( - std::chrono::milliseconds(10), - std::bind(&controller_manager::ControllerManager::update, cm.get())); - - executor->add_node(cm); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->configure()); - - auto future_handle = std::async( - std::launch::async, [this]() -> void { - executor->spin(); - }); - - rclcpp::executors::SingleThreadedExecutor srv_executor; - rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); - srv_executor.add_node(srv_node); - rclcpp::Client::SharedPtr client = - srv_node->create_client("list_controllers"); - auto request = std::make_shared(); - - auto result = call_service_and_wait(*client, request, srv_executor); - ASSERT_EQ( - 0u, - result->controller.size()); - - - auto test_controller = std::make_shared(); - static const std::string CONTROLLER_NAME = "test_controller"; - static const std::string CONTROLLER_TYPE = "TestControllerType"; - auto abstract_test_controller = cm->add_controller( - test_controller, "test_controller", - "TestControllerType"); - EXPECT_EQ(1u, cm->get_loaded_controllers().size()); - result = call_service_and_wait(*client, request, srv_executor); - ASSERT_EQ( - 1u, - result->controller.size()); - ASSERT_EQ(CONTROLLER_NAME, result->controller[0].name); - ASSERT_EQ(CONTROLLER_TYPE, result->controller[0].type); - ASSERT_EQ("inactive", result->controller[0].state); - - cm->switch_controller( - {CONTROLLER_NAME}, {}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - - result = call_service_and_wait(*client, request, srv_executor); - ASSERT_EQ( - 1u, - result->controller.size()); - ASSERT_EQ("active", result->controller[0].state); - - - cm->switch_controller( - {}, {CONTROLLER_NAME}, - controller_manager_msgs::srv::SwitchController::Request::STRICT, true, - rclcpp::Duration(0, 0)); - - result = call_service_and_wait(*client, request, srv_executor); - ASSERT_EQ( - 1u, - result->controller.size()); - ASSERT_EQ("inactive", result->controller[0].state); - - cm->unload_controller(CONTROLLER_NAME); - result = call_service_and_wait(*client, request, srv_executor); - ASSERT_EQ( - 0u, - result->controller.size()); - executor->cancel(); -} diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp new file mode 100644 index 0000000000..e83c672f1b --- /dev/null +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -0,0 +1,333 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "controller_manager_test_common.hpp" +#include "controller_interface/controller_interface.hpp" +#include "controller_manager/controller_loader_interface.hpp" +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "controller_manager_msgs/srv/list_controller_types.hpp" +#include "controller_manager_msgs/srv/list_controllers.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +using ::testing::_; +using ::testing::Return; + +using namespace std::chrono_literals; + + +class TestControllerManagerSrvs : public TestControllerManager +{ +public: + TestControllerManagerSrvs() + { + } + + void SetUp() override + { + TestControllerManager::SetUp(); + cm_ = std::make_shared( + robot_, executor_, + "test_controller_manager"); + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + std::bind(&controller_manager::ControllerManager::update, cm_.get())); + + executor_->add_node(cm_); + + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->configure()); + executor_spin_future_ = std::async( + std::launch::async, [this]() -> void { + executor_->spin(); + }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override + { + executor_->cancel(); + } + + template + std::shared_ptr call_service_and_wait( + rclcpp::Client & client, + std::shared_ptr request, + rclcpp::Executor & service_executor, + bool update_controller_while_spinning = false) + { + EXPECT_TRUE(client.wait_for_service(std::chrono::milliseconds(500))); + auto result = client.async_send_request(request); + // Wait for the result. + if (update_controller_while_spinning) { + while (service_executor.spin_until_future_complete( + result, + 50ms) != rclcpp::FutureReturnCode::SUCCESS) + { + cm_->update(); + } + } else { + EXPECT_EQ( + service_executor.spin_until_future_complete(result), + rclcpp::FutureReturnCode::SUCCESS); + } + return result.get(); + } + +protected: + std::shared_ptr cm_; + rclcpp::TimerBase::SharedPtr update_timer_; + std::future executor_spin_future_; +}; + +TEST_F(TestControllerManagerSrvs, list_controller_types) +{ + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client( + "list_controller_types"); + auto request = std::make_shared(); + + auto result = call_service_and_wait(*client, request, srv_executor); + // Number depends on the controllers that exist on the system + size_t controller_types = result->types.size(); + ASSERT_GE( + controller_types, + 1u); + ASSERT_EQ( + controller_types, + result->base_classes.size()); + ASSERT_THAT(result->types, ::testing::Contains("test_controller")); + ASSERT_THAT( + result->base_classes, + ::testing::Contains("controller_interface::ControllerInterface")); + + std::shared_ptr mock_loader(new ControllerLoaderMock); + + cm_->register_controller_loader(mock_loader); + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + controller_types + 1, + result->types.size()); + ASSERT_EQ( + result->types.size(), + result->base_classes.size()); + ASSERT_THAT(result->types, ::testing::Contains("mock_test_controller")); + ASSERT_THAT( + result->base_classes, + ::testing::Contains("controller_interface::MockControllerInterface")); +} + + +TEST_F(TestControllerManagerSrvs, list_controllers_srv) { + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("list_controllers"); + auto request = std::make_shared(); + + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 0u, + result->controller.size()); + + auto test_controller = std::make_shared(); + auto abstract_test_controller = cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_TYPE); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 1u, + result->controller.size()); + ASSERT_EQ(test_controller::TEST_CONTROLLER_NAME, result->controller[0].name); + ASSERT_EQ(test_controller::TEST_CONTROLLER_TYPE, result->controller[0].type); + ASSERT_EQ("inactive", result->controller[0].state); + + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 1u, + result->controller.size()); + ASSERT_EQ("active", result->controller[0].state); + + + cm_->switch_controller( + {}, {test_controller::TEST_CONTROLLER_NAME}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 1u, + result->controller.size()); + ASSERT_EQ("inactive", result->controller[0].state); + + ASSERT_EQ( + controller_interface::return_type::SUCCESS, + cm_->unload_controller(test_controller::TEST_CONTROLLER_NAME)); + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ( + 0u, + result->controller.size()); +} + +TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client( + "reload_controller_libraries"); + auto request = + std::make_shared(); + + + std::shared_ptr mock_loader(new ControllerLoaderMock); + + cm_->register_controller_loader(mock_loader); + + // Reload with no controllers running + request->force_kill = false; + EXPECT_CALL(*mock_loader, is_available(_)).WillRepeatedly(Return(false)); + EXPECT_CALL(*mock_loader, reload).Times(1); + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_TRUE(result->ok); + + + // Add a controller, but stopped + auto test_controller = cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_TYPE); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); + ASSERT_GT( + test_controller.use_count(), + 1) << "Controller manager should have have a copy of this shared ptr"; + + request->force_kill = false; + EXPECT_CALL(*mock_loader, reload).Times(1); + RCLCPP_INFO(cm_->get_logger(), "Doing reload"); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + test_controller.use_count(), + 1) << "No more references to the controller after reloading."; + test_controller.reset(); + + test_controller = cm_->load_controller( + test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_TYPE); + // Start Controller + cm_->switch_controller( + {test_controller::TEST_CONTROLLER_NAME}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); + + + // Failed reload due to active controller + request->force_kill = false; + EXPECT_CALL(*mock_loader, reload).Times(0); + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_FALSE(result->ok) << "Cannot reload if controllers are running"; + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()); + ASSERT_GT( + test_controller.use_count(), + 1) << + "Controller manager should still have have a copy of " + "this shared ptr, no unloading was performed"; + + // Force stop active controller + request->force_kill = true; + EXPECT_CALL(*mock_loader, reload).Times(1); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + + + ASSERT_EQ( + test_controller.use_count(), + 1) << "No more references to the controller after reloading."; + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_node()->get_current_state().id()) << + "Controller should have been stopped and cleaned up with force_kill = true"; +} + +TEST_F(TestControllerManagerSrvs, load_controller_srv) { + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client( + "load_controller"); + + auto request = std::make_shared(); + request->name = test_controller::TEST_CONTROLLER_NAME; + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_FALSE(result->ok) << "There's no param specifying the type for " << request->name; + rclcpp::Parameter joint_parameters(std::string(test_controller::TEST_CONTROLLER_NAME) + ".type", + test_controller::TEST_CONTROLLER_TYPE); + cm_->set_parameter(joint_parameters); + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); +} + +TEST_F(TestControllerManagerSrvs, unload_controller_srv) { + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client( + "unload_controller"); + + auto request = std::make_shared(); + request->name = test_controller::TEST_CONTROLLER_NAME; + auto result = call_service_and_wait(*client, request, srv_executor); + ASSERT_FALSE(result->ok) << "Controller not loaded: " << request->name; + + + auto test_controller = std::make_shared(); + auto abstract_test_controller = cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_TYPE); + EXPECT_EQ(1u, cm_->get_loaded_controllers().size()); + + result = call_service_and_wait(*client, request, srv_executor, true); + ASSERT_TRUE(result->ok); + EXPECT_EQ(0u, cm_->get_loaded_controllers().size()); +} diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 45f159f7ee..5f644cad2b 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -18,6 +18,8 @@ #include #include +#include "controller_manager_test_common.hpp" + #include "controller_interface/controller_interface.hpp" #include "controller_manager/controller_loader_interface.hpp" @@ -26,64 +28,21 @@ #include "controller_manager_msgs/srv/switch_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" - -#include "rclcpp/utilities.hpp" - -#include "test_controller/test_controller.hpp" - -#include "test_robot_hardware/test_robot_hardware.hpp" - using ::testing::_; using ::testing::Return; -constexpr auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; -constexpr auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; - -class TestControllerManager : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - robot = std::make_shared(); - robot->init(); - - executor = std::make_shared(); - } - - - std::shared_ptr robot; - std::shared_ptr executor; -}; - -class ControllerMock : public controller_interface::ControllerInterface -{ -public: - MOCK_METHOD0(update, controller_interface::return_type(void)); -}; - -class ControllerLoaderMock : public controller_manager::ControllerLoaderInterface -{ -public: - MOCK_METHOD1(create, controller_interface::ControllerInterfaceSharedPtr(const std::string &)); - MOCK_CONST_METHOD1(is_available, bool(const std::string &)); -}; TEST_F(TestControllerManager, load_unknown_controller) { - controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); ASSERT_THROW( cm.load_controller("unknown_controller_name", "unknown_controller_type"), std::runtime_error); } TEST_F(TestControllerManager, load1_known_controller) { - controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller("test_controller_01", "test_controller")); + controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); + ASSERT_NO_THROW(cm.load_controller("test_controller_01", test_controller::TEST_CONTROLLER_TYPE)); EXPECT_EQ(1u, cm.get_loaded_controllers().size()); controller_manager::ControllerSpec abstract_test_controller = @@ -98,8 +57,8 @@ TEST_F(TestControllerManager, load1_known_controller) TEST_F(TestControllerManager, load2_known_controller) { - controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); - std::string controller_type = "test_controller"; + controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); + std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; // load the controller with name1 std::string controller_name1 = "test_controller1"; @@ -132,8 +91,8 @@ TEST_F(TestControllerManager, load2_known_controller) TEST_F(TestControllerManager, update) { - controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller("test_controller_01", "test_controller")); + controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); + ASSERT_NO_THROW(cm.load_controller("test_controller_01", test_controller::TEST_CONTROLLER_TYPE)); controller_manager::ControllerSpec abstract_test_controller = cm.get_loaded_controllers()[0]; @@ -147,7 +106,7 @@ TEST_F(TestControllerManager, update) TEST_F(TestControllerManager, register_controller_loader) { - controller_manager::ControllerManager cm(robot, executor, "test_controller_manager"); + controller_manager::ControllerManager cm(robot_, executor_, "test_controller_manager"); std::shared_ptr mock_loader(new ControllerLoaderMock); std::shared_ptr mock_controller(new ControllerMock); @@ -181,9 +140,9 @@ TEST_F(TestControllerManager, register_controller_loader) TEST_F(TestControllerManager, switch_controller_empty) { auto cm = std::make_shared( - robot, executor, + robot_, executor_, "test_controller_manager"); - std::string controller_type = "test_controller"; + std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; // load the controller with name1 std::string controller_name1 = "test_controller1"; @@ -241,7 +200,6 @@ TEST_F(TestControllerManager, switch_controller_empty) // From now on will only test STRICT and BEST_EFFORT - start_controllers = {}; stop_controllers = {"nonexistent_controller"}; EXPECT_EQ( @@ -274,22 +232,15 @@ TEST_F(TestControllerManager, switch_controller_empty) start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)) ) << "BEST_EFFORT switch with nonexistent controller specified"; - - - auto switch_future = std::async( - std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, - start_controllers, stop_controllers, - STRICT, true, rclcpp::Duration(0, 0)); } TEST_F(TestControllerManager, switch_controller) { auto cm = std::make_shared( - robot, executor, + robot_, executor_, "test_controller_manager"); cm->configure(); - std::string controller_type = "test_controller"; + std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; // load the controller with name1 std::string controller_name1 = "test_controller1"; @@ -402,10 +353,10 @@ TEST_F(TestControllerManager, switch_controller) TEST_F(TestControllerManager, switch_multiple_controllers) { auto cm = std::make_shared( - robot, executor, + robot_, executor_, "test_controller_manager"); cm->configure(); - std::string controller_type = "test_controller"; + std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; // load the controller with name1 std::string controller_name1 = "test_controller1"; @@ -515,10 +466,10 @@ TEST_F(TestControllerManager, switch_multiple_controllers) TEST_F(TestControllerManager, controller_lifecycle_states) { auto cm = std::make_shared( - robot, executor, + robot_, executor_, "test_controller_manager"); cm->configure(); - std::string controller_type = "test_controller"; + std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; // load the controller with name1 std::string controller_name1 = "test_controller1"; diff --git a/controller_manager_msgs/srv/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv index 8bfce99daa..3c68b0e746 100644 --- a/controller_manager_msgs/srv/SwitchController.srv +++ b/controller_manager_msgs/srv/SwitchController.srv @@ -12,7 +12,7 @@ # the service will still try to start/stop the remaining controllers # * start the controllers as soon as their hardware dependencies are ready, will # wait for all interfaces to be ready otherwise -# * the timeout in seconds before aborting pending controllers. Zero for infinite +# * the timeout before aborting pending controllers. Zero for infinite # The return value "ok" indicates if the controllers were switched # successfully or not. The meaning of success depends on the @@ -25,6 +25,6 @@ int32 strictness int32 BEST_EFFORT=1 int32 STRICT=2 bool start_asap -float64 timeout +builtin_interfaces/Duration timeout --- bool ok diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 076bcbb7af..ec520fac78 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -1,37 +1,24 @@ -/////////////////////////////////////////////////////////////////////////////// -// Copyright (C) 2012, hiDOF, INC and Willow Garage, Inc +// Copyright 2017 Open Source Robotics Foundation, Inc. // -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of Willow Garage Inc, hiDOF Inc, nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -////////////////////////////////////////////////////////////////////////////// - -#pragma once +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ +#define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ #include // TODO(v-lopez) -//#include -//#include +// #include +// #include namespace hardware_interface { @@ -54,4 +41,5 @@ struct ControllerInfo // std::vector claimed_resources; }; -} +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ From 81a8d213709d62d0a2376e16ea46af382a612b8b Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 18 Sep 2020 10:41:27 +0200 Subject: [PATCH 07/27] Remove whitespaces --- controller_manager/src/controller_manager.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f7a498de33..2a0f12c94c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -281,7 +281,6 @@ controller_interface::return_type ControllerManager::switch_controller( } } - const bool is_running = isControllerRunning(*controller.c); if (!is_running && in_stop_list) { // check for double stop @@ -815,7 +814,6 @@ ControllerManager::configure() { auto ret = controller_interface::return_type::SUCCESS; - using namespace std::placeholders; list_controllers_service_ = create_service( "list_controllers", std::bind( From 160a7c7381fe908f3d9d6a9bd743dbe625c7f41a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 18 Sep 2020 10:48:11 +0200 Subject: [PATCH 08/27] Reference issues for todos added during services PR --- controller_manager/src/controller_manager.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2a0f12c94c..3228a4a2b1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -435,6 +435,7 @@ ControllerManager::add_controller_impl( // TODO(v-lopez) this should only be done if controller_manager is configured. // Probably the whole load_controller part should fail if the controller_manager // is not configured, should it implement a LifecycleNodeInterface + // https://github.com/ros-controls/ros2_control/issues/152 controller.c->get_lifecycle_node()->configure(); executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface()); to.emplace_back(controller); @@ -794,6 +795,7 @@ ControllerManager::update() auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : controllers_lists_[used_by_realtime_]) { // TODO(v-lopez) we could cache this information + // https://github.com/ros-controls/ros2_control/issues/153 if (isControllerRunning(*loaded_controller.c)) { auto controller_ret = loaded_controller.c->update(); if (controller_ret != controller_interface::return_type::SUCCESS) { From 4c6a609282f21e1fe8d33adce0dc523cb28eadf3 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 18 Sep 2020 13:25:16 +0200 Subject: [PATCH 09/27] Refactor rt double buffered list for easier comprehension and safety --- .../controller_manager/controller_manager.hpp | 81 +++++++-- controller_manager/src/controller_manager.cpp | 170 +++++++++++------- 2 files changed, 173 insertions(+), 78 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b9ec60293b..0910ef4577 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -118,15 +118,15 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC controller_interface::return_type - activate() const; + activate(); CONTROLLER_MANAGER_PUBLIC controller_interface::return_type - deactivate() const; + deactivate(); CONTROLLER_MANAGER_PUBLIC controller_interface::return_type - cleanup() const; + cleanup(); protected: CONTROLLER_MANAGER_PUBLIC @@ -186,18 +186,71 @@ class ControllerManager : public rclcpp::Node std::shared_ptr executor_; std::vector loaders_; - /** \name Controllers List - * The controllers list is double-buffered to avoid needing to lock the - * real-time thread when switching controllers in the non-real-time thread. - *\{*/ - /// Mutex protecting the current controllers list - std::recursive_mutex controllers_lock_; - std::vector controllers_lists_[2]; - /// The index of the current controllers list - int current_controllers_list_ = {0}; - /// The index of the controllers list being used in the real-time thread. - int used_by_realtime_ = {-1}; + /** + * @brief The RTControllerListWrapper class wraps a double-buffered list of controllers + * to avoid needing to lock the real-time thread when switching controllers in + * the non-real-time thread. + * + * There's always an "updated" list and an "outdated" one + * There's always an "used by rt" list and an "unused by rt" list + * + * The updated state changes on the switch_updated_list() + * The rt usage state changes on the get_used_by_rt_list() + */ + class RTControllerListWrapper + { +public: + /** + * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list + * @warning Should only be called by the RT thread, noone should modify the + * updated list while it's being used + * @return reference to the updated list + */ + std::vector & get_used_by_rt_list(); + + /** + * @brief get_unused_list Waits until the "outdated" and "unused by rt" + * lists match and returns a reference to it + * This referenced list can be modified safely until switch_updated_controller_list() + * is called, at this point the RT thread may start using it at any time + * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list + */ + std::vector & get_unused_list( + const std::lock_guard & guard); + + /** + * @brief get_updated_list Returns a const reference to the most updated list, + * @warning May or may not being used by the realtime thread, read-only reference for safety + * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list + */ + const std::vector & get_updated_list( + const std::lock_guard & guard) const; + + /** + * @brief switch_updated_list Switches the "updated" and "outdated" lists, and waits + * until the RT thread is using the new "updated" list. + * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list + */ + void switch_updated_list(const std::lock_guard & guard); + + + // Mutex protecting the controllers list + // must be acquired before using any list other than the "used by rt" + mutable std::recursive_mutex controllers_lock_; + +private: + int get_other_list(int index) const; + + void wait_until_rt_not_using(int index) const; + + std::vector controllers_lists_[2]; + /// The index of the controller list with the most updated information + int updated_controllers_index_ = {0}; + /// The index of the controllers list being used in the real-time thread. + int used_by_realtime_controllers_index_ = {-1}; + }; + RTControllerListWrapper rt_controllers_wrapper_; /// mutex copied from ROS1 Control, protects service callbacks /// not needed if we're guaranteed that the callbacks don't come from multiple threads std::mutex services_lock_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3228a4a2b1..60c5ac41c7 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -93,17 +93,9 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_contr controller_interface::return_type ControllerManager::unload_controller( const std::string & controller_name) { - // get reference to controller list - int free_controllers_list = (current_controllers_list_ + 1) % 2; - while (rclcpp::ok() && free_controllers_list == used_by_realtime_) { - if (!rclcpp::ok()) { - return controller_interface::return_type::ERROR; - } - std::this_thread::sleep_for(std::chrono::microseconds(200)); - } - std::vector - & from = controllers_lists_[current_controllers_list_], - & to = controllers_lists_[free_controllers_list]; + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); to.clear(); // Transfers the running controllers over, skipping the one to be removed and the running ones. @@ -140,16 +132,11 @@ controller_interface::return_type ControllerManager::unload_controller( // Destroys the old controllers list when the realtime thread is finished with it. RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list"); - int former_current_controllers_list_ = current_controllers_list_; - current_controllers_list_ = free_controllers_list; - while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) { - if (!rclcpp::ok()) { - return controller_interface::return_type::ERROR; - } - std::this_thread::sleep_for(std::chrono::microseconds(200)); - } + rt_controllers_wrapper_.switch_updated_list(guard); + std::vector & new_unused_list = rt_controllers_wrapper_.get_unused_list( + guard); RCLCPP_DEBUG(get_logger(), "Destruct controller"); - from.clear(); + new_unused_list.clear(); RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); RCLCPP_DEBUG(get_logger(), "Successfully unloaded controller '%s'", controller_name.c_str()); @@ -158,7 +145,8 @@ controller_interface::return_type ControllerManager::unload_controller( std::vector ControllerManager::get_loaded_controllers() const { - return controllers_lists_[current_controllers_list_]; + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + return rt_controllers_wrapper_.get_updated_list(guard); } void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) @@ -198,9 +186,6 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str()); } - // lock controllers - std::lock_guard guard(controllers_lock_); - controller_interface::ControllerInterface * ct; // list all controllers to stop for (const auto & controller : stop_controllers) { @@ -263,7 +248,11 @@ controller_interface::return_type ControllerManager::switch_controller( switch_stop_list_.clear(); #endif - const auto & controllers = controllers_lists_[current_controllers_list_]; + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + + const std::vector & controllers = + rt_controllers_wrapper_.get_updated_list(guard); for (const auto & controller : controllers) { bool in_stop_list = false; for (const auto & request : stop_request_) { @@ -400,17 +389,11 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl( const ControllerSpec & controller) { - // get reference to controller list - int free_controllers_list = (current_controllers_list_ + 1) % 2; - while (rclcpp::ok() && free_controllers_list == used_by_realtime_) { - if (!rclcpp::ok()) { - return nullptr; - } - std::this_thread::sleep_for(std::chrono::microseconds(200)); - } - std::vector - & from = controllers_lists_[current_controllers_list_], - & to = controllers_lists_[free_controllers_list]; + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + + std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); + const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); to.clear(); // Copy all controllers from the 'from' list to the 'to' list @@ -441,15 +424,13 @@ ControllerManager::add_controller_impl( to.emplace_back(controller); // Destroys the old controllers list when the realtime thread is finished with it. - int former_current_controllers_list_ = current_controllers_list_; - current_controllers_list_ = free_controllers_list; - while (rclcpp::ok() && used_by_realtime_ == former_current_controllers_list_) { - if (!rclcpp::ok()) { - return nullptr; - } - std::this_thread::sleep_for(std::chrono::microseconds(200)); - } - from.clear(); + RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list"); + rt_controllers_wrapper_.switch_updated_list(guard); + RCLCPP_DEBUG(get_logger(), "Destruct controller"); + std::vector & new_unused_list = rt_controllers_wrapper_.get_unused_list( + guard); + new_unused_list.clear(); + RCLCPP_DEBUG(get_logger(), "Destruct controller finished"); return to.back().c; } @@ -457,10 +438,10 @@ ControllerManager::add_controller_impl( controller_interface::ControllerInterface * ControllerManager::get_controller_by_name( const std::string & name) { - // Lock recursive mutex in this context - std::lock_guard guard(controllers_lock_); + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - for (const auto & controller : controllers_lists_[current_controllers_list_]) { + for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { if (controller.info.name == name) { return controller.c.get(); } @@ -605,9 +586,10 @@ void ControllerManager::list_controllers_srv_cb( std::lock_guard services_guard(services_lock_); RCLCPP_DEBUG(get_logger(), "list controller service locked"); - // lock controllers to get all names/types/states - std::lock_guard controller_guard(controllers_lock_); - auto & controllers = controllers_lists_[current_controllers_list_]; + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + const std::vector & controllers = + rt_controllers_wrapper_.get_updated_list(guard); response->controller.resize(controllers.size()); for (size_t i = 0; i < controllers.size(); ++i) { @@ -685,8 +667,9 @@ void ControllerManager::reload_controller_libraries_service_cb( std::vector loaded_controllers, running_controllers; get_controller_names(loaded_controllers); { - std::lock_guard guard(controllers_lock_); - for (const auto & controller : controllers_lists_[current_controllers_list_]) { + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { if (isControllerRunning(*controller.c)) { running_controllers.push_back(controller.info.name); } @@ -780,9 +763,11 @@ void ControllerManager::unload_controller_service_cb( void ControllerManager::get_controller_names(std::vector & names) { - std::lock_guard guard(controllers_lock_); names.clear(); - for (const auto & controller : controllers_lists_[current_controllers_list_]) { + + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { names.push_back(controller.info.name); } } @@ -790,10 +775,11 @@ void ControllerManager::get_controller_names(std::vector & names) controller_interface::return_type ControllerManager::update() { - used_by_realtime_ = current_controllers_list_; + std::vector & rt_controller_list = + rt_controllers_wrapper_.get_used_by_rt_list(); auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : controllers_lists_[used_by_realtime_]) { + for (auto loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 if (isControllerRunning(*loaded_controller.c)) { @@ -844,7 +830,9 @@ ControllerManager::configure() &ControllerManager::unload_controller_service_cb, this, _1, _2)); - for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + for (auto loaded_controller : rt_controllers_wrapper_.get_updated_list(guard)) { auto controller_state = loaded_controller.c->get_lifecycle_node()->configure(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { ret = controller_interface::return_type::ERROR; @@ -855,10 +843,10 @@ ControllerManager::configure() } controller_interface::return_type -ControllerManager::activate() const +ControllerManager::activate() { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { + for (auto loaded_controller : rt_controllers_wrapper_.get_used_by_rt_list()) { auto controller_state = loaded_controller.c->get_lifecycle_node()->activate(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { ret = controller_interface::return_type::ERROR; @@ -869,10 +857,10 @@ ControllerManager::activate() const } controller_interface::return_type -ControllerManager::deactivate() const +ControllerManager::deactivate() { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { + for (auto loaded_controller : rt_controllers_wrapper_.get_used_by_rt_list()) { auto controller_state = loaded_controller.c->get_lifecycle_node()->deactivate(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { ret = controller_interface::return_type::ERROR; @@ -883,10 +871,10 @@ ControllerManager::deactivate() const } controller_interface::return_type -ControllerManager::cleanup() const +ControllerManager::cleanup() { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : controllers_lists_[current_controllers_list_]) { + for (auto loaded_controller : rt_controllers_wrapper_.get_used_by_rt_list()) { auto controller_state = loaded_controller.c->get_lifecycle_node()->cleanup(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { ret = controller_interface::return_type::ERROR; @@ -896,4 +884,58 @@ ControllerManager::cleanup() const return ret; } +std::vector & +ControllerManager::RTControllerListWrapper::get_used_by_rt_list() +{ + used_by_realtime_controllers_index_ = updated_controllers_index_; + return controllers_lists_[used_by_realtime_controllers_index_]; +} + +std::vector & +ControllerManager::RTControllerListWrapper::get_unused_list( + const std::lock_guard &) +{ + assert(controllers_lock_.try_lock()); + controllers_lock_.unlock(); + // Get the index to the outdated controller list + int free_controllers_list = get_other_list(updated_controllers_index_); + + // Wait until the outdated controller list is not being used by the realtime thread + wait_until_rt_not_using(free_controllers_list); + return controllers_lists_[free_controllers_list]; +} + +const std::vector & ControllerManager::RTControllerListWrapper::get_updated_list( + const std::lock_guard &) const +{ + assert(controllers_lock_.try_lock()); + controllers_lock_.unlock(); + return controllers_lists_[updated_controllers_index_]; +} + +void ControllerManager::RTControllerListWrapper::switch_updated_list( + const std::lock_guard &) +{ + assert(controllers_lock_.try_lock()); + controllers_lock_.unlock(); + int former_current_controllers_list_ = updated_controllers_index_; + updated_controllers_index_ = get_other_list(former_current_controllers_list_); + wait_until_rt_not_using(former_current_controllers_list_); +} + +int ControllerManager::RTControllerListWrapper::get_other_list(int index) const +{ + return (index + 1) % 2; +} + +void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(int index) const +{ + while (used_by_realtime_controllers_index_ == index) { + if (!rclcpp::ok()) { + throw std::runtime_error("rclcpp interrupted"); + } + std::this_thread::sleep_for(std::chrono::microseconds(200)); + } +} + } // namespace controller_manager From 2b81d5d13140e32df3b85aaa606498923be698d3 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 18 Sep 2020 13:26:07 +0200 Subject: [PATCH 10/27] Rename isControllerRunning function --- controller_manager/src/controller_manager.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 60c5ac41c7..dd70fb54f9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -31,7 +31,7 @@ namespace controller_manager { -bool isControllerRunning(controller_interface::ControllerInterface & controller) +bool is_controller_running(controller_interface::ControllerInterface & controller) { return controller.get_lifecycle_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; @@ -102,7 +102,7 @@ controller_interface::return_type ControllerManager::unload_controller( bool removed = false; for (const auto & controller : from) { if (controller.info.name == controller_name) { - if (isControllerRunning(*controller.c)) { + if (is_controller_running(*controller.c)) { to.clear(); RCLCPP_ERROR( get_logger(), @@ -270,7 +270,7 @@ controller_interface::return_type ControllerManager::switch_controller( } } - const bool is_running = isControllerRunning(*controller.c); + const bool is_running = is_controller_running(*controller.c); if (!is_running && in_stop_list) { // check for double stop if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { @@ -474,7 +474,7 @@ void ControllerManager::stop_controllers() { // stop controllers for (const auto & request : stop_request_) { - if (isControllerRunning(*request)) { + if (is_controller_running(*request)) { const auto new_state = request->get_lifecycle_node()->deactivate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( @@ -670,7 +670,7 @@ void ControllerManager::reload_controller_libraries_service_cb( // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { - if (isControllerRunning(*controller.c)) { + if (is_controller_running(*controller.c)) { running_controllers.push_back(controller.info.name); } } @@ -782,7 +782,7 @@ ControllerManager::update() for (auto loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 - if (isControllerRunning(*loaded_controller.c)) { + if (is_controller_running(*loaded_controller.c)) { auto controller_ret = loaded_controller.c->update(); if (controller_ret != controller_interface::return_type::SUCCESS) { ret = controller_ret; From e4e226c8c2c3d7d828d46b6025fbe8bb932bf0c7 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 18 Sep 2020 13:30:27 +0200 Subject: [PATCH 11/27] Don't use raw pointers in switch controller and get_controller_by_name --- .../controller_manager/controller_manager.hpp | 5 +++-- controller_manager/src/controller_manager.cpp | 21 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 0910ef4577..506f7a66f1 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -134,7 +134,8 @@ class ControllerManager : public rclcpp::Node add_controller_impl(const ControllerSpec & controller); CONTROLLER_MANAGER_PUBLIC - controller_interface::ControllerInterface * get_controller_by_name(const std::string & name); + controller_interface::ControllerInterfaceSharedPtr get_controller_by_name( + const std::string & name); CONTROLLER_MANAGER_PUBLIC void manage_switch(); @@ -267,7 +268,7 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr unload_controller_service_; - std::vector start_request_, stop_request_; + std::vector start_request_, stop_request_; #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING // std::list switch_start_list_, switch_stop_list_; #endif diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index dd70fb54f9..2d5bdbdb01 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -186,11 +186,10 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str()); } - controller_interface::ControllerInterface * ct; // list all controllers to stop for (const auto & controller : stop_controllers) { - ct = get_controller_by_name(controller); - if (ct == nullptr) { + controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller); + if (!ct.get()) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR( get_logger(), @@ -216,8 +215,8 @@ controller_interface::return_type ControllerManager::switch_controller( // list all controllers to start for (const auto & controller : start_controllers) { - ct = get_controller_by_name(controller); - if (ct == nullptr) { + controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller); + if (!ct.get()) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR( get_logger(), @@ -256,7 +255,7 @@ controller_interface::return_type ControllerManager::switch_controller( for (const auto & controller : controllers) { bool in_stop_list = false; for (const auto & request : stop_request_) { - if (request == controller.c.get()) { + if (request == controller.c) { in_stop_list = true; break; } @@ -264,7 +263,7 @@ controller_interface::return_type ControllerManager::switch_controller( bool in_start_list = false; for (const auto & request : start_request_) { - if (request == controller.c.get()) { + if (request == controller.c) { in_start_list = true; break; } @@ -290,7 +289,7 @@ controller_interface::return_type ControllerManager::switch_controller( stop_request_.erase( std::remove( stop_request_.begin(), stop_request_.end(), - controller.c.get()), stop_request_.end()); + controller.c), stop_request_.end()); } } @@ -312,7 +311,7 @@ controller_interface::return_type ControllerManager::switch_controller( start_request_.erase( std::remove( start_request_.begin(), start_request_.end(), - controller.c.get()), start_request_.end()); + controller.c), start_request_.end()); } } @@ -435,7 +434,7 @@ ControllerManager::add_controller_impl( return to.back().c; } -controller_interface::ControllerInterface * ControllerManager::get_controller_by_name( +controller_interface::ControllerInterfaceSharedPtr ControllerManager::get_controller_by_name( const std::string & name) { // lock controllers @@ -443,7 +442,7 @@ controller_interface::ControllerInterface * ControllerManager::get_controller_by for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { if (controller.info.name == name) { - return controller.c.get(); + return controller.c; } } return nullptr; From 64ff2b2ab85c5f9088f8625bac16bbbc47991cde Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Fri, 18 Sep 2020 13:41:39 +0200 Subject: [PATCH 12/27] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../include/controller_manager/controller_loader_interface.hpp | 2 +- .../include/controller_manager/controller_loader_pluginlib.hpp | 2 +- .../include/hardware_interface/controller_info.hpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_loader_interface.hpp b/controller_manager/include/controller_manager/controller_loader_interface.hpp index 91eb670983..32ea50d5a2 100644 --- a/controller_manager/include/controller_manager/controller_loader_interface.hpp +++ b/controller_manager/include/controller_manager/controller_loader_interface.hpp @@ -47,7 +47,7 @@ class ControllerLoaderInterface const std::string & getName() const {return name_;} CONTROLLER_MANAGER_PUBLIC - virtual std::vector getDeclaredClasses() = 0; + virtual std::vector getDeclaredClasses() const = 0; CONTROLLER_MANAGER_PUBLIC virtual void reload() = 0; diff --git a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp index d4fa10a071..00454d423a 100644 --- a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp +++ b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp @@ -42,7 +42,7 @@ class ControllerLoaderPluginlib : public ControllerLoaderInterface bool is_available(const std::string & controller_type) const; CONTROLLER_MANAGER_PUBLIC - std::vector getDeclaredClasses() override; + std::vector getDeclaredClasses() const override; CONTROLLER_MANAGER_PUBLIC void reload() override; diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index ec520fac78..75a79dc191 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -38,7 +38,7 @@ struct ControllerInfo // TODO(v-lopez) /** Claimed resources, grouped by the hardware interface they belong to. */ -// std::vector claimed_resources; +std::map> resources; }; } // namespace hardware_interface From 34816769d559389dcd72fa9aa34fc62576954b16 Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Mon, 21 Sep 2020 10:34:33 +0200 Subject: [PATCH 13/27] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Bence Magyar Co-authored-by: Denis Štogl --- .../include/controller_manager/controller_manager.hpp | 10 +++++----- controller_manager/src/controller_manager.cpp | 1 - .../test/test_controller_manager_srvs.cpp | 2 +- controller_manager_msgs/srv/LoadController.srv | 2 +- .../srv/ReloadControllerLibraries.srv | 1 - controller_manager_msgs/srv/UnloadController.srv | 2 +- .../include/hardware_interface/controller_info.hpp | 4 ++-- 7 files changed, 10 insertions(+), 12 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 506f7a66f1..54987e5370 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -86,8 +86,8 @@ class ControllerManager : public rclcpp::Node T *, controller_interface::ControllerInterface *>::value, T>::type * = nullptr> controller_interface::ControllerInterfaceSharedPtr add_controller( - std::shared_ptr controller, std::string controller_name, - std::string controller_type) + std::shared_ptr controller, const std::string & controller_name, + const std::string & controller_type) { ControllerSpec controller_spec; controller_spec.c = controller; @@ -189,7 +189,7 @@ class ControllerManager : public rclcpp::Node /** * @brief The RTControllerListWrapper class wraps a double-buffered list of controllers - * to avoid needing to lock the real-time thread when switching controllers in + * to avoid needing to lock the real-time thread when switching controllers in * the non-real-time thread. * * There's always an "updated" list and an "outdated" one @@ -246,9 +246,9 @@ class ControllerManager : public rclcpp::Node std::vector controllers_lists_[2]; /// The index of the controller list with the most updated information - int updated_controllers_index_ = {0}; + int updated_controllers_index_ = 0; /// The index of the controllers list being used in the real-time thread. - int used_by_realtime_controllers_index_ = {-1}; + int used_by_realtime_controllers_index_ = -1; }; RTControllerListWrapper rt_controllers_wrapper_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2d5bdbdb01..35a02e1d02 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -25,7 +25,6 @@ #include "controller_manager_msgs/srv/switch_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" - #include "rclcpp/rclcpp.hpp" namespace controller_manager diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index e83c672f1b..65886dfdaf 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 PAL Robotics S.L. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager_msgs/srv/LoadController.srv b/controller_manager_msgs/srv/LoadController.srv index 106b288142..8130706870 100644 --- a/controller_manager_msgs/srv/LoadController.srv +++ b/controller_manager_msgs/srv/LoadController.srv @@ -7,4 +7,4 @@ string name --- -bool ok \ No newline at end of file +bool ok diff --git a/controller_manager_msgs/srv/ReloadControllerLibraries.srv b/controller_manager_msgs/srv/ReloadControllerLibraries.srv index 01483856fb..f5bc880fe1 100644 --- a/controller_manager_msgs/srv/ReloadControllerLibraries.srv +++ b/controller_manager_msgs/srv/ReloadControllerLibraries.srv @@ -1,7 +1,6 @@ # The ReloadControllerLibraries service will reload all controllers that are available in # the system as plugins - # Reloading libraries only works if there are no controllers loaded. If there # are still some controllers loaded, the reloading will fail. # If this bool is set to true, all loaded controllers will get diff --git a/controller_manager_msgs/srv/UnloadController.srv b/controller_manager_msgs/srv/UnloadController.srv index ecb4a88447..3b9e7b62f5 100644 --- a/controller_manager_msgs/srv/UnloadController.srv +++ b/controller_manager_msgs/srv/UnloadController.srv @@ -7,4 +7,4 @@ string name --- -bool ok \ No newline at end of file +bool ok diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 75a79dc191..c0f02a6992 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 PAL Robotics S.L. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -38,7 +38,7 @@ struct ControllerInfo // TODO(v-lopez) /** Claimed resources, grouped by the hardware interface they belong to. */ -std::map> resources; +// std::map> resources; }; } // namespace hardware_interface From 62df3aabb89dfd1912dc16cb3ca7409539dd60c0 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 21 Sep 2020 12:06:05 +0200 Subject: [PATCH 14/27] Documentation for controller_manager RT List wrapper and parameters --- .../controller_manager/controller_manager.hpp | 11 +++++++---- controller_manager/src/controller_manager.cpp | 15 +++++++++++---- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 54987e5370..1a4d398fbd 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -196,18 +196,18 @@ class ControllerManager : public rclcpp::Node * There's always an "used by rt" list and an "unused by rt" list * * The updated state changes on the switch_updated_list() - * The rt usage state changes on the get_used_by_rt_list() + * The rt usage state changes on the update_and_get_used_by_rt_list() */ class RTControllerListWrapper { public: /** - * @brief get_used_by_rt_list Makes the "updated" list the "used by rt" list - * @warning Should only be called by the RT thread, noone should modify the + * @brief update_and_get_used_by_rt_list Makes the "updated" list the "used by rt" list + * @warning Should only be called by the RT thread, no one should modify the * updated list while it's being used * @return reference to the updated list */ - std::vector & get_used_by_rt_list(); + std::vector & update_and_get_used_by_rt_list(); /** * @brief get_unused_list Waits until the "outdated" and "unused by rt" @@ -240,6 +240,9 @@ class ControllerManager : public rclcpp::Node mutable std::recursive_mutex controllers_lock_; private: + /** + * @brief get_other_list get the list not pointed by index + */ int get_other_list(int index) const; void wait_until_rt_not_using(int index) const; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 35a02e1d02..d0e0946dbd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -79,6 +79,13 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_contr { const std::string param_name = controller_name + ".type"; std::string controller_type; + + // A priori we don't know the name of the controllers that will be loaded, so we cannot + // declare paramaters with their names. + // So when we're told to load a controller by name, we need to declare the parameter if + // we haven't done so, and then read it. + + // Check if parameter has been declared if (!has_parameter(param_name)) { declare_parameter(param_name, rclcpp::ParameterValue()); } @@ -774,7 +781,7 @@ controller_interface::return_type ControllerManager::update() { std::vector & rt_controller_list = - rt_controllers_wrapper_.get_used_by_rt_list(); + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); auto ret = controller_interface::return_type::SUCCESS; for (auto loaded_controller : rt_controller_list) { @@ -858,7 +865,7 @@ controller_interface::return_type ControllerManager::deactivate() { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : rt_controllers_wrapper_.get_used_by_rt_list()) { + for (auto loaded_controller : rt_controllers_wrapper_.update_and_get_used_by_rt_list()) { auto controller_state = loaded_controller.c->get_lifecycle_node()->deactivate(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { ret = controller_interface::return_type::ERROR; @@ -872,7 +879,7 @@ controller_interface::return_type ControllerManager::cleanup() { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : rt_controllers_wrapper_.get_used_by_rt_list()) { + for (auto loaded_controller : rt_controllers_wrapper_.update_and_get_used_by_rt_list()) { auto controller_state = loaded_controller.c->get_lifecycle_node()->cleanup(); if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { ret = controller_interface::return_type::ERROR; @@ -883,7 +890,7 @@ ControllerManager::cleanup() } std::vector & -ControllerManager::RTControllerListWrapper::get_used_by_rt_list() +ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list() { used_by_realtime_controllers_index_ = updated_controllers_index_; return controllers_lists_[used_by_realtime_controllers_index_]; From bc63827074a5ebba9cc862bf90f5edd56ac3969a Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 21 Sep 2020 12:06:43 +0200 Subject: [PATCH 15/27] controller_manager activate will not activate controllers --- controller_manager/src/controller_manager.cpp | 7 ----- .../test/test_controller_manager.cpp | 29 +++++++++++++++++++ 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d0e0946dbd..2420500bad 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -851,13 +851,6 @@ controller_interface::return_type ControllerManager::activate() { auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : rt_controllers_wrapper_.get_used_by_rt_list()) { - auto controller_state = loaded_controller.c->get_lifecycle_node()->activate(); - if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - ret = controller_interface::return_type::ERROR; - } - } - return ret; } diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 6bf57c4876..190d455e0f 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "controller_manager/controller_manager.hpp" #include "controller_manager_msgs/srv/list_controllers.hpp" @@ -47,6 +48,34 @@ TEST_F(TestControllerManager, controller_lifecycle) { test_controller->get_lifecycle_node()->get_current_state().id()); EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->activate()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_node()->get_current_state().id()) << + "Controllers should be started manually"; + + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); From 3650c388817d690e2f0f540fd6ba9857748cdc63 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 21 Sep 2020 12:07:08 +0200 Subject: [PATCH 16/27] Fix constness in controller_loader --- controller_manager/src/controller_loader_pluginlib.cpp | 2 +- controller_manager/test/controller_manager_test_common.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_loader_pluginlib.cpp b/controller_manager/src/controller_loader_pluginlib.cpp index 911d25e133..583c4d17e1 100644 --- a/controller_manager/src/controller_loader_pluginlib.cpp +++ b/controller_manager/src/controller_loader_pluginlib.cpp @@ -38,7 +38,7 @@ bool ControllerLoaderPluginlib::is_available(const std::string & controller_type return loader_->isClassAvailable(controller_type); } -std::vector ControllerLoaderPluginlib::getDeclaredClasses() +std::vector ControllerLoaderPluginlib::getDeclaredClasses() const { return loader_->getDeclaredClasses(); } diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 9768aed773..ddfc8835c3 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -73,7 +73,7 @@ class ControllerLoaderMock : public controller_manager::ControllerLoaderInterfac {} MOCK_METHOD1(create, controller_interface::ControllerInterfaceSharedPtr(const std::string &)); MOCK_CONST_METHOD1(is_available, bool(const std::string &)); - std::vector getDeclaredClasses() override + std::vector getDeclaredClasses() const override { return {MOCK_TEST_CONTROLLER_NAME}; } From 9c05a5e4d1c52cfb7c98b7b58927ca78aa9c8f27 Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Tue, 22 Sep 2020 09:47:26 +0200 Subject: [PATCH 17/27] Apply suggestions from code review Co-authored-by: Karsten Knese --- .../include/controller_manager/controller_manager.hpp | 4 ++-- controller_manager/src/controller_manager.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 1a4d398fbd..bea2881483 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -105,7 +105,8 @@ class ControllerManager : public rclcpp::Node switch_controller( const std::vector & start_controllers, const std::vector & stop_controllers, - int strictness, bool start_asap = WAIT_FOR_ALL_RESOURCES, + int strictness, + bool start_asap = WAIT_FOR_ALL_RESOURCES, const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT)); CONTROLLER_MANAGER_PUBLIC @@ -182,7 +183,6 @@ class ControllerManager : public rclcpp::Node private: void get_controller_names(std::vector & names); - std::shared_ptr hw_; std::shared_ptr executor_; std::vector loaders_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2420500bad..ce1a1b30f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -30,11 +30,12 @@ namespace controller_manager { -bool is_controller_running(controller_interface::ControllerInterface & controller) +inline bool is_controller_running(controller_interface::ControllerInterface & controller) { return controller.get_lifecycle_node()->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } + ControllerManager::ControllerManager( std::shared_ptr hw, std::shared_ptr executor, @@ -162,7 +163,9 @@ void ControllerManager::register_controller_loader(ControllerLoaderInterfaceShar controller_interface::return_type ControllerManager::switch_controller( const std::vector & start_controllers, - const std::vector & stop_controllers, int strictness, bool start_asap, + const std::vector & stop_controllers, + int strictness, + bool start_asap, const rclcpp::Duration & timeout) { switch_params_ = SwitchParams(); From 09671227c7dc5559fbe22cd31a65f506eb938930 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 22 Sep 2020 08:43:56 +0200 Subject: [PATCH 18/27] Fix remaining camelCase to snake_case --- .../controller_manager/controller_loader_interface.hpp | 4 ++-- .../controller_manager/controller_loader_pluginlib.hpp | 2 +- controller_manager/src/controller_loader_pluginlib.cpp | 2 +- controller_manager/src/controller_manager.cpp | 6 +++--- controller_manager/test/controller_manager_test_common.hpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_loader_interface.hpp b/controller_manager/include/controller_manager/controller_loader_interface.hpp index 32ea50d5a2..099c5f41f1 100644 --- a/controller_manager/include/controller_manager/controller_loader_interface.hpp +++ b/controller_manager/include/controller_manager/controller_loader_interface.hpp @@ -44,10 +44,10 @@ class ControllerLoaderInterface virtual bool is_available(const std::string & controller_type) const = 0; CONTROLLER_MANAGER_PUBLIC - const std::string & getName() const {return name_;} + const std::string & get_name() const {return name_;} CONTROLLER_MANAGER_PUBLIC - virtual std::vector getDeclaredClasses() const = 0; + virtual std::vector get_declared_classes() const = 0; CONTROLLER_MANAGER_PUBLIC virtual void reload() = 0; diff --git a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp index 00454d423a..6b56aa9684 100644 --- a/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp +++ b/controller_manager/include/controller_manager/controller_loader_pluginlib.hpp @@ -42,7 +42,7 @@ class ControllerLoaderPluginlib : public ControllerLoaderInterface bool is_available(const std::string & controller_type) const; CONTROLLER_MANAGER_PUBLIC - std::vector getDeclaredClasses() const override; + std::vector get_declared_classes() const override; CONTROLLER_MANAGER_PUBLIC void reload() override; diff --git a/controller_manager/src/controller_loader_pluginlib.cpp b/controller_manager/src/controller_loader_pluginlib.cpp index 583c4d17e1..4d5fa04bd5 100644 --- a/controller_manager/src/controller_loader_pluginlib.cpp +++ b/controller_manager/src/controller_loader_pluginlib.cpp @@ -38,7 +38,7 @@ bool ControllerLoaderPluginlib::is_available(const std::string & controller_type return loader_->isClassAvailable(controller_type); } -std::vector ControllerLoaderPluginlib::getDeclaredClasses() const +std::vector ControllerLoaderPluginlib::get_declared_classes() const { return loader_->getDeclaredClasses(); } diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ce1a1b30f3..50195141e2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -635,10 +635,10 @@ void ControllerManager::list_controller_types_srv_cb( RCLCPP_DEBUG(get_logger(), "list types service locked"); for (const auto & controller_loader : loaders_) { - std::vector cur_types = controller_loader->getDeclaredClasses(); + std::vector cur_types = controller_loader->get_declared_classes(); for (const auto & cur_type : cur_types) { response->types.push_back(cur_type); - response->base_classes.push_back(controller_loader->getName()); + response->base_classes.push_back(controller_loader->get_name()); RCLCPP_INFO(get_logger(), cur_type); } } @@ -727,7 +727,7 @@ void ControllerManager::reload_controller_libraries_service_cb( controller_loader->reload(); RCLCPP_INFO( get_logger(), "Controller manager: reloaded controller libraries for '%s'", - controller_loader->getName().c_str()); + controller_loader->get_name().c_str()); } response->ok = true; diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index ddfc8835c3..1ae27fc58e 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -73,7 +73,7 @@ class ControllerLoaderMock : public controller_manager::ControllerLoaderInterfac {} MOCK_METHOD1(create, controller_interface::ControllerInterfaceSharedPtr(const std::string &)); MOCK_CONST_METHOD1(is_available, bool(const std::string &)); - std::vector getDeclaredClasses() const override + std::vector get_declared_classes() const override { return {MOCK_TEST_CONTROLLER_NAME}; } From 2805044d5b1bc68f25008d57e607c0ec4d556d03 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 22 Sep 2020 09:54:26 +0200 Subject: [PATCH 19/27] Update copyright year --- .../include/controller_manager/controller_manager.hpp | 2 +- .../include/controller_manager/controller_spec.hpp | 2 +- .../include/controller_manager/visibility_control.h | 2 +- controller_manager/src/controller_manager.cpp | 2 +- controller_manager/test/controller_manager_test_common.hpp | 2 +- controller_manager/test/test_controller/test_controller.cpp | 2 +- controller_manager/test/test_controller/test_controller.hpp | 2 +- controller_manager/test/test_controller_manager.cpp | 2 +- controller_manager/test/test_load_controller.cpp | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index bea2881483..20394ade8e 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index dcc2c513c9..5b5deb1ebb 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/include/controller_manager/visibility_control.h b/controller_manager/include/controller_manager/visibility_control.h index d3628b9c11..fa029670c3 100644 --- a/controller_manager/include/controller_manager/visibility_control.h +++ b/controller_manager/include/controller_manager/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 50195141e2..f637898446 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 1ae27fc58e..bd7a606a1e 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 6b15f33718..22113d66ea 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 0c8bf86e40..195dcb6beb 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 190d455e0f..d9c08909b9 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 5f644cad2b..80e65a792e 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From ef1187f0bcddcf815aabc68bd2f7b1cc5714d89e Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 22 Sep 2020 10:34:56 +0200 Subject: [PATCH 20/27] Apply PR suggestions --- .../controller_manager/controller_manager.hpp | 14 ++++++++--- controller_manager/src/controller_manager.cpp | 25 +++++++++++-------- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 20394ade8e..b270f7d242 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -181,7 +181,7 @@ class ControllerManager : public rclcpp::Node std::shared_ptr response); private: - void get_controller_names(std::vector & names); + std::vector get_controller_names(); std::shared_ptr hw_; std::shared_ptr executor_; @@ -200,7 +200,9 @@ class ControllerManager : public rclcpp::Node */ class RTControllerListWrapper { -public: +// *INDENT-OFF* + public: +// *INDENT-ON* /** * @brief update_and_get_used_by_rt_list Makes the "updated" list the "used by rt" list * @warning Should only be called by the RT thread, no one should modify the @@ -239,13 +241,17 @@ class ControllerManager : public rclcpp::Node // must be acquired before using any list other than the "used by rt" mutable std::recursive_mutex controllers_lock_; -private: +// *INDENT-OFF* + private: +// *INDENT-ON* /** * @brief get_other_list get the list not pointed by index */ int get_other_list(int index) const; - void wait_until_rt_not_using(int index) const; + void wait_until_rt_not_using( + int index, + std::chrono::microseconds sleep_delay = std::chrono::microseconds(200)) const; std::vector controllers_lists_[2]; /// The index of the controller list with the most updated information diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f637898446..292a7b09ad 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -206,12 +206,11 @@ controller_interface::return_type ControllerManager::switch_controller( controller.c_str()); stop_request_.clear(); return controller_interface::return_type::ERROR; - } else { - RCLCPP_DEBUG( - get_logger(), - "Could not stop controller with name '%s' because no controller with this name exists", - controller.c_str()); } + RCLCPP_DEBUG( + get_logger(), + "Could not stop controller with name '%s' because no controller with this name exists", + controller.c_str()); } else { RCLCPP_DEBUG( get_logger(), @@ -673,7 +672,7 @@ void ControllerManager::reload_controller_libraries_service_cb( // only reload libraries if no controllers are running std::vector loaded_controllers, running_controllers; - get_controller_names(loaded_controllers); + loaded_controllers = get_controller_names(); { // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); @@ -718,7 +717,7 @@ void ControllerManager::reload_controller_libraries_service_cb( return; } } - get_controller_names(loaded_controllers); + loaded_controllers = get_controller_names(); } assert(loaded_controllers.empty()); @@ -769,15 +768,16 @@ void ControllerManager::unload_controller_service_cb( request->name.c_str()); } -void ControllerManager::get_controller_names(std::vector & names) +std::vector ControllerManager::get_controller_names() { - names.clear(); + std::vector names; // lock controllers std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { names.push_back(controller.info.name); } + return names; } controller_interface::return_type @@ -929,13 +929,16 @@ int ControllerManager::RTControllerListWrapper::get_other_list(int index) const return (index + 1) % 2; } -void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(int index) const +void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using( + int index, + std::chrono::microseconds sleep_period) +const { while (used_by_realtime_controllers_index_ == index) { if (!rclcpp::ok()) { throw std::runtime_error("rclcpp interrupted"); } - std::this_thread::sleep_for(std::chrono::microseconds(200)); + std::this_thread::sleep_for(sleep_period); } } From 0067623a5fde952a9a3995a42a7eb8231142a903 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 22 Sep 2020 11:36:03 +0200 Subject: [PATCH 21/27] Use find algorithms for finding controllers --- controller_manager/src/controller_manager.cpp | 103 ++++++++---------- 1 file changed, 46 insertions(+), 57 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 292a7b09ad..5ceb59edb2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -36,6 +36,11 @@ inline bool is_controller_running(controller_interface::ControllerInterface & co lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } +bool controller_name_compare(const ControllerSpec & a, const std::string & name) +{ + return a.info.name == name; +} + ControllerManager::ControllerManager( std::shared_ptr hw, std::shared_ptr executor, @@ -103,31 +108,15 @@ controller_interface::return_type ControllerManager::unload_controller( std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); - to.clear(); // Transfers the running controllers over, skipping the one to be removed and the running ones. - bool removed = false; - for (const auto & controller : from) { - if (controller.info.name == controller_name) { - if (is_controller_running(*controller.c)) { - to.clear(); - RCLCPP_ERROR( - get_logger(), - "Could not unload controller with name '%s' because it is still running", - controller_name.c_str()); - return controller_interface::return_type::ERROR; - } - RCLCPP_DEBUG(get_logger(), "Cleanup controller"); - controller.c->get_lifecycle_node()->cleanup(); - executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface()); - removed = true; - } else { - to.push_back(controller); - } - } + to = from; - // Fails if we could not remove the controllers - if (!removed) { + auto found_it = std::find_if( + to.begin(), to.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (found_it == to.end()) { + // Fails if we could not remove the controllers to.clear(); RCLCPP_ERROR( get_logger(), @@ -136,6 +125,21 @@ controller_interface::return_type ControllerManager::unload_controller( return controller_interface::return_type::ERROR; } + auto & controller = *found_it; + + if (is_controller_running(*controller.c)) { + to.clear(); + RCLCPP_ERROR( + get_logger(), + "Could not unload controller with name '%s' because it is still running", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + + RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + controller.c->get_lifecycle_node()->cleanup(); + executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface()); + to.erase(found_it); // Destroys the old controllers list when the realtime thread is finished with it. RCLCPP_DEBUG(get_logger(), "Realtime switches over to new controller list"); @@ -260,22 +264,15 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + for (const auto & controller : controllers) { - bool in_stop_list = false; - for (const auto & request : stop_request_) { - if (request == controller.c) { - in_stop_list = true; - break; - } - } + auto stop_list_it = std::find( + stop_request_.begin(), stop_request_.end(), controller.c); + bool in_stop_list = stop_list_it != stop_request_.end(); - bool in_start_list = false; - for (const auto & request : start_request_) { - if (request == controller.c) { - in_start_list = true; - break; - } - } + auto start_list_it = std::find( + start_request_.begin(), start_request_.end(), controller.c); + bool in_start_list = start_list_it != start_request_.end(); const bool is_running = is_controller_running(*controller.c); @@ -294,10 +291,7 @@ controller_interface::return_type ControllerManager::switch_controller( "Could not stop controller '" << controller.info.name << "' since it is not running"); in_stop_list = false; - stop_request_.erase( - std::remove( - stop_request_.begin(), stop_request_.end(), - controller.c), stop_request_.end()); + stop_request_.erase(stop_list_it); } } @@ -316,10 +310,7 @@ controller_interface::return_type ControllerManager::switch_controller( "Could not start controller '" << controller.info.name << "' since it is already running "); in_start_list = false; - start_request_.erase( - std::remove( - start_request_.begin(), start_request_.end(), - controller.c), start_request_.end()); + start_request_.erase(start_list_it); } } @@ -401,23 +392,21 @@ ControllerManager::add_controller_impl( std::vector & to = rt_controllers_wrapper_.get_unused_list(guard); const std::vector & from = rt_controllers_wrapper_.get_updated_list(guard); - to.clear(); // Copy all controllers from the 'from' list to the 'to' list - for (const auto & from_controller : from) { - to.push_back(from_controller); - } + to = from; + auto found_it = std::find_if( + to.begin(), to.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller.info.name)); // Checks that we're not duplicating controllers - for (const auto & to_controller : to) { - if (controller.info.name == to_controller.info.name) { - to.clear(); - RCLCPP_ERROR( - get_logger(), - "A controller named '%s' was already loaded inside the controller manager", - controller.info.name.c_str()); - return nullptr; - } + if (found_it != to.end()) { + to.clear(); + RCLCPP_ERROR( + get_logger(), + "A controller named '%s' was already loaded inside the controller manager", + controller.info.name.c_str()); + return nullptr; } controller.c->init(hw_, controller.info.name); From b6ea36051cebc152d07ab6343b95e6c9e2d3e4ee Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 22 Sep 2020 12:14:20 +0200 Subject: [PATCH 22/27] Deduplicate code --- controller_manager/src/controller_manager.cpp | 145 +++++++++--------- 1 file changed, 72 insertions(+), 73 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5ceb59edb2..8787752a5f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -199,58 +199,57 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str()); } - // list all controllers to stop - for (const auto & controller : stop_controllers) { - controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller); - if (!ct.get()) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { - RCLCPP_ERROR( - get_logger(), - "Could not stop controller with name '%s' because no controller with this name exists", - controller.c_str()); - stop_request_.clear(); - return controller_interface::return_type::ERROR; + const auto foo = [this, strictness](const std::vector & controller_list, + std::vector & request_list, + const std::string & action) + { + // list all controllers to stop/start + for (const auto & controller : controller_list) { + controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller); + if (!ct.get()) { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + RCLCPP_ERROR( + get_logger(), + "Could not %s controller with name '%s' because no controller with this name exists", + action.c_str(), + controller.c_str()); + return controller_interface::return_type::ERROR; + } + RCLCPP_DEBUG( + get_logger(), + "Could not %s controller with name '%s' because no controller with this name exists", + action.c_str(), + controller.c_str()); + } else { + RCLCPP_DEBUG( + get_logger(), + "Found controller '%s' that needs to be %sed in list of controllers", + controller.c_str(), + action.c_str()); + request_list.push_back(ct); + } } RCLCPP_DEBUG( - get_logger(), - "Could not stop controller with name '%s' because no controller with this name exists", - controller.c_str()); - } else { - RCLCPP_DEBUG( - get_logger(), - "Found controller '%s' that needs to be stopped in list of controllers", - controller.c_str()); - stop_request_.push_back(ct); - } + get_logger(), "%s request vector has size %i", + action.c_str(), (int)request_list.size()); + + return controller_interface::return_type::SUCCESS; + }; + + // list all controllers to stop + auto ret = foo(stop_controllers, stop_request_, "stop"); + if (ret != controller_interface::return_type::SUCCESS) { + stop_request_.clear(); + return ret; } - RCLCPP_DEBUG(get_logger(), "Stop request vector has size %i", (int)stop_request_.size()); // list all controllers to start - for (const auto & controller : start_controllers) { - controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller); - if (!ct.get()) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { - RCLCPP_ERROR( - get_logger(), - "Could not start controller with name '%s' because no controller with this name exists", - controller.c_str()); - stop_request_.clear(); - start_request_.clear(); - return controller_interface::return_type::ERROR; - } else { - RCLCPP_DEBUG( - get_logger(), - "Could not start controller with name '%s' because no controller with this name exists", - controller.c_str()); - } - } else { - RCLCPP_DEBUG( - get_logger(), "Found controller '%s' that needs to be started in list of controllers", - controller.c_str()); - start_request_.push_back(ct); - } + ret = foo(start_controllers, start_request_, "start"); + if (ret != controller_interface::return_type::SUCCESS) { + stop_request_.clear(); + start_request_.clear(); + return ret; } - RCLCPP_DEBUG(get_logger(), "Start request vector has size %i", (int)start_request_.size()); #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING // Do the resource management checking @@ -276,42 +275,42 @@ controller_interface::return_type ControllerManager::switch_controller( const bool is_running = is_controller_running(*controller.c); - if (!is_running && in_stop_list) { // check for double stop - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Could not stop controller '" << controller.info.name << - "' since it is not running"); - stop_request_.clear(); - start_request_.clear(); - return controller_interface::return_type::ERROR; - } else { + auto handle_conflict = [&](const std::string & msg) + { + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { + RCLCPP_ERROR_STREAM( + get_logger(), + msg); + stop_request_.clear(); + start_request_.clear(); + return controller_interface::return_type::ERROR; + } RCLCPP_DEBUG_STREAM( get_logger(), "Could not stop controller '" << controller.info.name << "' since it is not running"); - in_stop_list = false; - stop_request_.erase(stop_list_it); + return controller_interface::return_type::SUCCESS; + }; + if (!is_running && in_stop_list) { // check for double stop + auto ret = handle_conflict( + "Could not stop controller '" + controller.info.name + + "' since it is not running"); + if (ret != controller_interface::return_type::SUCCESS) { + return ret; } + in_stop_list = false; + stop_request_.erase(stop_list_it); } if (is_running && !in_stop_list && in_start_list) { // check for doubled start - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Controller '" << controller.info.name << - "' is already running"); - stop_request_.clear(); - start_request_.clear(); - return controller_interface::return_type::ERROR; - } else { - RCLCPP_DEBUG_STREAM( - get_logger(), - "Could not start controller '" << controller.info.name << - "' since it is already running "); - in_start_list = false; - start_request_.erase(start_list_it); + auto ret = handle_conflict( + "Could not start controller '" + controller.info.name + + "' since it is already running"); + if (ret != controller_interface::return_type::SUCCESS) { + return ret; } + in_start_list = false; + start_request_.erase(start_list_it); } #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING From 2fba683d2fb8b46fae1a37e66bcdba6acfacd543 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 25 Sep 2020 07:59:59 +0200 Subject: [PATCH 23/27] Remove get_controller_by_name and use names in stop/start request lists get_controller_by_name was confusing as to in which list it might look into, and was returning a non const pointer to a controller that might be being used in the RT thread --- .../controller_manager/controller_manager.hpp | 6 +- controller_manager/src/controller_manager.cpp | 74 ++++++++++++------- 2 files changed, 48 insertions(+), 32 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b270f7d242..20dcf97da0 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -134,10 +134,6 @@ class ControllerManager : public rclcpp::Node controller_interface::ControllerInterfaceSharedPtr add_controller_impl(const ControllerSpec & controller); - CONTROLLER_MANAGER_PUBLIC - controller_interface::ControllerInterfaceSharedPtr get_controller_by_name( - const std::string & name); - CONTROLLER_MANAGER_PUBLIC void manage_switch(); @@ -277,7 +273,7 @@ class ControllerManager : public rclcpp::Node rclcpp::Service::SharedPtr unload_controller_service_; - std::vector start_request_, stop_request_; + std::vector start_request_, stop_request_; #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING // std::list switch_start_list_, switch_stop_list_; #endif diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8787752a5f..e0bc56de7c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -86,8 +86,8 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_contr const std::string param_name = controller_name + ".type"; std::string controller_type; - // A priori we don't know the name of the controllers that will be loaded, so we cannot - // declare paramaters with their names. + // We cannot declare the parameters for the controllers that will be loaded in the future, + // because they are plugins and we cannot be aware of all of them. // So when we're told to load a controller by name, we need to declare the parameter if // we haven't done so, and then read it. @@ -200,13 +200,21 @@ controller_interface::return_type ControllerManager::switch_controller( } const auto foo = [this, strictness](const std::vector & controller_list, - std::vector & request_list, + std::vector & request_list, const std::string & action) { + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + // list all controllers to stop/start for (const auto & controller : controller_list) { - controller_interface::ControllerInterfaceSharedPtr ct = get_controller_by_name(controller); - if (!ct.get()) { + const auto & updated_controllers = rt_controllers_wrapper_.get_updated_list(guard); + + auto found_it = std::find_if( + updated_controllers.begin(), updated_controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller)); + + if (found_it == updated_controllers.end()) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) { RCLCPP_ERROR( get_logger(), @@ -226,7 +234,7 @@ controller_interface::return_type ControllerManager::switch_controller( "Found controller '%s' that needs to be %sed in list of controllers", controller.c_str(), action.c_str()); - request_list.push_back(ct); + request_list.push_back(controller); } } RCLCPP_DEBUG( @@ -266,11 +274,11 @@ controller_interface::return_type ControllerManager::switch_controller( for (const auto & controller : controllers) { auto stop_list_it = std::find( - stop_request_.begin(), stop_request_.end(), controller.c); + stop_request_.begin(), stop_request_.end(), controller.info.name); bool in_stop_list = stop_list_it != stop_request_.end(); auto start_list_it = std::find( - start_request_.begin(), start_request_.end(), controller.c); + start_request_.begin(), start_request_.end(), controller.info.name); bool in_start_list = start_list_it != start_request_.end(); const bool is_running = is_controller_running(*controller.c); @@ -430,20 +438,6 @@ ControllerManager::add_controller_impl( return to.back().c; } -controller_interface::ControllerInterfaceSharedPtr ControllerManager::get_controller_by_name( - const std::string & name) -{ - // lock controllers - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - - for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { - if (controller.info.name == name) { - return controller.c; - } - } - return nullptr; -} - void ControllerManager::manage_switch() { #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING @@ -467,15 +461,28 @@ void ControllerManager::manage_switch() void ControllerManager::stop_controllers() { + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); // stop controllers for (const auto & request : stop_request_) { - if (is_controller_running(*request)) { - const auto new_state = request->get_lifecycle_node()->deactivate(); + auto found_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, request)); + if (found_it == rt_controller_list.end()) { + RCLCPP_ERROR( + get_logger(), + "Got request to stop controller %s but it is not in the realtime controller list", + request.c_str()); + continue; + } + auto controller = found_it->c; + if (is_controller_running(*controller)) { + const auto new_state = controller->get_lifecycle_node()->deactivate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( get_logger(), "After deactivating, controller %s is in state %s, expected Inactive", - request->get_lifecycle_node()->get_name(), + request.c_str(), new_state.label().c_str()); } } @@ -511,13 +518,26 @@ void ControllerManager::start_controllers() } #else // Dummy implementation, replace with the code above when migrated + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); for (const auto & request : start_request_) { - const auto new_state = request->get_lifecycle_node()->activate(); + auto found_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, request)); + if (found_it == rt_controller_list.end()) { + RCLCPP_ERROR( + get_logger(), + "Got request to start controller %s but it is not in the realtime controller list", + request.c_str()); + continue; + } + auto controller = found_it->c; + const auto new_state = controller->get_lifecycle_node()->activate(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( get_logger(), "After activating, controller %s is in state %s, expected Active", - request->get_lifecycle_node()->get_name(), + controller->get_lifecycle_node()->get_name(), new_state.label().c_str()); } } From dd0280151fad027ca13ac8bcc44e83138f845413 Mon Sep 17 00:00:00 2001 From: Victor Lopez <3469405+v-lopez@users.noreply.github.com> Date: Fri, 25 Sep 2020 08:45:01 +0200 Subject: [PATCH 24/27] Apply suggestions from code review Co-authored-by: Karsten Knese --- .../include/controller_manager/controller_manager.hpp | 5 ++--- .../include/controller_manager/controller_spec.hpp | 1 - controller_manager/src/controller_manager.cpp | 7 +++---- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 20dcf97da0..145512d00c 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -184,7 +184,7 @@ class ControllerManager : public rclcpp::Node std::vector loaders_; /** - * @brief The RTControllerListWrapper class wraps a double-buffered list of controllers + * @brief The RTControllerListWrapper class wraps a double-buffered list of controllers * to avoid needing to lock the real-time thread when switching controllers in * the non-real-time thread. * @@ -209,7 +209,7 @@ class ControllerManager : public rclcpp::Node /** * @brief get_unused_list Waits until the "outdated" and "unused by rt" - * lists match and returns a reference to it + * lists match and returns a reference to it * This referenced list can be modified safely until switch_updated_controller_list() * is called, at this point the RT thread may start using it at any time * @param guard Guard needed to make sure the caller is the only one accessing the unused by rt list @@ -232,7 +232,6 @@ class ControllerManager : public rclcpp::Node */ void switch_updated_list(const std::lock_guard & guard); - // Mutex protecting the controllers list // must be acquired before using any list other than the "used by rt" mutable std::recursive_mutex controllers_lock_; diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 5b5deb1ebb..107713e345 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - /* * Author: Wim Meeussen */ diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e0bc56de7c..0ee0cf4c0b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -199,7 +199,7 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_DEBUG(get_logger(), "- stopping controller '%s'", controller.c_str()); } - const auto foo = [this, strictness](const std::vector & controller_list, + const auto list_controllers = [this, strictness](const std::vector & controller_list, std::vector & request_list, const std::string & action) { @@ -245,14 +245,14 @@ controller_interface::return_type ControllerManager::switch_controller( }; // list all controllers to stop - auto ret = foo(stop_controllers, stop_request_, "stop"); + auto ret = list_controllers(stop_controllers, stop_request_, "stop"); if (ret != controller_interface::return_type::SUCCESS) { stop_request_.clear(); return ret; } // list all controllers to start - ret = foo(start_controllers, start_request_, "start"); + ret = list_controllers(start_controllers, start_request_, "start"); if (ret != controller_interface::return_type::SUCCESS) { stop_request_.clear(); start_request_.clear(); @@ -374,7 +374,6 @@ controller_interface::return_type ControllerManager::switch_controller( switch_params_.timeout = timeout; switch_params_.do_switch = true; - // wait until switch is finished RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop"); while (rclcpp::ok() && switch_params_.do_switch) { From ae107414e21ee9d69c047d623e866f98cef444b8 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Fri, 25 Sep 2020 08:40:38 +0200 Subject: [PATCH 25/27] Remove activate, deactivate, configure and cleanup As discussed in https://github.com/ros-controls/ros2_control/pull/139#pullrequestreview-495110978 Also removed a test that was redundant after these changes --- .../controller_manager/controller_manager.hpp | 16 --- controller_manager/src/controller_manager.cpp | 107 +++++------------- .../test/test_controller_manager.cpp | 53 +++++++-- .../test/test_controller_manager_srvs.cpp | 1 - .../test/test_load_controller.cpp | 99 ---------------- 5 files changed, 69 insertions(+), 207 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 145512d00c..83b8374fb3 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -113,22 +113,6 @@ class ControllerManager : public rclcpp::Node controller_interface::return_type update(); - CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - configure(); - - CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - activate(); - - CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - deactivate(); - - CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - cleanup(); - protected: CONTROLLER_MANAGER_PUBLIC controller_interface::ControllerInterfaceSharedPtr diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 0ee0cf4c0b..9a98f5d82c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -51,6 +51,33 @@ ControllerManager::ControllerManager( // add pluginlib loader by default loaders_({std::make_shared()}) { + using namespace std::placeholders; + list_controllers_service_ = create_service( + "list_controllers", std::bind( + &ControllerManager::list_controllers_srv_cb, this, _1, + _2)); + list_controller_types_service_ = + create_service( + "list_controller_types", std::bind( + &ControllerManager::list_controller_types_srv_cb, this, _1, + _2)); + load_controller_service_ = create_service( + "load_controller", std::bind( + &ControllerManager::load_controller_service_cb, this, _1, + _2)); + reload_controller_libraries_service_ = + create_service( + "reload_controller_libraries", std::bind( + &ControllerManager::reload_controller_libraries_service_cb, this, _1, + _2)); + switch_controller_service_ = create_service( + "switch_controller", std::bind( + &ControllerManager::switch_controller_service_cb, this, _1, + _2)); + unload_controller_service_ = create_service( + "unload_controller", std::bind( + &ControllerManager::unload_controller_service_cb, this, _1, + _2)); } controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( @@ -812,86 +839,6 @@ ControllerManager::update() return ret; } -controller_interface::return_type -ControllerManager::configure() -{ - auto ret = controller_interface::return_type::SUCCESS; - - using namespace std::placeholders; - list_controllers_service_ = create_service( - "list_controllers", std::bind( - &ControllerManager::list_controllers_srv_cb, this, _1, - _2)); - list_controller_types_service_ = - create_service( - "list_controller_types", std::bind( - &ControllerManager::list_controller_types_srv_cb, this, _1, - _2)); - load_controller_service_ = create_service( - "load_controller", std::bind( - &ControllerManager::load_controller_service_cb, this, _1, - _2)); - reload_controller_libraries_service_ = - create_service( - "reload_controller_libraries", std::bind( - &ControllerManager::reload_controller_libraries_service_cb, this, _1, - _2)); - switch_controller_service_ = create_service( - "switch_controller", std::bind( - &ControllerManager::switch_controller_service_cb, this, _1, - _2)); - unload_controller_service_ = create_service( - "unload_controller", std::bind( - &ControllerManager::unload_controller_service_cb, this, _1, - _2)); - - // lock controllers - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); - for (auto loaded_controller : rt_controllers_wrapper_.get_updated_list(guard)) { - auto controller_state = loaded_controller.c->get_lifecycle_node()->configure(); - if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - ret = controller_interface::return_type::ERROR; - } - } - - return ret; -} - -controller_interface::return_type -ControllerManager::activate() -{ - auto ret = controller_interface::return_type::SUCCESS; - return ret; -} - -controller_interface::return_type -ControllerManager::deactivate() -{ - auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : rt_controllers_wrapper_.update_and_get_used_by_rt_list()) { - auto controller_state = loaded_controller.c->get_lifecycle_node()->deactivate(); - if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - ret = controller_interface::return_type::ERROR; - } - } - - return ret; -} - -controller_interface::return_type -ControllerManager::cleanup() -{ - auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : rt_controllers_wrapper_.update_and_get_used_by_rt_list()) { - auto controller_state = loaded_controller.c->get_lifecycle_node()->cleanup(); - if (controller_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - ret = controller_interface::return_type::ERROR; - } - } - - return ret; -} - std::vector & ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list() { diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index d9c08909b9..e3db44d203 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -31,28 +31,22 @@ TEST_F(TestControllerManager, controller_lifecycle) { auto test_controller = std::make_shared(); - auto abstract_test_controller = cm->add_controller( + cm->add_controller( test_controller, test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_TYPE); EXPECT_EQ(1u, cm->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); EXPECT_EQ( 0u, test_controller->internal_counter) << - "Update should not reached an unconfigured and deactivated controller"; + "Update should not reach an unstarted controller"; - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->configure()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->activate()); - EXPECT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - test_controller->get_lifecycle_node()->get_current_state().id()) << - "Controllers should be started manually"; - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; @@ -83,13 +77,50 @@ TEST_F(TestControllerManager, controller_lifecycle) { EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); EXPECT_EQ(1u, test_controller->internal_counter); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->deactivate()); + // Stop controller, will take effect at the end of the update function + start_controllers = {}; + stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; + switch_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::switch_controller, cm, + start_controllers, stop_controllers, + STRICT, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ( + std::future_status::timeout, + switch_future.wait_for(std::chrono::milliseconds(100))) << + "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ( + 2u, + test_controller->internal_counter) << + "Controller is stopped at the end of update, so it should have done one more update"; + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + switch_future.get() + ); + EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_lifecycle_node()->get_current_state().id()); + auto unload_future = std::async( + std::launch::async, + &controller_manager::ControllerManager::unload_controller, cm, + test_controller::TEST_CONTROLLER_NAME); + + ASSERT_EQ( + std::future_status::timeout, + unload_future.wait_for(std::chrono::milliseconds(100))) << + "unload_controller should be blocking until next update cycle"; + cm->update(); + EXPECT_EQ( + controller_interface::return_type::SUCCESS, + unload_future.get() + ); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->cleanup()); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_lifecycle_node()->get_current_state().id()); + EXPECT_EQ(1, test_controller.use_count()); } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 65886dfdaf..d6e97747f9 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -52,7 +52,6 @@ class TestControllerManagerSrvs : public TestControllerManager executor_->add_node(cm_); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->configure()); executor_spin_future_ = std::async( std::launch::async, [this]() -> void { executor_->spin(); diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 80e65a792e..cfd20c320e 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -239,7 +239,6 @@ TEST_F(TestControllerManager, switch_controller) auto cm = std::make_shared( robot_, executor_, "test_controller_manager"); - cm->configure(); std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; // load the controller with name1 @@ -355,7 +354,6 @@ TEST_F(TestControllerManager, switch_multiple_controllers) auto cm = std::make_shared( robot_, executor_, "test_controller_manager"); - cm->configure(); std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; // load the controller with name1 @@ -462,100 +460,3 @@ TEST_F(TestControllerManager, switch_multiple_controllers) abstract_test_controller2.c->get_lifecycle_node()->get_current_state().id()); } } - -TEST_F(TestControllerManager, controller_lifecycle_states) -{ - auto cm = std::make_shared( - robot_, executor_, - "test_controller_manager"); - cm->configure(); - std::string controller_type = test_controller::TEST_CONTROLLER_TYPE; - - // load the controller with name1 - std::string controller_name1 = "test_controller1"; - ASSERT_NO_THROW(cm->load_controller(controller_name1, controller_type)); - EXPECT_EQ(1u, cm->get_loaded_controllers().size()); - controller_manager::ControllerSpec abstract_test_controller1 = - cm->get_loaded_controllers()[0]; - - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); - - RCLCPP_INFO( - cm->get_logger(), - "Starting stopped controller"); - std::vector start_controllers = {controller_name1}; - std::vector stop_controllers = {}; - auto switch_future = std::async( - std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, - start_controllers, stop_controllers, - STRICT, true, rclcpp::Duration(0, 0)); - - ASSERT_EQ( - std::future_status::timeout, - switch_future.wait_for(std::chrono::milliseconds(100))) << - "switch_controller should be blocking until next update cycle"; - cm->update(); - EXPECT_EQ( - controller_interface::return_type::SUCCESS, - switch_future.get() - ); - - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); - - - // Stop controller - start_controllers = {}; - stop_controllers = {controller_name1}; - RCLCPP_INFO( - cm->get_logger(), - "Stopping started controller"); - switch_future = std::async( - std::launch::async, - &controller_manager::ControllerManager::switch_controller, cm, - start_controllers, stop_controllers, - STRICT, true, rclcpp::Duration(0, 0)); - - ASSERT_EQ( - std::future_status::timeout, - switch_future.wait_for(std::chrono::milliseconds(100))) << - "switch_controller should be blocking until next update cycle"; - cm->update(); - EXPECT_EQ( - controller_interface::return_type::SUCCESS, - switch_future.get() - ); - - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); - - - RCLCPP_INFO( - cm->get_logger(), - "Unloading controller"); - EXPECT_EQ(2, abstract_test_controller1.c.use_count()); - - auto unload_future = std::async( - std::launch::async, - &controller_manager::ControllerManager::unload_controller, cm, - controller_name1); - - ASSERT_EQ( - std::future_status::timeout, - unload_future.wait_for(std::chrono::milliseconds(100))) << - "unload_controller should be blocking until next update cycle"; - cm->update(); - EXPECT_EQ( - controller_interface::return_type::SUCCESS, - unload_future.get() - ); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); - EXPECT_EQ(1, abstract_test_controller1.c.use_count()); -} From 9aff325e405f0de3e44e3b375696affccff75302 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 29 Sep 2020 09:29:29 +0200 Subject: [PATCH 26/27] Add namespace to controller_manager services --- controller_manager/src/controller_manager.cpp | 12 ++++++------ .../test/test_controller_manager_srvs.cpp | 11 ++++++----- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9a98f5d82c..958b91eb91 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -53,29 +53,29 @@ ControllerManager::ControllerManager( { using namespace std::placeholders; list_controllers_service_ = create_service( - "list_controllers", std::bind( + "~/list_controllers", std::bind( &ControllerManager::list_controllers_srv_cb, this, _1, _2)); list_controller_types_service_ = create_service( - "list_controller_types", std::bind( + "~/list_controller_types", std::bind( &ControllerManager::list_controller_types_srv_cb, this, _1, _2)); load_controller_service_ = create_service( - "load_controller", std::bind( + "~/load_controller", std::bind( &ControllerManager::load_controller_service_cb, this, _1, _2)); reload_controller_libraries_service_ = create_service( - "reload_controller_libraries", std::bind( + "~/reload_controller_libraries", std::bind( &ControllerManager::reload_controller_libraries_service_cb, this, _1, _2)); switch_controller_service_ = create_service( - "switch_controller", std::bind( + "~/switch_controller", std::bind( &ControllerManager::switch_controller_service_cb, this, _1, _2)); unload_controller_service_ = create_service( - "unload_controller", std::bind( + "~/unload_controller", std::bind( &ControllerManager::unload_controller_service_cb, this, _1, _2)); } diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index d6e97747f9..9a0f6e8b9c 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -104,7 +104,7 @@ TEST_F(TestControllerManagerSrvs, list_controller_types) srv_executor.add_node(srv_node); rclcpp::Client::SharedPtr client = srv_node->create_client( - "list_controller_types"); + "test_controller_manager/list_controller_types"); auto request = std::make_shared(); auto result = call_service_and_wait(*client, request, srv_executor); @@ -143,7 +143,8 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) { rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); srv_executor.add_node(srv_node); rclcpp::Client::SharedPtr client = - srv_node->create_client("list_controllers"); + srv_node->create_client( + "test_controller_manager/list_controllers"); auto request = std::make_shared(); auto result = call_service_and_wait(*client, request, srv_executor); @@ -202,7 +203,7 @@ TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv) { srv_executor.add_node(srv_node); rclcpp::Client::SharedPtr client = srv_node->create_client( - "reload_controller_libraries"); + "test_controller_manager/reload_controller_libraries"); auto request = std::make_shared(); @@ -293,7 +294,7 @@ TEST_F(TestControllerManagerSrvs, load_controller_srv) { srv_executor.add_node(srv_node); rclcpp::Client::SharedPtr client = srv_node->create_client( - "load_controller"); + "test_controller_manager/load_controller"); auto request = std::make_shared(); request->name = test_controller::TEST_CONTROLLER_NAME; @@ -312,7 +313,7 @@ TEST_F(TestControllerManagerSrvs, unload_controller_srv) { srv_executor.add_node(srv_node); rclcpp::Client::SharedPtr client = srv_node->create_client( - "unload_controller"); + "test_controller_manager/unload_controller"); auto request = std::make_shared(); request->name = test_controller::TEST_CONTROLLER_NAME; From 528d1c942754b1d05d353ffa078bb2a420bda654 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Tue, 29 Sep 2020 09:59:38 +0200 Subject: [PATCH 27/27] Allow undeclared parameters on controller_manager --- controller_manager/src/controller_manager.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 958b91eb91..5e06f66992 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -41,11 +41,19 @@ bool controller_name_compare(const ControllerSpec & a, const std::string & name) return a.info.name == name; } +rclcpp::NodeOptions get_cm_node_options() +{ + rclcpp::NodeOptions node_options; + // Required for getting types of controllers to be loaded via service call + node_options.allow_undeclared_parameters(true); + return node_options; +} + ControllerManager::ControllerManager( std::shared_ptr hw, std::shared_ptr executor, const std::string & manager_node_name) -: rclcpp::Node(manager_node_name), +: rclcpp::Node(manager_node_name, get_cm_node_options()), hw_(hw), executor_(executor), // add pluginlib loader by default