diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index db1796da8c..59b20dbeaf 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 @@ -62,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 ) @@ -85,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 @@ -100,6 +114,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_loader_interface.hpp b/controller_manager/include/controller_manager/controller_loader_interface.hpp index d0d1b82594..099c5f41f1 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 & get_name() const {return name_;} + + CONTROLLER_MANAGER_PUBLIC + virtual std::vector get_declared_classes() const = 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..6b56aa9684 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 get_declared_classes() const 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 be7d4bdc7c..83b8374fb3 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. @@ -23,7 +23,14 @@ #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 "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" @@ -36,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, @@ -47,20 +57,25 @@ 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); + /** + * @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 - std::vector> - get_loaded_controllers() const; + controller_interface::return_type unload_controller( + const std::string & controller_name); - [[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); @@ -69,44 +84,196 @@ 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, const std::string & controller_name, + const 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); } + /** + * @brief switch_controller Stops some controllers and others. + * @see Documentation in controller_manager_msgs/SwitchController.srv + */ CONTROLLER_MANAGER_PUBLIC controller_interface::return_type - update(); + switch_controller( + const std::vector & start_controllers, + const std::vector & stop_controllers, + int strictness, + bool start_asap = WAIT_FOR_ALL_RESOURCES, + const rclcpp::Duration & timeout = rclcpp::Duration(INFINITE_TIMEOUT)); CONTROLLER_MANAGER_PUBLIC controller_interface::return_type - configure() const; + update(); +protected: CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - activate() const; + controller_interface::ControllerInterfaceSharedPtr + add_controller_impl(const ControllerSpec & controller); CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - deactivate() const; + void manage_switch(); CONTROLLER_MANAGER_PUBLIC - controller_interface::return_type - cleanup() const; + void stop_controllers(); -protected: CONTROLLER_MANAGER_PUBLIC - std::shared_ptr - add_controller_impl( - std::shared_ptr controller, - const std::string & controller_name); + void start_controllers(); + + CONTROLLER_MANAGER_PUBLIC + void start_controllers_asap(); + + CONTROLLER_MANAGER_PUBLIC + void list_controllers_srv_cb( + 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: + std::vector get_controller_names(); + std::shared_ptr hw_; std::shared_ptr executor_; std::vector loaders_; - std::vector> loaded_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. + * + * 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 update_and_get_used_by_rt_list() + */ + class RTControllerListWrapper + { +// *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 + * updated list while it's being used + * @return reference to the updated list + */ + std::vector & update_and_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_; + +// *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, + 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 + 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_; + 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 +// 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/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp new file mode 100644 index 0000000000..107713e345 --- /dev/null +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -0,0 +1,44 @@ +// 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. +// 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_ + +#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/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/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_loader_pluginlib.cpp b/controller_manager/src/controller_loader_pluginlib.cpp index 8e4a63ebf5..4d5fa04bd5 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::get_declared_classes() const +{ + 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 88a722e1ec..5e06f66992 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. @@ -14,6 +14,7 @@ #include "controller_manager/controller_manager.hpp" +#include #include #include #include @@ -21,28 +22,73 @@ #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" - #include "rclcpp/rclcpp.hpp" namespace controller_manager { +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; +} + +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 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)); } -std::shared_ptr -ControllerManager::load_controller( +controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( const std::string & controller_name, const std::string & controller_type) { @@ -53,7 +99,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 { @@ -61,14 +107,92 @@ 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); } -std::vector> -ControllerManager::get_loaded_controllers() const +controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller( + const std::string & controller_name) { - return loaded_controllers_; + const std::string param_name = controller_name + ".type"; + std::string controller_type; + + // 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. + + // Check if parameter has been declared + 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) +{ + 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); + + // Transfers the running controllers over, skipping the one to be removed and the running ones. + to = from; + + 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(), + "Could not unload controller with name '%s' because no controller with this name exists", + controller_name.c_str()); + 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"); + 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"); + new_unused_list.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 +{ + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + return rt_controllers_wrapper_.get_updated_list(guard); } void ControllerManager::register_controller_loader(ControllerLoaderInterfaceSharedPtr loader) @@ -76,86 +200,708 @@ void ControllerManager::register_controller_loader(ControllerLoaderInterfaceShar loaders_.push_back(loader); } -std::shared_ptr +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()); + } + + const auto list_controllers = [this, strictness](const std::vector & controller_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) { + 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(), + "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(controller); + } + } + RCLCPP_DEBUG( + 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 = 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 = list_controllers(start_controllers, start_request_, "start"); + if (ret != controller_interface::return_type::SUCCESS) { + stop_request_.clear(); + start_request_.clear(); + return ret; + } + +#ifdef TODO_IMPLEMENT_RESOURCE_CHECKING + // Do the resource management checking + std::list info_list; + switch_start_list_.clear(); + switch_stop_list_.clear(); +#endif + + // 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) { + auto stop_list_it = std::find( + 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.info.name); + bool in_start_list = start_list_it != start_request_.end(); + + const bool is_running = is_controller_running(*controller.c); + + 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"); + 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 + 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 + 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; +} + +controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_controller_impl( - std::shared_ptr controller, - const std::string & controller_name) + const ControllerSpec & controller) { - controller->init(hw_, controller_name); - executor_->add_node(controller->get_lifecycle_node()->get_node_base_interface()); + // 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); + + // Copy all controllers from the 'from' list to the 'to' list + to = from; - loaded_controllers_.emplace_back(controller); - return loaded_controllers_.back(); + 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 + 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); + + // 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); + + // Destroys the old controllers list when the realtime thread is finished with it. + 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; } -controller_interface::return_type -ControllerManager::update() +void ControllerManager::manage_switch() { - 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; +#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() +{ + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + // stop controllers + for (const auto & request : stop_request_) { + 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.c_str(), + new_state.label().c_str()); + } } } +} - return ret; +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 + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + for (const auto & request : start_request_) { + 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", + controller->get_lifecycle_node()->get_name(), + new_state.label().c_str()); + } + } + // All controllers started, switching done + switch_params_.do_switch = false; +#endif } -controller_interface::return_type -ControllerManager::configure() const +void ControllerManager::start_controllers_asap() { - auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { - 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; +#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; + } } } - return ret; + // 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::activate() const +void ControllerManager::list_controllers_srv_cb( + const std::shared_ptr, + std::shared_ptr response) { - auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { - 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; + // 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 + 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) { + 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 } - return ret; + RCLCPP_DEBUG(get_logger(), "list controller service finished"); } -controller_interface::return_type -ControllerManager::deactivate() const +void ControllerManager::list_controller_types_srv_cb( + const std::shared_ptr, + std::shared_ptr response) { - auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { - 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; + // 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->get_declared_classes(); + for (const auto & cur_type : cur_types) { + response->types.push_back(cur_type); + response->base_classes.push_back(controller_loader->get_name()); + RCLCPP_INFO(get_logger(), cur_type); } } - return ret; + 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; + loaded_controllers = get_controller_names(); + { + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + for (const auto & controller : rt_controllers_wrapper_.get_updated_list(guard)) { + if (is_controller_running(*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; + } + } + loaded_controllers = get_controller_names(); + } + 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->get_name().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()); +} + +std::vector ControllerManager::get_controller_names() +{ + 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 -ControllerManager::cleanup() const +ControllerManager::update() { + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + auto ret = controller_interface::return_type::SUCCESS; - for (auto loaded_controller : loaded_controllers_) { - 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; + 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 (is_controller_running(*loaded_controller.c)) { + auto controller_ret = loaded_controller.c->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; } +std::vector & +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_]; +} + +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, + 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(sleep_period); + } +} + } // namespace controller_manager 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..bd7a606a1e --- /dev/null +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -0,0 +1,83 @@ +// 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. +// 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 get_declared_classes() const 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.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 f372d2d646..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. @@ -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 89e2081873..e3db44d203 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. @@ -15,64 +15,112 @@ #include #include #include +#include #include "controller_manager/controller_manager.hpp" - +#include "controller_manager_msgs/srv/list_controllers.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) { - 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(test_controller, "test_controller"); - EXPECT_EQ(1u, cm.get_loaded_controllers().size()); + 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(1u, test_controller->internal_counter); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ( + 0u, + test_controller->internal_counter) << + "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(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()); - EXPECT_EQ(controller_interface::return_type::SUCCESS, cm.deactivate()); + EXPECT_EQ(controller_interface::return_type::SUCCESS, cm->update()); + EXPECT_EQ(1u, test_controller->internal_counter); + + // 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 new file mode 100644 index 0000000000..9a0f6e8b9c --- /dev/null +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -0,0 +1,333 @@ +// 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. +// 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_); + + 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( + "test_controller_manager/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( + "test_controller_manager/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( + "test_controller_manager/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( + "test_controller_manager/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( + "test_controller_manager/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 b38b3eebee..cfd20c320e 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. @@ -16,129 +16,97 @@ #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 "lifecycle_msgs/msg/state.hpp" - -#include "rclcpp/utilities.hpp" - -#include "test_controller/test_controller.hpp" - -#include "test_robot_hardware/test_robot_hardware.hpp" +#include "controller_manager_msgs/srv/switch_controller.hpp" +#include "lifecycle_msgs/msg/state.hpp" using ::testing::_; using ::testing::Return; -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()); - 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) { - 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"; 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) { - 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)); - 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) { - 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); @@ -159,12 +127,336 @@ 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) +{ + auto cm = std::make_shared( + 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"; + 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"; +} + +TEST_F(TestControllerManager, switch_controller) +{ + auto cm = std::make_shared( + 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"; + 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()); + + + { // 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.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()); + } +} + +TEST_F(TestControllerManager, switch_multiple_controllers) +{ + auto cm = std::make_shared( + 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"; + 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()); + controller_manager::ControllerSpec abstract_test_controller1 = + cm->get_loaded_controllers()[0]; + controller_manager::ControllerSpec abstract_test_controller2 = + cm->get_loaded_controllers()[1]; + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1.c->get_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + 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 + 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.c->get_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->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.c->get_lifecycle_node()->get_current_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller2.c->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.c->get_lifecycle_node()->get_current_state().id()); + } } diff --git a/controller_manager_msgs/CMakeLists.txt b/controller_manager_msgs/CMakeLists.txt new file mode 100644 index 0000000000..dba0452ecc --- /dev/null +++ b/controller_manager_msgs/CMakeLists.txt @@ -0,0 +1,41 @@ +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(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) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES builtin_interfaces + ADD_LINTER_TESTS +) +ament_export_dependencies(rosidl_default_runtime) +ament_package() 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/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/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..8130706870 --- /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 diff --git a/controller_manager_msgs/srv/ReloadControllerLibraries.srv b/controller_manager_msgs/srv/ReloadControllerLibraries.srv new file mode 100644 index 0000000000..f5bc880fe1 --- /dev/null +++ b/controller_manager_msgs/srv/ReloadControllerLibraries.srv @@ -0,0 +1,10 @@ +# 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/SwitchController.srv b/controller_manager_msgs/srv/SwitchController.srv new file mode 100644 index 0000000000..3c68b0e746 --- /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 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 +builtin_interfaces/Duration timeout +--- +bool ok diff --git a/controller_manager_msgs/srv/UnloadController.srv b/controller_manager_msgs/srv/UnloadController.srv new file mode 100644 index 0000000000..3b9e7b62f5 --- /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 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..c0f02a6992 --- /dev/null +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -0,0 +1,45 @@ +// 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. +// 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 HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ +#define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ + +#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::map> resources; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_