diff --git a/controller_interface/include/controller_interface/controller_interface.hpp b/controller_interface/include/controller_interface/controller_interface.hpp index efdab02d62..a3d006755f 100644 --- a/controller_interface/include/controller_interface/controller_interface.hpp +++ b/controller_interface/include/controller_interface/controller_interface.hpp @@ -53,7 +53,7 @@ class ControllerInterface : public controller_interface::ControllerInterfaceBase /** * Controller is not chainable, therefore no chained mode can be set. * - * \returns false; + * \returns false. */ CONTROLLER_INTERFACE_PUBLIC bool set_chained_mode(bool chained_mode) final; diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index af63f4e9a5..eaaeb9dab8 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -48,7 +48,11 @@ bool get_ordered_interfaces( { if (!interface_type.empty()) { - if ((name == interface.get_name()) && (interface_type == interface.get_interface_name())) + // check case where: + // ( == AND == ) OR / == 'full name' + if ( + ((name == interface.get_name()) && (interface_type == interface.get_interface_name())) || + ((name + "/" + interface_type) == interface.get_full_name())) { ordered_interfaces.push_back(std::ref(interface)); } diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index b103e4c094..be6bd89e4b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -89,6 +89,16 @@ if(BUILD_TESTING) DESTINATION lib ) + add_library(test_chainable_controller SHARED test/test_chainable_controller/test_chainable_controller.cpp) + target_include_directories(test_chainable_controller PRIVATE include) + target_link_libraries(test_chainable_controller controller_manager) + target_compile_definitions(test_chainable_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_chainable_controller/test_chainable_controller.xml) + install(TARGETS test_chainable_controller + DESTINATION lib + ) + ament_add_gmock( test_controller_manager test/test_controller_manager.cpp @@ -114,6 +124,14 @@ if(BUILD_TESTING) target_link_libraries(test_load_controller ${PROJECT_NAME} test_controller test_controller_failed_init) ament_target_dependencies(test_load_controller ros2_control_test_assets) + ament_add_gmock( + test_controllers_chaining_with_controller_manager + test/test_controllers_chaining_with_controller_manager.cpp + ) + target_include_directories(test_controllers_chaining_with_controller_manager PRIVATE include) + target_link_libraries(test_controllers_chaining_with_controller_manager controller_manager test_chainable_controller test_controller) + ament_target_dependencies(test_controllers_chaining_with_controller_manager ros2_control_test_assets) + ament_add_gmock( test_controller_manager_srvs test/test_controller_manager_srvs.cpp diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index e3f253c5e7..b178e21665 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -15,9 +15,11 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_ +#include #include #include #include +#include #include #include "controller_interface/chainable_controller_interface.hpp" @@ -40,15 +42,21 @@ #include "controller_manager_msgs/srv/switch_controller.hpp" #include "controller_manager_msgs/srv/unload_controller.hpp" +#include "hardware_interface/handle.hpp" #include "hardware_interface/resource_manager.hpp" #include "pluginlib/class_loader.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/parameter.hpp" namespace controller_manager { +using ControllersListIterator = std::vector::const_iterator; + class ControllerManager : public rclcpp::Node { public: @@ -169,6 +177,18 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC void stop_controllers(); + /** + * Switch chained mode for all the controllers with respect to the following cases: + * - a preceding controller is getting activated --> switch controller to chained mode; + * - all preceding controllers are deactivated --> switch controller from chained mode. + * + * \param[in] chained_mode_switch_list list of controller to switch chained mode. + * \param[in] to_chained_mode flag if controller should be switched *to* or *from* chained mode. + */ + CONTROLLER_MANAGER_PUBLIC + void switch_chained_mode( + const std::vector & chained_mode_switch_list, bool to_chained_mode); + CONTROLLER_MANAGER_PUBLIC void start_controllers(); @@ -243,11 +263,78 @@ class ControllerManager : public rclcpp::Node // Per controller update rate support unsigned int update_loop_counter_ = 0; unsigned int update_rate_ = 100; + std::vector> chained_controllers_configuration_; + + std::unique_ptr resource_manager_; private: std::vector get_controller_names(); - std::unique_ptr resource_manager_; + /** + * Clear request lists used when switching controllers. The lists are shared between "callback" and + * "control loop" threads. + */ + void clear_requests(); + + /** + * If a controller is deactivated all following controllers (if any exist) should be switched + * 'from' the chained mode. + * + * \param[in] controllers list with controllers. + */ + void propagate_deactivation_of_chained_mode(const std::vector & controllers); + + /// Check if all the following controllers will be in active state and in the chained mode + /// after controllers' switch. + /** + * Check recursively that all following controllers of the @controller_it + * - are already active, + * - will not be deactivated, + * - or will be activated. + * The following controllers are added to the request to switch in the chained mode or removed + * from the request to switch from the chained mode. + * + * For each controller the whole chain of following controllers is checked. + * + * NOTE: The automatically adding of following controller into starting list is not implemented + * yet. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following + * controllers will be automatically added to the activate request list if they are not in the + * deactivate request. + * \param[in] controller_it iterator to the controller for which the following controllers are + * checked. + * + * \returns return_type::OK if all following controllers pass the checks, otherwise + * return_type::ERROR. + */ + controller_interface::return_type check_following_controllers_for_activate( + const std::vector & controllers, int strictness, + const ControllersListIterator controller_it); + + /// Check if all the preceding controllers will be in inactive state after controllers' switch. + /** + * Check that all preceding controllers of the @controller_it + * - are inactive, + * - will be deactivated, + * - and will not be activated. + * + * NOTE: The automatically adding of preceding controllers into stopping list is not implemented + * yet. + * + * \param[in] controllers list with controllers. + * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding + * controllers will be automatically added to the deactivate request list. + * \param[in] controller_it iterator to the controller for which the preceding controllers are + * checked. + * + * \returns return_type::OK if all preceding controllers pass the checks, otherwise + * return_type::ERROR. + */ + controller_interface::return_type check_preceeding_controllers_for_deactivate( + const std::vector & controllers, int strictness, + const ControllersListIterator controller_it); std::shared_ptr executor_; @@ -366,6 +453,7 @@ class ControllerManager : public rclcpp::Node set_hardware_component_state_service_; std::vector start_request_, stop_request_; + std::vector to_chained_mode_request_, from_chained_mode_request_; std::vector start_command_interface_request_, stop_command_interface_request_; struct SwitchParams diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 46e4a1260d..81aa87f42a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -27,9 +27,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" -namespace -{ // utility - +namespace // utility +{ static constexpr const char * kControllerInterfaceNamespace = "controller_interface"; static constexpr const char * kControllerInterfaceClassName = "controller_interface::ControllerInterface"; @@ -75,6 +74,56 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const return a.info.name == name; } +/// Checks if a command interface belongs to a controller based on its prefix. +/** + * A command interface can be provided by a controller in which case is called "reference" + * interface. + * This means that the @interface_name starts with the name of a controller. + * + * \param[in] interface_name to be found in the map. + * \param[in] controllers list of controllers to compare their names to interface's prefix. + * \param[out] following_controller_it iterator to the following controller that reference interface + * @interface_name belongs to. + * \return true if interface has a controller name as prefix, false otherwise. + */ +bool command_interface_is_reference_interface_of_controller( + const std::string interface_name, + const std::vector & controllers, + controller_manager::ControllersListIterator & following_controller_it) +{ + auto split_pos = interface_name.find_first_of('/'); + if (split_pos == std::string::npos) // '/' exist in the string (should be always false) + { + RCLCPP_FATAL( + rclcpp::get_logger("ControllerManager::utils"), + "Character '/', was not find in the interface name '%s'. This should never happen. " + "Stop the controller manager immediately and restart it.", + interface_name.c_str()); + throw std::runtime_error("Mismatched interface name. See the FATAL message above."); + } + + auto interface_prefix = interface_name.substr(0, split_pos); + following_controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, interface_prefix)); + + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Deduced interface prefix '%s' - searching for the controller with the same name.", + interface_prefix.c_str()); + + if (following_controller_it == controllers.end()) + { + RCLCPP_DEBUG( + rclcpp::get_logger("ControllerManager::utils"), + "Required command interface '%s' with prefix '%s' is not reference interface.", + interface_name.c_str(), interface_prefix.c_str()); + + return false; + } + return true; +} + } // namespace namespace controller_manager @@ -258,6 +307,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } return nullptr; } + RCLCPP_DEBUG(get_logger(), "Loader for controller '%s' found.", controller_name.c_str()); controller_interface::ControllerInterfaceBaseSharedPtr controller; @@ -339,6 +389,8 @@ controller_interface::return_type ControllerManager::unload_controller( } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); + // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for + // cleaning-up controllers? controller.c->get_node()->cleanup(); executor_->remove_node(controller.c->get_node()->get_node_base_interface()); to.erase(found_it); @@ -398,6 +450,8 @@ controller_interface::return_type ControllerManager::configure_controller( { RCLCPP_DEBUG( get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str()); + // TODO(destogl): remove reference interface if chainable; i.e., add a separate method for + // cleaning-up controllers? new_state = controller->get_node()->cleanup(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { @@ -417,9 +471,40 @@ controller_interface::return_type ControllerManager::configure_controller( return controller_interface::return_type::ERROR; } + // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers + if (controller->is_chainable()) + { + RCLCPP_DEBUG( + get_logger(), + "Controller '%s' is chainable. Interfaces are being exported to resource manager.", + controller_name.c_str()); + auto interfaces = controller->export_reference_interfaces(); + if (interfaces.empty()) + { + // TODO(destogl): Add test for this! + RCLCPP_ERROR( + get_logger(), "Controller '%s' is chainable, but does not export any reference interfaces.", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + resource_manager_->import_controller_reference_interfaces(controller_name, interfaces); + + // TODO(destogl): check and resort controllers in the vector + } + return controller_interface::return_type::OK; } +void ControllerManager::clear_requests() +{ + stop_request_.clear(); + start_request_.clear(); + to_chained_mode_request_.clear(); + from_chained_mode_request_.clear(); + start_command_interface_request_.clear(); + stop_command_interface_request_.clear(); +} + controller_interface::return_type ControllerManager::switch_controller( const std::vector & start_controllers, const std::vector & stop_controllers, int strictness, bool start_asap, @@ -432,14 +517,24 @@ controller_interface::return_type ControllerManager::switch_controller( 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."); + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } if (!stop_command_interface_request_.empty() || !start_command_interface_request_.empty()) { RCLCPP_FATAL( get_logger(), "The internal stop and start requests command interface lists are not empty at the " - "switch_controller() call. This should not happen."); + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); + } + if (!from_chained_mode_request_.empty() || !to_chained_mode_request_.empty()) + { + RCLCPP_FATAL( + get_logger(), + "The internal 'from' and 'to' chained mode requests are not empty at the " + "switch_controller() call. This should never happen."); + throw std::runtime_error("CM's internal state is not correct. See the FATAL message above."); } if (strictness == 0) { @@ -482,19 +577,15 @@ controller_interface::return_type ControllerManager::switch_controller( if (found_it == updated_controllers.end()) { - if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) - { - RCLCPP_ERROR( - get_logger(), - R"(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_WARN( get_logger(), "Could not '%s' controller with name '%s' because no controller with this name exists", action.c_str(), controller.c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! ('STRICT' switch)"); + return controller_interface::return_type::ERROR; + } } else { @@ -510,7 +601,7 @@ controller_interface::return_type ControllerManager::switch_controller( return controller_interface::return_type::OK; }; - // list all controllers to stop + // list all controllers to stop (check if all controllers exist) auto ret = list_controllers(stop_controllers, stop_request_, "stop"); if (ret != controller_interface::return_type::OK) { @@ -518,7 +609,7 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } - // list all controllers to start + // list all controllers to start (check if all controllers exist) ret = list_controllers(start_controllers, start_request_, "start"); if (ret != controller_interface::return_type::OK) { @@ -527,42 +618,146 @@ controller_interface::return_type ControllerManager::switch_controller( return ret; } - const auto list_interfaces = - [this](const ControllerSpec controller, std::vector & request_interface_list) + // lock controllers + std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + + const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + + // if a preceding controller is deactivated, all first-level controllers should be switched 'from' + // chained mode + propagate_deactivation_of_chained_mode(controllers); + + // check if controllers should be switched 'to' chained mode when controllers are activated + for (auto ctrl_it = start_request_.begin(); ctrl_it != start_request_.end(); ++ctrl_it) { - auto command_interface_config = controller.c->command_interface_configuration(); - std::vector command_interface_names = {}; - if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type ret = controller_interface::return_type::OK; + + // if controller is not inactive then do not do any following-controllers checks + if (!is_controller_inactive(controller_it->c)) { - command_interface_names = resource_manager_->available_command_interfaces(); + RCLCPP_WARN( + get_logger(), + "Controller with name '%s' is not inactive so its following" + "controllers do not have to be checked, because it cannot be activated.", + controller_it->info.name.c_str()); + ret = controller_interface::return_type::ERROR; } - if ( - command_interface_config.type == - controller_interface::interface_configuration_type::INDIVIDUAL) + else { - command_interface_names = command_interface_config.names; + ret = check_following_controllers_for_activate(controllers, strictness, controller_it); } - request_interface_list.insert( - request_interface_list.end(), command_interface_names.begin(), command_interface_names.end()); - }; - // lock controllers - std::lock_guard guard(rt_controllers_wrapper_.controllers_lock_); + if (ret != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not activate controller with name '%s'. (Check above warnings for more details.) " + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // TODO(destogl): automatic manipulation of the chain: + // || strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN); + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + start_request_.erase(ctrl_it); + --ctrl_it; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return controller_interface::return_type::ERROR; + } + } + } - const std::vector & controllers = rt_controllers_wrapper_.get_updated_list(guard); + // check if controllers should be deactivated if used in chained mode + for (auto ctrl_it = stop_request_.begin(); ctrl_it != stop_request_.end(); ++ctrl_it) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, *ctrl_it)); + controller_interface::return_type ret = controller_interface::return_type::OK; + + // if controller is not active then skip preceding-controllers checks + if (!is_controller_active(controller_it->c)) + { + RCLCPP_WARN( + get_logger(), "Controller with name '%s' can not be deactivated since is not active.", + controller_it->info.name.c_str()); + ret = controller_interface::return_type::ERROR; + } + else + { + ret = check_preceeding_controllers_for_deactivate(controllers, strictness, controller_it); + } + + if (ret != controller_interface::return_type::OK) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s'. (Check above warnings for more details.)" + "Check the state of the controllers and their required interfaces using " + "`ros2 control list_controllers -v` to get more information.", + (*ctrl_it).c_str()); + if (strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT) + { + // remove controller that can not be activated from the activation request and step-back + // iterator to correctly step to the next element in the list in the loop + stop_request_.erase(ctrl_it); + --ctrl_it; + } + if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) + { + RCLCPP_ERROR(get_logger(), "Aborting, no controller is switched! (::STRICT switch)"); + // reset all lists + clear_requests(); + return controller_interface::return_type::ERROR; + } + } + } for (const auto & controller : controllers) { + auto to_chained_mode_list_it = std::find( + to_chained_mode_request_.begin(), to_chained_mode_request_.end(), controller.info.name); + bool in_to_chained_mode_list = to_chained_mode_list_it != to_chained_mode_request_.end(); + + auto from_chained_mode_list_it = std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), controller.info.name); + bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end(); + 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(); + const bool is_active = is_controller_active(*controller.c); + const bool is_inactive = is_controller_inactive(*controller.c); + + // restart controllers that need to switch their 'chained mode' - add to stop/start lists + if (in_to_chained_mode_list || in_from_chained_mode_list) + { + if (is_active && !in_stop_list) + { + stop_request_.push_back(controller.info.name); + start_request_.push_back(controller.info.name); + } + } + + // get pointers to places in stop and start lists (start/stop lists have changed) + stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); + 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_active = is_controller_active(*controller.c); - const bool is_inactive = is_controller_inactive(*controller.c); - auto handle_conflict = [&](const std::string & msg) { if (strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT) @@ -572,6 +767,8 @@ controller_interface::return_type ControllerManager::switch_controller( stop_command_interface_request_.clear(); start_request_.clear(); start_command_interface_request_.clear(); + to_chained_mode_request_.clear(); + from_chained_mode_request_.clear(); return controller_interface::return_type::ERROR; } RCLCPP_WARN(get_logger(), "%s", msg.c_str()); @@ -618,21 +815,40 @@ controller_interface::return_type ControllerManager::switch_controller( start_request_.erase(start_list_it); } + const auto extract_interfaces_for_controller = + [this](const ControllerSpec controller, std::vector & request_interface_list) + { + auto command_interface_config = controller.c->command_interface_configuration(); + std::vector command_interface_names = {}; + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + command_interface_names = resource_manager_->available_command_interfaces(); + } + if ( + command_interface_config.type == + controller_interface::interface_configuration_type::INDIVIDUAL) + { + command_interface_names = command_interface_config.names; + } + request_interface_list.insert( + request_interface_list.end(), command_interface_names.begin(), + command_interface_names.end()); + }; + if (in_start_list) { - list_interfaces(controller, start_command_interface_request_); + extract_interfaces_for_controller(controller, start_command_interface_request_); } if (in_stop_list) { - list_interfaces(controller, stop_command_interface_request_); + extract_interfaces_for_controller(controller, stop_command_interface_request_); } } if (start_request_.empty() && stop_request_.empty()) { RCLCPP_INFO(get_logger(), "Empty start and stop list, not requesting switch"); - start_command_interface_request_.clear(); - stop_command_interface_request_.clear(); + clear_requests(); return controller_interface::return_type::OK; } @@ -644,10 +860,7 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_ERROR( get_logger(), "Could not switch controllers since prepare command mode switch was rejected."); - start_request_.clear(); - stop_request_.clear(); - start_command_interface_request_.clear(); - stop_command_interface_request_.clear(); + clear_requests(); return controller_interface::return_type::ERROR; } } @@ -659,7 +872,7 @@ controller_interface::return_type ControllerManager::switch_controller( switch_params_.do_switch = true; // wait until switch is finished - RCLCPP_DEBUG(get_logger(), "Request atomic controller switch from realtime loop"); + RCLCPP_DEBUG(get_logger(), "Requested atomic controller switch from realtime loop"); while (rclcpp::ok() && switch_params_.do_switch) { if (!rclcpp::ok()) @@ -700,11 +913,8 @@ controller_interface::return_type ControllerManager::switch_controller( // clear unused list rt_controllers_wrapper_.get_unused_list(guard).clear(); - start_request_.clear(); - stop_request_.clear(); + clear_requests(); - start_command_interface_request_.clear(); - stop_command_interface_request_.clear(); RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); return controller_interface::return_type::OK; } @@ -780,6 +990,9 @@ void ControllerManager::manage_switch() stop_controllers(); + switch_chained_mode(to_chained_mode_request_, true); + switch_chained_mode(from_chained_mode_request_, false); + // start controllers once the switch is fully complete if (!switch_params_.start_asap) { @@ -790,6 +1003,8 @@ void ControllerManager::manage_switch() // start controllers as soon as their required joints are done switching start_controllers_asap(); } + + // TODO(destogl): move here "do_switch = false" } void ControllerManager::stop_controllers() @@ -825,6 +1040,63 @@ void ControllerManager::stop_controllers() } } +void ControllerManager::switch_chained_mode( + const std::vector & chained_mode_switch_list, bool to_chained_mode) +{ + std::vector & rt_controller_list = + rt_controllers_wrapper_.update_and_get_used_by_rt_list(); + + for (const auto & request : chained_mode_switch_list) + { + 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_FATAL( + get_logger(), + "Got request to turn %s chained mode for controller '%s', but controller is not in the " + "realtime controller list. (This should never happen!)", + (to_chained_mode ? "ON" : "OFF"), request.c_str()); + continue; + } + auto controller = found_it->c; + if (!is_controller_active(*controller)) + { + if (controller->set_chained_mode(to_chained_mode)) + { + if (to_chained_mode) + { + resource_manager_->make_controller_reference_interfaces_available(request); + } + else + { + resource_manager_->make_controller_reference_interfaces_unavailable(request); + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Got request to turn %s chained mode for controller '%s', but controller refused to do " + "it! The control will probably not work as expected. Try to restart all controllers. " + "If " + "the error persist check controllers' individual configuration.", + (to_chained_mode ? "ON" : "OFF"), request.c_str()); + } + } + else + { + RCLCPP_FATAL( + get_logger(), + "Got request to turn %s chained mode for controller '%s', but this can not happen if " + "controller is in '%s' state. (This should never happen!)", + (to_chained_mode ? "ON" : "OFF"), request.c_str(), + hardware_interface::lifecycle_state_names::ACTIVE); + } + } +} + void ControllerManager::start_controllers() { std::vector & rt_controller_list = @@ -1504,4 +1776,250 @@ void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +void ControllerManager::propagate_deactivation_of_chained_mode( + const std::vector & controllers) +{ + for (const auto & controller : controllers) + { + // get pointers to places in stop and start lists (start/stop lists have changed) + auto stop_list_it = std::find(stop_request_.begin(), stop_request_.end(), controller.info.name); + + if (stop_list_it != stop_request_.end()) + { + // if controller is not active then skip adding following-controllers to "from" chained mode + // request + if (!is_controller_active(controller.c)) + { + RCLCPP_DEBUG( + get_logger(), + "Controller with name '%s' can not be deactivated since is not active. " + "The controller will be removed from the list later." + "Skipping adding following controllers to 'from' chained mode request.", + controller.info.name.c_str()); + break; + } + + for (const auto & cmd_itf_name : controller.c->command_interface_configuration().names) + { + // controller that 'cmd_tf_name' belongs to + ControllersListIterator following_ctrl_it; + if (command_interface_is_reference_interface_of_controller( + cmd_itf_name, controllers, following_ctrl_it)) + { + // currently iterated "controller" is preceding controller --> add following controller + // with matching interface name to "from" chained mode list (if not already in it) + if ( + std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), + following_ctrl_it->info.name) == from_chained_mode_request_.end()) + { + from_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'from chained mode' request.", + following_ctrl_it->info.name.c_str()); + } + } + } + } + } +} + +controller_interface::return_type ControllerManager::check_following_controllers_for_activate( + const std::vector & controllers, int strictness, + const ControllersListIterator controller_it) +{ + // we assume that the controller exists is checked in advance + RCLCPP_DEBUG( + get_logger(), "Checking following controllers of preceding controller with name '%s'.", + controller_it->info.name.c_str()); + + for (const auto & cmd_itf_name : controller_it->c->command_interface_configuration().names) + { + ControllersListIterator following_ctrl_it; + // Check if interface if reference interface and following controller exist. + if (!command_interface_is_reference_interface_of_controller( + cmd_itf_name, controllers, following_ctrl_it)) + { + continue; + } + // TODO(destogl): performance of this code could be optimized by adding additional lists with + // controllers that cache if the check has failed and has succeeded. Then the following would be + // done only once per controller, otherwise in complex scenarios the same controller is checked + // multiple times + + // check that all following controllers exits, are either: activated, will be activated, or + // will not be deactivated + RCLCPP_DEBUG( + get_logger(), "Checking following controller with name '%s'.", + following_ctrl_it->info.name.c_str()); + + // check if following controller is chainable + if (!following_ctrl_it->c->is_chainable()) + { + RCLCPP_WARN( + get_logger(), + "No reference interface '%s' exist, since the following controller with name '%s' " + "is not chainable.", + cmd_itf_name.c_str(), following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + + if (is_controller_active(following_ctrl_it->c)) + { + // will following controller be deactivated? + if ( + std::find(stop_request_.begin(), stop_request_.end(), following_ctrl_it->info.name) != + stop_request_.end()) + { + RCLCPP_WARN( + get_logger(), "The following controller with name '%s' will be deactivated.", + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + // check if following controller will not be activated + else if ( + std::find(start_request_.begin(), start_request_.end(), following_ctrl_it->info.name) == + start_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "The following controller with name '%s' is not active and will not be activated.", + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + + // Trigger recursion to check all the following controllers only if they are OK, add this + // controller update chained mode requests + if ( + check_following_controllers_for_activate(controllers, strictness, following_ctrl_it) == + controller_interface::return_type::ERROR) + { + return controller_interface::return_type::ERROR; + } + + // TODO(destogl): this should be discussed how to it the best - just a placeholder for now + // else if (strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) + // { + // // insert to the begin of start request list to be started before preceding controller + // start_request_.insert(start_request_.begin(), following_ctrl_name); + // } + if (!following_ctrl_it->c->is_in_chained_mode()) + { + auto found_it = std::find( + to_chained_mode_request_.begin(), to_chained_mode_request_.end(), + following_ctrl_it->info.name); + if (found_it == to_chained_mode_request_.end()) + { + to_chained_mode_request_.push_back(following_ctrl_it->info.name); + RCLCPP_DEBUG( + get_logger(), "Adding controller '%s' in 'to chained mode' request.", + following_ctrl_it->info.name.c_str()); + } + } + else + { + // Check if following controller is in 'from' chained mode list and remove it, if so + auto found_it = std::find( + from_chained_mode_request_.begin(), from_chained_mode_request_.end(), + following_ctrl_it->info.name); + if (found_it != from_chained_mode_request_.end()) + { + from_chained_mode_request_.erase(found_it); + RCLCPP_DEBUG( + get_logger(), + "Removing controller '%s' in 'from chained mode' request because it " + "should stay in chained mode.", + following_ctrl_it->info.name.c_str()); + } + } + } + return controller_interface::return_type::OK; +}; + +controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate( + const std::vector & controllers, int /*strictness*/, + const ControllersListIterator controller_it) +{ + // if not chainable no need for any checks + if (!controller_it->c->is_chainable()) + { + return controller_interface::return_type::OK; + } + + if (!controller_it->c->is_in_chained_mode()) + { + RCLCPP_DEBUG( + get_logger(), + "Controller with name '%s' is chainable but not in chained mode. " + "No need to do any checks of preceding controllers when stopping it.", + controller_it->info.name.c_str()); + return controller_interface::return_type::OK; + } + + RCLCPP_DEBUG( + get_logger(), "Checking preceding controller of following controller with name '%s'.", + controller_it->info.name.c_str()); + + for (const auto & ref_itf_name : + resource_manager_->get_controller_reference_interface_names(controller_it->info.name)) + { + std::vector preceding_controllers_using_ref_itf; + + // TODO(destogl): This data could be cached after configuring controller into a map for faster + // access here + for (auto preceding_ctrl_it = controllers.begin(); preceding_ctrl_it != controllers.end(); + ++preceding_ctrl_it) + { + const auto preceding_ctrl_cmd_itfs = + preceding_ctrl_it->c->command_interface_configuration().names; + + // if controller is not preceding go the next one + if ( + std::find(preceding_ctrl_cmd_itfs.begin(), preceding_ctrl_cmd_itfs.end(), ref_itf_name) == + preceding_ctrl_cmd_itfs.end()) + { + continue; + } + + // check if preceding controller will be activated + if ( + is_controller_inactive(preceding_ctrl_it->c) && + std::find(start_request_.begin(), start_request_.end(), preceding_ctrl_it->info.name) != + start_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' will be activated. ", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + // check if preceding controller will not be deactivated + else if ( + is_controller_active(preceding_ctrl_it->c) && + std::find(stop_request_.begin(), stop_request_.end(), preceding_ctrl_it->info.name) == + stop_request_.end()) + { + RCLCPP_WARN( + get_logger(), + "Could not deactivate controller with name '%s' because " + "preceding controller with name '%s' is active and will not be deactivated.", + controller_it->info.name.c_str(), preceding_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + // TODO(destogl): this should be discussed how to it the best - just a placeholder for now + // else if ( + // strictness == + // controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN) + // { + // // insert to the begin of start request list to be started before preceding controller + // start_request_.insert(start_request_.begin(), preceding_ctrl_name); + // } + } + } + return controller_interface::return_type::OK; +}; + } // namespace controller_manager diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp new file mode 100644 index 0000000000..ba1132e68b --- /dev/null +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp @@ -0,0 +1,192 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "test_chainable_controller.hpp" + +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" + +namespace test_chainable_controller +{ +TestChainableController::TestChainableController() +: controller_interface::ChainableControllerInterface(), + cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE}, + state_iface_cfg_{controller_interface::interface_configuration_type::NONE} +{ +} + +controller_interface::InterfaceConfiguration +TestChainableController::command_interface_configuration() const +{ + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return cmd_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get command interface configuration until the controller is configured."); + } +} + +controller_interface::InterfaceConfiguration +TestChainableController::state_interface_configuration() const +{ + if ( + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + return state_iface_cfg_; + } + else + { + throw std::runtime_error( + "Can not get state interface configuration until the controller is configured."); + } +} + +controller_interface::return_type TestChainableController::update_reference_from_subscribers() +{ + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), + "Value of reference interface '%s' before checking external input is %f", + (std::string(get_node()->get_name()) + "/" + reference_interface_names_[i]).c_str(), + reference_interfaces_[i]); + } + + auto joint_commands = rt_command_ptr_.readFromRT(); + reference_interfaces_ = (*joint_commands)->data; + for (size_t i = 0; i < reference_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), + "Updated value of reference interface '%s' after applying external input is %f", + (std::string(get_node()->get_name()) + "/" + reference_interface_names_[i]).c_str(), + reference_interfaces_[i]); + } + + return controller_interface::return_type::OK; +} + +controller_interface::return_type TestChainableController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + ++internal_counter; + + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value()); + } + + return controller_interface::return_type::OK; +} + +CallbackReturn TestChainableController::on_init() { return CallbackReturn::SUCCESS; } + +CallbackReturn TestChainableController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joints_command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const CmdType::SharedPtr msg) + { + auto joint_commands = rt_command_ptr_.readFromNonRT(); + + if (msg->data.size() != (*joint_commands)->data.size()) + { + rt_command_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "command size (%zu) does not match number of reference interfaces (%zu)", + (*joint_commands)->data.size(), reference_interfaces_.size()); + } + }); + + auto msg = std::make_shared(); + msg->data.resize(reference_interfaces_.size()); + rt_command_ptr_.writeFromNonRT(msg); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn TestChainableController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!is_in_chained_mode()) + { + auto msg = rt_command_ptr_.readFromRT(); + (*msg)->data = reference_interfaces_; + } + + return CallbackReturn::SUCCESS; +} + +CallbackReturn TestChainableController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joints_command_subscriber_.reset(); + return CallbackReturn::SUCCESS; +} + +std::vector +TestChainableController::on_export_reference_interfaces() +{ + std::vector reference_interfaces; + + for (size_t i = 0; i < reference_interface_names_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +void TestChainableController::set_command_interface_configuration( + const controller_interface::InterfaceConfiguration & cfg) +{ + cmd_iface_cfg_ = cfg; +} + +void TestChainableController::set_state_interface_configuration( + const controller_interface::InterfaceConfiguration & cfg) +{ + state_iface_cfg_ = cfg; +} + +void TestChainableController::set_reference_interface_names( + const std::vector & reference_interface_names) +{ + reference_interface_names_ = reference_interface_names; + + reference_interfaces_.resize(reference_interface_names.size(), 0.0); +} + +} // namespace test_chainable_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + test_chainable_controller::TestChainableController, + controller_interface::ChainableControllerInterface) diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp new file mode 100644 index 0000000000..a7205d3024 --- /dev/null +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_ +#define TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "controller_manager/visibility_control.h" +#include "rclcpp/subscription.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "std_msgs/msg/float64_multi_array.hpp" + +namespace test_chainable_controller +{ +using CmdType = std_msgs::msg::Float64MultiArray; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +// indicating the node name under which the controller node +// is being loaded. +constexpr char TEST_CONTROLLER_NAME[] = "test_chainable_controller_name"; +// corresponds to the name listed within the pluginlib xml +constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_chainable_controller"; +class TestChainableController : public controller_interface::ChainableControllerInterface +{ +public: + CONTROLLER_MANAGER_PUBLIC + TestChainableController(); + + CONTROLLER_MANAGER_PUBLIC + virtual ~TestChainableController() = default; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_init() override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers() override; + + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // Testing-relevant methods + CONTROLLER_MANAGER_PUBLIC + void set_command_interface_configuration( + const controller_interface::InterfaceConfiguration & cfg); + + CONTROLLER_MANAGER_PUBLIC + void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg); + + CONTROLLER_MANAGER_PUBLIC + void set_reference_interface_names(const std::vector & reference_interface_names); + + size_t internal_counter; + controller_interface::InterfaceConfiguration cmd_iface_cfg_; + controller_interface::InterfaceConfiguration state_iface_cfg_; + std::vector reference_interface_names_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; +}; + +} // namespace test_chainable_controller + +#endif // TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_ diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.xml b/controller_manager/test/test_chainable_controller/test_chainable_controller.xml new file mode 100644 index 0000000000..30ae99a946 --- /dev/null +++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.xml @@ -0,0 +1,11 @@ + + + + + Controller used for testing + + + + diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 2b8b8bc262..51e83ac452 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -61,6 +61,15 @@ controller_interface::return_type TestController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { ++internal_counter; + + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + RCLCPP_INFO( + get_node()->get_logger(), "Setting value of command interface '%s' to %f", + command_interfaces_[i].get_full_name().c_str(), external_commands_for_testing_[i]); + command_interfaces_[i].set_value(external_commands_for_testing_[i]); + } + return controller_interface::return_type::OK; } @@ -89,6 +98,7 @@ void TestController::set_command_interface_configuration( const controller_interface::InterfaceConfiguration & cfg) { cmd_iface_cfg_ = cfg; + external_commands_for_testing_.resize(cmd_iface_cfg_.names.size(), 0.0); } void TestController::set_state_interface_configuration( diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 4e4ec44d17..403d7d1211 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" @@ -71,6 +72,8 @@ class TestController : public controller_interface::ControllerInterface size_t * cleanup_calls = nullptr; controller_interface::InterfaceConfiguration cmd_iface_cfg_; controller_interface::InterfaceConfiguration state_iface_cfg_; + + std::vector external_commands_for_testing_; }; } // namespace test_controller diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index baf1f3ba0f..b927734640 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -50,6 +50,9 @@ TEST_F(TestControllerManagerSrvs, list_controller_types) ASSERT_THAT(result->types, ::testing::Contains(test_controller::TEST_CONTROLLER_CLASS_NAME)); ASSERT_THAT( result->base_classes, ::testing::Contains("controller_interface::ControllerInterface")); + ASSERT_THAT( + result->base_classes, + ::testing::Contains("controller_interface::ChainableControllerInterface")); } TEST_F(TestControllerManagerSrvs, list_controllers_srv) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp new file mode 100644 index 0000000000..0d798a1b79 --- /dev/null +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -0,0 +1,621 @@ +// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_interface/controller_interface.hpp" +#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/parameter.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" +#include "test_controller/test_controller.hpp" + +// The tests in this file are implementing example of chained-control for DiffDrive robot example: +// https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use +// The controller have the names as stated in figure, but they are simply forwarding values without +// functionality that their name would suggest + +class TestControllerChainingWithControllerManager; + +class TestableTestChainableController : public test_chainable_controller::TestChainableController +{ + friend TestControllerChainingWithControllerManager; + + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_auto_switch_to_chained_mode); +}; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerChainingWithControllerManager; + + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter); + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter_failure_group0); + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter_failure_wrong_type); + FRIEND_TEST( + TestControllerChainingWithControllerManagerAndChainedControllersParameter, + test_cm_reading_chained_controllers_parameter_failure_duplicated_controller); + + FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers); + FRIEND_TEST( + TestControllerChainingWithControllerManager, + test_chained_controllers_auto_switch_to_chained_mode); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & namespace_ = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, namespace_) + { + } +}; + +class TestControllerChainingWithControllerManager +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +public: + void SetUp() + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique( + ros2_control_test_assets::diffbot_urdf, true, true), + executor_, TEST_CM_NAME); + run_updater_ = false; + } + + void SetupControllers() + { + test_param = GetParam(); + + pid_left_wheel_controller = std::make_shared(); + pid_right_wheel_controller = std::make_shared(); + diff_drive_controller = std::make_shared(); + position_tracking_controller = std::make_shared(); + + // configure Left Wheel controller + controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; + controller_interface::InterfaceConfiguration pid_left_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}}; + pid_left_wheel_controller->set_command_interface_configuration(pid_left_cmd_ifs_cfg); + pid_left_wheel_controller->set_state_interface_configuration(pid_left_state_ifs_cfg); + pid_left_wheel_controller->set_reference_interface_names({"velocity"}); + + // configure Left Wheel controller + controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; + controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}}; + pid_right_wheel_controller->set_command_interface_configuration(pid_right_cmd_ifs_cfg); + pid_right_wheel_controller->set_state_interface_configuration(pid_right_state_ifs_cfg); + pid_right_wheel_controller->set_reference_interface_names({"velocity"}); + + // configure Diff Drive controller + controller_interface::InterfaceConfiguration diff_drive_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(PID_LEFT_WHEEL) + "/velocity", std::string(PID_RIGHT_WHEEL) + "/velocity"}}; + controller_interface::InterfaceConfiguration diff_drive_state_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"wheel_left/velocity", "wheel_right/velocity"}}; + diff_drive_controller->set_command_interface_configuration(diff_drive_cmd_ifs_cfg); + diff_drive_controller->set_state_interface_configuration(diff_drive_state_ifs_cfg); + diff_drive_controller->set_reference_interface_names({"vel_x", "vel_y", "rot_z"}); + + // configure Position Tracking controller + controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {std::string(DIFF_DRIVE_CONTROLLER) + "/vel_x", + std::string(DIFF_DRIVE_CONTROLLER) + "/vel_y"}}; + // in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel" + position_tracking_controller->set_command_interface_configuration( + position_tracking_cmd_ifs_cfg); + } + + void CheckIfControllersAreAddedCorrectly() + { + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, pid_left_wheel_controller.use_count()); + EXPECT_EQ(2, pid_right_wheel_controller.use_count()); + EXPECT_EQ(2, diff_drive_controller.use_count()); + EXPECT_EQ(2, position_tracking_controller.use_count()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + pid_left_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + pid_right_wheel_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + diff_drive_controller->get_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + position_tracking_controller->get_state().id()); + } + + // order or controller configuration is not important therefore we can reuse the same method + void ConfigureAndCheckControllers() + { + // Store initial values of command interfaces + size_t number_of_cmd_itfs = cm_->resource_manager_->command_interface_keys().size(); + + // configure chainable controller and check exported interfaces + cm_->configure_controller(PID_LEFT_WHEEL); + EXPECT_EQ( + pid_left_wheel_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 1); + for (const auto & interface : {"pid_left_wheel_controller/velocity"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + + cm_->configure_controller(PID_RIGHT_WHEEL); + EXPECT_EQ( + pid_right_wheel_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 2); + for (const auto & interface : {"pid_right_wheel_controller/velocity"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + + cm_->configure_controller(DIFF_DRIVE_CONTROLLER); + EXPECT_EQ( + diff_drive_controller->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); + for (const auto & interface : + {"diff_drive_controller/vel_x", "diff_drive_controller/vel_y", + "diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + + cm_->configure_controller(POSITION_TRACKING_CONTROLLER); + EXPECT_EQ( + position_tracking_controller->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ(cm_->resource_manager_->command_interface_keys().size(), number_of_cmd_itfs + 5); + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void SetToChainedModeAndMakeReferenceInterfacesAvailable( + std::shared_ptr & controller, const std::string & controller_name, + const std::vector & reference_interfaces) + { + controller->set_chained_mode(true); + EXPECT_TRUE(controller->is_in_chained_mode()); + // make reference interface command_interfaces available + cm_->resource_manager_->make_controller_reference_interfaces_available(controller_name); + for (const auto & interface : reference_interfaces) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void check_after_de_activate( + std::shared_ptr & controller, const std::vector & claimed_command_itfs, + size_t expected_internal_counter, const controller_interface::return_type expected_return, + bool deactivated, bool claimed_interfaces_from_hw = false) + { + for (const auto & interface : claimed_command_itfs) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + // successful xor deactivated + if ((expected_return == controller_interface::return_type::OK) != deactivated) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + else + { + if (claimed_interfaces_from_hw) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + } + else + { + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_available(interface)); + } + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + } + if (expected_internal_counter != 0) + { + ASSERT_EQ(controller->internal_counter, expected_internal_counter); + } + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void ActivateAndCheckController( + std::shared_ptr & controller, const std::string & controller_name, + const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, + const controller_interface::return_type expected_return = controller_interface::return_type::OK) + { + switch_test_controllers( + {controller_name}, {}, test_param.strictness, std::future_status::timeout, expected_return); + check_after_de_activate( + controller, claimed_command_itfs, expected_internal_counter, expected_return, false); + } + + template < + typename T, typename std::enable_if< + std::is_convertible::value, + T>::type * = nullptr> + void DeactivateAndCheckController( + std::shared_ptr & controller, const std::string & controller_name, + const std::vector & claimed_command_itfs, size_t expected_internal_counter = 0u, + const bool claimed_interfaces_from_hw = false, + const controller_interface::return_type expected_return = controller_interface::return_type::OK) + { + switch_test_controllers( + {}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return); + check_after_de_activate( + controller, claimed_command_itfs, expected_internal_counter, expected_return, true, + claimed_interfaces_from_hw); + } + + void UpdateAllControllerAndCheck( + const std::vector & reference, size_t exp_internal_counter_pos_ctrl) + { + // test value that could cause bad-memory access --> cleaner error during writing tests + ASSERT_EQ(reference.size(), 2u); + + position_tracking_controller->external_commands_for_testing_[0] = reference[0]; + position_tracking_controller->external_commands_for_testing_[1] = reference[1]; + + cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + + // check if all controllers are updated + ASSERT_EQ(position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl); + ASSERT_EQ(diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 2); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 6); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 4); + + // check if values are set properly in controllers + ASSERT_EQ( + diff_drive_controller->reference_interfaces_[0], reference[0]); // command from Position to + ASSERT_EQ( + diff_drive_controller->reference_interfaces_[1], reference[1]); // DiffDrive is forwarded + + // Command of DiffDrive controller are references of PID controllers + EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); + EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); + ASSERT_EQ(diff_drive_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_REF); + ASSERT_EQ(diff_drive_controller->command_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_REF); + ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); + ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); + EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + + EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); + EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ( + pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + } + + // check data propagation through controllers and if individual controllers are working + double chained_ctrl_calculation(double reference, double state) { return reference - state; } + double hardware_calculation(double command) { return command / 2.0; } + + // set controllers' names + static constexpr char PID_LEFT_WHEEL[] = "pid_left_wheel_controller"; + static constexpr char PID_RIGHT_WHEEL[] = "pid_right_wheel_controller"; + static constexpr char DIFF_DRIVE_CONTROLLER[] = "diff_drive_controller"; + static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller"; + + const std::vector PID_LEFT_WHEEL_REFERENCE_INTERFACES = { + "pid_left_wheel_controller/velocity"}; + const std::vector PID_RIGHT_WHEEL_REFERENCE_INTERFACES = { + "pid_right_wheel_controller/velocity"}; + const std::vector DIFF_DRIVE_REFERENCE_INTERFACES = { + "diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"}; + + const std::vector PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"}; + const std::vector PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"}; + const std::vector DIFF_DRIVE_CLAIMED_INTERFACES = { + "pid_left_wheel_controller/velocity", "pid_right_wheel_controller/velocity"}; + const std::vector POSITION_CONTROLLER_CLAIMED_INTERFACES = { + "diff_drive_controller/vel_x", "diff_drive_controller/vel_y"}; + + // controllers objects + std::shared_ptr pid_left_wheel_controller; + std::shared_ptr pid_right_wheel_controller; + std::shared_ptr diff_drive_controller; + std::shared_ptr position_tracking_controller; + + testing::WithParamInterface::ParamType test_param; + + // expected values for tests - shared between multiple test runs + double EXP_LEFT_WHEEL_CMD = 0.0; + double EXP_LEFT_WHEEL_HW_STATE = 0.0; + double EXP_RIGHT_WHEEL_CMD = 0.0; + double EXP_RIGHT_WHEEL_HW_STATE = 0.0; + double EXP_LEFT_WHEEL_REF = 0.0; + double EXP_RIGHT_WHEEL_REF = 0.0; +}; + +// The tests are implementing example of chained-control for DiffDrive robot shown here: +// https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use +// The controller have the names as stated in figure, but they are simply forwarding values without +// functionality that their name would suggest +TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES); + SetToChainedModeAndMakeReferenceInterfacesAvailable( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES); + + EXPECT_THROW( + cm_->resource_manager_->make_controller_reference_interfaces_available( + POSITION_TRACKING_CONTROLLER), + std::out_of_range); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + // activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER + // (otherwise, interface will be missing) + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u); + + // Diff-Drive Controller claims the reference interfaces of PID controllers + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u); + + // Position-Tracking Controller uses reference interfaces of Diff-Drive Controller + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + // 'rot_z' reference interface is not claimed + for (const auto & interface : {"diff_drive_controller/rot_z"}) + { + EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface)); + EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface)); + EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface)); + } + ASSERT_EQ(diff_drive_controller->internal_counter, 3u); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u); + + // update controllers + std::vector reference = {32.0, 128.0}; + + // update 'Position Tracking' controller + for (auto & value : diff_drive_controller->reference_interfaces_) + { + ASSERT_EQ(value, 0.0); // default reference values are 0.0 + } + position_tracking_controller->external_commands_for_testing_[0] = reference[0]; + position_tracking_controller->external_commands_for_testing_[1] = reference[1]; + position_tracking_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(position_tracking_controller->internal_counter, 2u); + + ASSERT_EQ(diff_drive_controller->reference_interfaces_[0], reference[0]); // position_controller + ASSERT_EQ(diff_drive_controller->reference_interfaces_[1], reference[1]); // is pass-through + + // update 'Diff Drive' Controller + diff_drive_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(diff_drive_controller->internal_counter, 4u); + // default reference values are 0.0 - they should be changed now + EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE); // 32-0 + EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE); // 128-0 + ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF); + ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF); + + // update PID controllers that are writing to hardware + pid_left_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_left_wheel_controller->internal_counter, 8u); + pid_right_wheel_controller->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + ASSERT_EQ(pid_right_wheel_controller->internal_counter, 6u); + + // update hardware ('read' is sufficient for test hardware) + cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // 32 - 0 + EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE); + // 32 / 2 + EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD); + ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE); + + // 128 - 0 + EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE); + // 128 / 2 + EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD); + ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + // DiffDrive uses the same state + ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE); + + // update all controllers at once and see that all have expected values --> also checks the order + // of controller execution + + reference = {1024.0, 4096.0}; + UpdateAllControllerAndCheck(reference, 3u); + + // TODO(destogl): Add here also slow disabling of controllers + + // TODO(destogl): Activate test parameter use +} + +TEST_P( + TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode) +{ + SetupControllers(); + + // add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER + cm_->add_controller( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_left_wheel_controller, PID_LEFT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + pid_right_wheel_controller, PID_RIGHT_WHEEL, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + + CheckIfControllersAreAddedCorrectly(); + + ConfigureAndCheckControllers(); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + rclcpp::get_logger("ControllerManager::utils").set_level(rclcpp::Logger::Level::Debug); + + // at beginning controllers are not in chained mode + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // still not in chained mode because no preceding controller is activated + ActivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u); + ActivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // DiffDrive (preceding) controller is activated --> PID controller in chained mode + ActivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // PositionController is activated --> all following controller in chained mode + ActivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_TRUE(diff_drive_controller->is_in_chained_mode()); + + UpdateAllControllerAndCheck({32.0, 128.0}, 2u); + UpdateAllControllerAndCheck({1024.0, 4096.0}, 3u); + + // Test switch 'from chained mode' when controllers are deactivated + + // PositionController is deactivated --> DiffDrive controller is not in chained mode anymore + DeactivateAndCheckController( + position_tracking_controller, POSITION_TRACKING_CONTROLLER, + POSITION_CONTROLLER_CLAIMED_INTERFACES, 4u); + EXPECT_TRUE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_TRUE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // DiffDrive (preceding) controller is activated --> PID controller in chained mode + DeactivateAndCheckController( + diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 8u); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); + + // all controllers are deactivated --> chained mode is not changed + DeactivateAndCheckController( + pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u, true); + DeactivateAndCheckController( + pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u, true); + EXPECT_FALSE(pid_left_wheel_controller->is_in_chained_mode()); + EXPECT_FALSE(pid_right_wheel_controller->is_in_chained_mode()); + ASSERT_FALSE(diff_drive_controller->is_in_chained_mode()); +} + +// TODO(destogl): Add test case with controllers added in "random" order + +// TODO(destogl): Think about strictness and chaining controllers +// new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain +// + +INSTANTIATE_TEST_SUITE_P( + test_strict_best_effort, TestControllerChainingWithControllerManager, + testing::Values(strict, best_effort)); diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 00a504d69a..d3a92bfce4 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -66,8 +66,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; - // TODO(destogl): separate this to init_tests method where parameter can be set for each test - // separately cm_->set_parameter( rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); cm_->set_parameter(rclcpp::Parameter( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index d77950103c..5a919289b8 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -245,7 +245,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \note given that no hardware_info is available, the component has to be configured * externally and prior to the call to import. * \param[in] actuator pointer to the actuator interface. - * \param[in]hardware_info hardware info + * \param[in] hardware_info hardware info */ void import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info); diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 23a7d811df..355583a61d 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -75,6 +75,11 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { + // The next line is for the testing purposes. We need value to be changed to be sure that + // the feedback from hardware to controllers in the chain is working as it should. + // This makes value checks clearer and confirms there is no "state = command" line or some + // other mixture of interfaces somewhere in the test stack. + velocity_state_ = velocity_command_ / 2; return return_type::OK; }