diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 61d2c780b8..e2c1fa480a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -28,13 +28,14 @@ return_type ControllerInterfaceBase::init( const std::string & node_namespace, const rclcpp::NodeOptions & node_options) { urdf_ = urdf; + update_rate_ = cm_update_rate; node_ = std::make_shared( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces try { - auto_declare("update_rate", cm_update_rate); + auto_declare("update_rate", update_rate_); auto_declare("is_async", false); } catch (const std::exception & e) @@ -85,7 +86,24 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() // Other solution is to add check into the LifecycleNode if a transition is valid to trigger if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { - update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); + const auto update_rate = get_node()->get_parameter("update_rate").as_int(); + if (update_rate < 0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!"); + return get_lifecycle_state(); + } + if (update_rate_ != 0u && update_rate > update_rate_) + { + RCLCPP_WARN( + get_node()->get_logger(), + "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the " + "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.", + update_rate, update_rate_); + } + else + { + update_rate_ = static_cast(update_rate); + } is_async_ = get_node()->get_parameter("is_async").as_bool(); } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index f5c0a6b3de..4204d32f45 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -19,6 +19,7 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -46,8 +47,8 @@ TEST(TestableControllerInterface, init) controller_interface::return_type::OK); ASSERT_NO_THROW(controller.get_node()); - // update_rate is set to default 0 - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 10u); // Even after configure is 10 controller.configure(); @@ -56,6 +57,32 @@ TEST(TestableControllerInterface, init) rclcpp::shutdown(); } +TEST(TestableControllerInterface, setting_negative_update_rate_in_configure) +{ + // mocks the declaration of overrides parameters in a yaml file + rclcpp::init(0, nullptr); + + TestableControllerInterface controller; + // initialize, create node + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=-100"); + node_options = node_options.arguments(node_options_arguments); + ASSERT_EQ( + controller.init(TEST_CONTROLLER_NAME, "", 1000.0, "", node_options), + controller_interface::return_type::OK); + + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 1000u); + + // The configure should fail and the update rate should stay the same + ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(controller.get_update_rate(), 1000u); + rclcpp::shutdown(); +} + TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file @@ -70,7 +97,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) node_options_arguments.push_back("update_rate:=2812"); node_options = node_options.arguments(node_options_arguments); ASSERT_EQ( - controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), + controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options), controller_interface::return_type::OK); // initialize executor to be able to get parameter update @@ -85,8 +112,8 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure) using namespace std::chrono_literals; std::this_thread::sleep_for(50ms); - // update_rate is set to default 0 because it is set on configure - ASSERT_EQ(controller.get_update_rate(), 0u); + // update_rate is set to controller_manager's rate + ASSERT_EQ(controller.get_update_rate(), 5000u); // Even after configure is 0 controller.configure(); diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 5dea15c0d1..e44fb5fb9b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -91,6 +91,7 @@ if(BUILD_TESTING) target_link_libraries(test_controller_manager controller_manager test_controller + test_chainable_controller ros2_control_test_assets::ros2_control_test_assets ) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a1d3c0f5ad..102531f8e8 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -253,24 +253,57 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti ) -def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): +def get_parameter_from_param_file( + node, controller_name, namespace, parameter_file, parameter_name +): with open(parameter_file) as f: namespaced_controller = ( - controller_name if namespace == "/" else f"{namespace}/{controller_name}" + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" ) + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" parameters = yaml.safe_load(f) - if namespaced_controller in parameters: - value = parameters[namespaced_controller] - if not isinstance(value, dict) or "ros__parameters" not in value: + controller_param_dict = None + # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name' + for key in [ + controller_name, + namespaced_controller, + f"{WILDCARD_KEY}/{controller_name}", + f"{WILDCARD_KEY}{namespaced_controller}", + ]: + if key in parameters: + if key == controller_name and namespace != "/": + node.get_logger().fatal( + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + ) + break + controller_param_dict = parameters[key] + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_param_dict = parameters[WILDCARD_KEY][key] + + if controller_param_dict and ( + not isinstance(controller_param_dict, dict) + or ROS_PARAMS_KEY not in controller_param_dict + ): raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}" ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - else: - return None + if ( + controller_param_dict + and ROS_PARAMS_KEY in controller_param_dict + and parameter_name in controller_param_dict[ROS_PARAMS_KEY] + ): + break + + if controller_param_dict is None: + node.get_logger().fatal( + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}" + ) + if parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + + return None def set_controller_parameters( @@ -324,7 +357,7 @@ def set_controller_parameters_from_param_file( ) controller_type = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "type" + node, controller_name, spawner_namespace, parameter_file, "type" ) if controller_type: if not set_controller_parameters( @@ -333,7 +366,7 @@ def set_controller_parameters_from_param_file( return False fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, parameter_file, "fallback_controllers" + node, controller_name, spawner_namespace, parameter_file, "fallback_controllers" ) if fallback_controllers: if not set_controller_parameters( diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 0a3bb0a99a..abd5383ea8 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -87,6 +87,18 @@ update_rate (mandatory; integer) Name of a plugin exported using ``pluginlib`` for a controller. This is a class from which controller's instance with name "``controller_name``" is created. +.params_file + The absolute path to the YAML file with parameters for the controller. + The file should contain the parameters for the controller in the standard ROS 2 YAML format. + +.fallback_controllers + List of controllers that are activated as a fallback strategy, when the spawned controllers fail by returning ``return_type::ERROR`` during the ``update`` cycle. + It is recommended to add all the controllers needed for the fallback strategy to the list, including the chainable controllers whose interfaces are used by the main fallback controllers. + +.. warning:: + The fallback controllers activation is subject to the availability of the state and command interfaces at the time of activation. + It is recommended to test the fallback strategy in simulation before deploying it on the real robot. + Handling Multiple Controller Managers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -171,6 +183,60 @@ There are two scripts to interact with controller manager from launch files: --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether +The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files: + + .. code-block:: yaml + + /**/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + + .. code-block:: yaml + + /rrbot_1/position_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - joint1 + - joint2 + + command_interfaces: + - position + ..... + ``unspawner`` ^^^^^^^^^^^^^^^^ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index b8c05efcc0..273b48b022 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -128,6 +128,12 @@ class ControllerManager : public rclcpp::Node return add_controller_impl(controller_spec); } + controller_interface::ControllerInterfaceBaseSharedPtr add_controller( + const ControllerSpec & controller_spec) + { + return add_controller_impl(controller_spec); + } + /// configure_controller Configure controller by name calling their "configure" method. /** * \param[in] controller_name as a string. @@ -418,6 +424,18 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers, int strictness, const ControllersListIterator controller_it); + /// Checks if the fallback controllers of the given controllers are in the right + /// state, so they can be activated immediately + /** + * \param[in] controllers is a list of controllers to activate. + * \param[in] controller_it is the iterator pointing to the controller to be activated. + * \return return_type::OK if all fallback controllers are in the right state, otherwise + * return_type::ERROR. + */ + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it); + /** * @brief Inserts a controller into an ordered list based on dependencies to compute the * controller chain. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b8a587f92e..72cebaa1e5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -170,6 +170,41 @@ void controller_chain_spec_cleanup( } ctrl_chain_spec.erase(controller); } + +// Gets the list of active controllers that use the command interface of the given controller +void get_active_controllers_using_command_interfaces_of_controller( + const std::string & controller_name, + const std::vector & controllers, + std::vector & controllers_using_command_interfaces) +{ + auto it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (it == controllers.end()) + { + RCLCPP_ERROR( + rclcpp::get_logger("ControllerManager::utils"), + "Controller '%s' not found in the list of controllers.", controller_name.c_str()); + return; + } + const auto cmd_itfs = it->c->command_interface_configuration().names; + for (const auto & cmd_itf : cmd_itfs) + { + for (const auto & controller : controllers) + { + const auto ctrl_cmd_itfs = controller.c->command_interface_configuration().names; + // check if the controller is active and has the command interface and make sure that it + // doesn't exist in the list already + if ( + is_controller_active(controller.c) && + std::find(ctrl_cmd_itfs.begin(), ctrl_cmd_itfs.end(), cmd_itf) != ctrl_cmd_itfs.end()) + { + add_element_to_list(controllers_using_command_interfaces, controller.info.name); + } + } + } +} + } // namespace namespace controller_manager @@ -536,6 +571,15 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c } if (get_parameter(fallback_ctrl_param, fallback_controllers) && !fallback_controllers.empty()) { + if ( + std::find(fallback_controllers.begin(), fallback_controllers.end(), controller_name) != + fallback_controllers.end()) + { + RCLCPP_ERROR( + get_logger(), "Controller '%s' cannot be a fallback controller for itself.", + controller_name.c_str()); + return nullptr; + } controller_spec.info.fallback_controllers_names = fallback_controllers; } @@ -1082,6 +1126,11 @@ controller_interface::return_type ControllerManager::switch_controller( status = check_following_controllers_for_activate(controllers, strictness, controller_it); } + if (status == controller_interface::return_type::OK) + { + status = check_fallback_controllers_state_pre_activation(controllers, controller_it); + } + if (status != controller_interface::return_type::OK) { RCLCPP_WARN( @@ -2360,16 +2409,68 @@ controller_interface::return_type ControllerManager::update( } if (!failed_controllers_list.empty()) { - std::string failed_controllers; + const auto FALLBACK_STACK_MAX_SIZE = 500; + std::vector active_controllers_using_interfaces(failed_controllers_list); + active_controllers_using_interfaces.reserve(FALLBACK_STACK_MAX_SIZE); + std::vector cumulative_fallback_controllers; + cumulative_fallback_controllers.reserve(FALLBACK_STACK_MAX_SIZE); + + for (const auto & failed_ctrl : failed_controllers_list) + { + auto ctrl_it = std::find_if( + rt_controller_list.begin(), rt_controller_list.end(), + std::bind(controller_name_compare, std::placeholders::_1, failed_ctrl)); + if (ctrl_it != rt_controller_list.end()) + { + for (const auto & fallback_controller : ctrl_it->info.fallback_controllers_names) + { + cumulative_fallback_controllers.push_back(fallback_controller); + get_active_controllers_using_command_interfaces_of_controller( + fallback_controller, rt_controller_list, active_controllers_using_interfaces); + } + } + } + std::string controllers_string; + controllers_string.reserve(500); for (const auto & controller : failed_controllers_list) { - failed_controllers += "\n\t- " + controller; + controllers_string.append(controller); + controllers_string.append(" "); } RCLCPP_ERROR( - get_logger(), "Deactivating following controllers as their update resulted in an error :%s", - failed_controllers.c_str()); - - deactivate_controllers(rt_controller_list, failed_controllers_list); + get_logger(), "Deactivating controllers : [ %s] as their update resulted in an error!", + controllers_string.c_str()); + if (active_controllers_using_interfaces.size() > failed_controllers_list.size()) + { + controllers_string.clear(); + for (size_t i = failed_controllers_list.size(); + i < active_controllers_using_interfaces.size(); i++) + { + controllers_string.append(active_controllers_using_interfaces[i]); + controllers_string.append(" "); + } + RCLCPP_ERROR_EXPRESSION( + get_logger(), !controllers_string.empty(), + "Deactivating controllers : [ %s] using the command interfaces needed for the fallback " + "controllers to activate.", + controllers_string.c_str()); + } + if (!cumulative_fallback_controllers.empty()) + { + controllers_string.clear(); + for (const auto & controller : cumulative_fallback_controllers) + { + controllers_string.append(controller); + controllers_string.append(" "); + } + RCLCPP_ERROR( + get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str()); + } + deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); + if (!cumulative_fallback_controllers.empty()) + { + activate_controllers(rt_controller_list, cumulative_fallback_controllers); + } } // there are controllers to (de)activate @@ -2765,6 +2866,162 @@ controller_interface::return_type ControllerManager::check_preceeding_controller return controller_interface::return_type::OK; } +controller_interface::return_type +ControllerManager::check_fallback_controllers_state_pre_activation( + const std::vector & controllers, const ControllersListIterator controller_it) +{ + for (const auto & fb_ctrl : controller_it->info.fallback_controllers_names) + { + auto fb_ctrl_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, fb_ctrl)); + if (fb_ctrl_it == controllers.end()) + { + RCLCPP_ERROR( + get_logger(), + "Unable to find the fallback controller : '%s' of the controller : '%s' " + "within the controller list", + fb_ctrl.c_str(), controller_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + else + { + if (!(is_controller_inactive(fb_ctrl_it->c) || is_controller_active(fb_ctrl_it->c))) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as it's fallback controller : '%s'" + " need to be configured and be in inactive/active state!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + for (const auto & fb_cmd_itf : fb_ctrl_it->c->command_interface_configuration().names) + { + if (!resource_manager_->command_interface_is_available(fb_cmd_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_cmd_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_ref_itfs = + resource_manager_->get_controller_reference_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_ref_itfs.begin(), exported_ref_itfs.end(), fb_cmd_itf) == + exported_ref_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the command interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_cmd_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' command interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + for (const auto & fb_state_itf : fb_ctrl_it->c->state_interface_configuration().names) + { + if (!resource_manager_->state_interface_is_available(fb_state_itf)) + { + ControllersListIterator following_ctrl_it; + if (is_interface_a_chained_interface(fb_state_itf, controllers, following_ctrl_it)) + { + // if following_ctrl_it is inactive and it is in the fallback list of the + // controller_it and then check it it's exported reference interface names list if + // it's available + if (is_controller_inactive(following_ctrl_it->c)) + { + if ( + std::find( + controller_it->info.fallback_controllers_names.begin(), + controller_it->info.fallback_controllers_names.end(), + following_ctrl_it->info.name) != + controller_it->info.fallback_controllers_names.end()) + { + const auto exported_state_itfs = + resource_manager_->get_controller_exported_state_interface_names( + following_ctrl_it->info.name); + if ( + std::find(exported_state_itfs.begin(), exported_state_itfs.end(), fb_state_itf) == + exported_state_itfs.end()) + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not exported by the " + "controller : '%s' in the current fallback list!", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as the state interface : " + "'%s' required by its fallback controller : '%s' is not available as the " + "controller is not in active state!. May be consider adding this controller to " + "the fallback list of the controller : '%s' or already have it activated.", + controller_it->info.name.c_str(), fb_state_itf.c_str(), fb_ctrl.c_str(), + following_ctrl_it->info.name.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + else + { + RCLCPP_ERROR( + get_logger(), + "Controller with name '%s' cannot be activated, as not all of its fallback " + "controller's : '%s' state interfaces are currently available!", + controller_it->info.name.c_str(), fb_ctrl.c_str()); + return controller_interface::return_type::ERROR; + } + } + } + } + } + return controller_interface::return_type::OK; +} + void ControllerManager::controller_activity_diagnostic_callback( diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7b9af6ecfc..ac89239e09 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -137,6 +137,11 @@ std::vector TestController::get_state_interface_data() const return state_intr_data; } +void TestController::set_external_commands_for_testing(const std::vector & commands) +{ + external_commands_for_testing_ = commands; +} + } // namespace test_controller #include "pluginlib/class_list_macros.hpp" diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index 4e5f827cd0..d57fd9ddd9 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -70,6 +70,8 @@ class TestController : public controller_interface::ControllerInterface const std::string & getRobotDescription() const; + void set_external_commands_for_testing(const std::vector & commands); + unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 28e9f0477c..a016b20440 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -20,6 +20,7 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -398,7 +399,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd // if we do 2 times of the controller_manager update rate, the internal counter should be // similarly incremented EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate())); - EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); + EXPECT_EQ(test_controller->get_update_rate(), cm_->get_update_rate()); auto deactivate_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, @@ -531,3 +532,921 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) INSTANTIATE_TEST_SUITE_P( per_controller_update_rate_check, TestControllerUpdateRates, testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98)); + +class TestControllerManagerFallbackControllers +: public ControllerManagerFixture, + public testing::WithParamInterface +{ +}; + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_same_controller_in_fallback_list) +{ + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + + const std::vector fallback_controllers = {test_controller_1_name, "random_ctrl2"}; + rclcpp::Parameter fallback_ctrls_parameter( + test_controller_1_name + std::string(".fallback_controllers"), fallback_controllers); + cm_->set_parameter(fallback_ctrls_parameter); + { + ControllerManagerRunner cm_runner(this); + ASSERT_EQ( + nullptr, + cm_->load_controller(test_controller_1_name, test_controller::TEST_CONTROLLER_CLASS_NAME)); + } +} + +TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_controller_not_configured) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activation_simple_case) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, 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::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_command_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + cmd_itfs_cfg.names = {"random_non_existing_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_failed_activation_on_missing_state_interface) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + auto test_controller_2 = std::make_shared(); + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_itfs_cfg.names = {"non_existing_state_interface/position"}; + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_2->set_state_interface_configuration(state_itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + } + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller_1_name); + cm_->configure_controller(test_controller_2_name); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_valid_activation_if_one_or_more_fallback_controllers_are_already_active) +{ + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_3->set_command_interface_configuration(itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + } + + EXPECT_EQ(3u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + + /// @note Here the order is important do not change the starting order + for (const auto & start_controller : {test_controller_3_name, test_controller_1_name}) + { + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {start_controller}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, 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::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing({std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_multiple_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"joint1/position"}); + test_controller_4->set_exported_state_interface_names({"joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + + controller_spec.c = test_controller_4; + controller_spec.info.name = test_controller_4_name; + controller_spec.info.type = "test_chainable_controller::TestChainableController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_3 alone and it should fail + // as it doesn't have the chained state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {test_controller_3_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + + // Now unload the controller_1 and set it up again with test_controller_2, test_controller_3 and + // test_controller_4, and now it should work as all the controllers are in the list + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + // It is expected to place all the chainable interfaces first, so they can make the interfaces + // available + controller_spec.info.fallback_controllers_names = { + test_controller_4_name, test_controller_3_name, test_controller_2_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + test_controller_1->set_external_commands_for_testing( + {std::numeric_limits::quiet_NaN()}); + EXPECT_EQ( + controller_interface::return_type::ERROR, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_4->get_lifecycle_state().id()); + } +} + +TEST_F( + TestControllerManagerFallbackControllers, + test_fallback_controllers_with_chainable_controllers_other_failing_checks) +{ + const std::string test_controller_1_name = "test_controller_1"; + const std::string test_controller_2_name = "test_controller_2"; + const std::string test_controller_3_name = "test_controller_3"; + const std::string test_controller_4_name = "test_chainable_controller_2"; + + const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT; + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint1/position"}; + // controller 1 + auto test_controller_1 = std::make_shared(); + test_controller_1->set_command_interface_configuration(cmd_itfs_cfg); + // controller 2 + cmd_itfs_cfg.names = {test_controller_4_name + "/joint1/position"}; + auto test_controller_2 = std::make_shared(); + test_controller_2->set_command_interface_configuration(cmd_itfs_cfg); + // controller 3 + auto test_controller_3 = std::make_shared(); + controller_interface::InterfaceConfiguration itfs_cfg; + itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + cmd_itfs_cfg.names = {"joint2/velocity"}; + itfs_cfg.names = {test_controller_4_name + "/joint2/velocity"}; + test_controller_3->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_3->set_state_interface_configuration(itfs_cfg); + + // controller 4 + auto test_controller_4 = std::make_shared(); + cmd_itfs_cfg.names = {"joint1/position"}; + itfs_cfg.names = {"joint2/velocity"}; + test_controller_4->set_command_interface_configuration(cmd_itfs_cfg); + test_controller_4->set_state_interface_configuration(itfs_cfg); + test_controller_4->set_reference_interface_names({"modified_joint1/position"}); + test_controller_4->set_exported_state_interface_names({"modified_joint2/velocity"}); + + { + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_2_name, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + ControllerManagerRunner cm_runner(this); + cm_->add_controller(controller_spec); // add controller_1 + + controller_spec.c = test_controller_2; + controller_spec.info.name = test_controller_2_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_2 + + controller_spec.c = test_controller_3; + controller_spec.info.name = test_controller_3_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_3 + + controller_spec.c = test_controller_4; + controller_spec.info.name = test_controller_4_name; + controller_spec.info.type = "test_chainable_controller::TestChainableController"; + controller_spec.info.fallback_controllers_names = {}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_4 + } + + EXPECT_EQ(4u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller_1.use_count()); + EXPECT_EQ(2, test_controller_2.use_count()); + EXPECT_EQ(2, test_controller_3.use_count()); + EXPECT_EQ(2, test_controller_4.use_count()); + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller_4->get_lifecycle_state().id()); + + // configure controllers + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_2_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_3_name)); + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_4_name)); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_3->get_lifecycle_state().id()); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_4->get_lifecycle_state().id()); + + std::vector start_controllers = {test_controller_1_name}; + std::vector stop_controllers = {}; + { + // Now the controller_1 is set with test_controller_2 and test_controller_4_name and it should + // fail as it has an non existing state interface Start controller, will take effect at the end + // of the update function + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } + } + + // Now unload the controller_1 and set it up again with test_controller_3 and + // test_controller_4_name and it should fail as it has an non existing state interface + { + ControllerManagerRunner cm_runner(this); + cm_->unload_controller(test_controller_1_name); + cm_->unload_controller(test_controller_4_name); + + controller_manager::ControllerSpec controller_spec; + controller_spec.c = test_controller_1; + controller_spec.info.name = test_controller_1_name; + controller_spec.info.type = "test_controller::TestController"; + controller_spec.info.fallback_controllers_names = { + test_controller_3_name, test_controller_4_name}; + controller_spec.next_update_cycle_time = std::make_shared(0); + cm_->add_controller(controller_spec); // add controller_1 + + EXPECT_EQ( + controller_interface::return_type::OK, cm_->configure_controller(test_controller_1_name)); + + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); + } +} diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 892427bab7..087994bd23 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -3,25 +3,40 @@ ctrl_with_parameters_and_type: type: "controller_manager/test_controller" joint_names: ["joint0"] -chainable_ctrl_with_parameters_and_type: - ros__parameters: - type: "controller_manager/test_chainable_controller" - joint_names: ["joint1"] +/**: + chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + + wildcard_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] -/foo_namespace/ctrl_with_parameters_and_type: +/foo_namespace/ns_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint1"] -/foo_namespace/chainable_ctrl_with_parameters_and_type: +/foo_namespace/ns_chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +/foo_namespace/ns_ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] + +/**/wildcard_chainable_ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_chainable_controller" joint_names: ["joint1"] -/foo_namespace/ctrl_with_parameters_and_no_type: +/**/wildcard_ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] diff --git a/controller_manager/test/test_load_controller.cpp b/controller_manager/test/test_load_controller.cpp index 3645c06c24..ce7285fc49 100644 --- a/controller_manager/test/test_load_controller.cpp +++ b/controller_manager/test/test_load_controller.cpp @@ -66,13 +66,13 @@ TEST_F(TestLoadController, can_set_and_get_non_default_update_rate) lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_lifecycle_state().id()); - controller_if->get_node()->set_parameter({"update_rate", 1337}); + controller_if->get_node()->set_parameter({"update_rate", 50}); cm_->configure_controller("test_controller_01"); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_lifecycle_state().id()); - EXPECT_EQ(1337u, controller_if->get_update_rate()); + EXPECT_EQ(50u, controller_if->get_update_rate()); } class TestLoadedController : public TestLoadController diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index cba832f8bf..74e1efeeed 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -674,14 +674,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 0); @@ -689,17 +689,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -707,12 +708,12 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail as no type is defined!"; @@ -720,21 +721,18 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); } TEST_F( @@ -747,28 +745,28 @@ TEST_F( // Provide controller type via the parsed file EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + test_file_path + " --ros-args -r __ns:=/random_namespace"), 256) << "Should fail when parsed namespace through both way with different namespaces"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail when parsed namespace through both ways even with same namespacing name"; EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " + "ns_ctrl_with_parameters_and_type ns_chainable_ctrl_with_parameters_and_type --load-only -c " "test_controller_manager --namespace foo_namespace -p " + test_file_path), 0) @@ -777,17 +775,18 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); + chain_ctrl_with_parameters_and_type.info.name, "ns_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( chain_ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); @@ -795,12 +794,13 @@ TEST_F( chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), test_file_path); EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace -p " + + "ns_ctrl_with_parameters_and_no_type -c test_controller_manager --namespace foo_namespace " + "-p " + test_file_path), 256) << "Should fail as no type is defined!"; @@ -808,19 +808,123 @@ TEST_F( ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string(), test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ns_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( - cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string(), test_file_path); + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string(), test_file_path); +} + +TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it due to timeout but can find the parameters"; + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_type wildcard_chainable_ctrl_with_parameters_and_type " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " --ros-args -r __ns:=/foo_namespace"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.name, + "wildcard_chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.info.type, + test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + chain_ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + call_spawner( + "wildcard_ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path + + " --ros-args -r __ns:=/foo_namespace"), + 256) + << "Should fail as no type is defined!"; + // Will still be same as the current call will fail + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "wildcard_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); + ASSERT_EQ(ctrl_2.info.name, "wildcard_chainable_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ( ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F( + TestLoadControllerWithNamespacedCM, + spawner_test_fail_namespaced_controllers_with_non_wildcard_entries) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --controller-manager-timeout 1.0 -p " + + test_file_path), + 256) + << "Should fail without the namespacing it"; + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 256) + << "Should fail even namespacing it as ctrl_with_parameters_and_type is not a wildcard entry"; + EXPECT_EQ( + call_spawner( + "chainable_ctrl_with_parameters_and_type --load-only -c " + "test_controller_manager --namespace foo_namespace -p " + + test_file_path), + 0) + << "Should work as chainable_ctrl_with_parameters_and_type is a wildcard entry"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); ASSERT_EQ( - cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string(), - test_file_path); + ctrl_with_parameters_and_type.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_parameters_and_type.c->get_lifecycle_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 738add2492..ace7cac23d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -26,6 +26,7 @@ For details see the controller_manager section. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) * The new ``PoseSensor`` semantic component provides a standard interface for hardware providing cartesian poses (`#1775 `_) +* The controllers now support the fallback controllers (a list of controllers that will be activated, when the spawned controllers fails by throwing an exception or returning ``return_type::ERROR`` during the ``update`` cycle) (`#1789 `_) controller_manager ****************** @@ -74,6 +75,7 @@ controller_manager * The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 `_). * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 `_). +* Added support for the wildcard entries for the controller configuration files (`#1724 `_). * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). hardware_interface diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 6b6a42e3ce..593df643a7 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import warnings from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments @@ -50,10 +51,14 @@ def print_controller_state(c, args, col_width_name, col_width_state, col_width_t for connection in c.chain_connections: for reference in connection.reference_interfaces: print(f"\t\t{reference:20s}") - if args.reference_interfaces or args.verbose: + if args.reference_interfaces or args.exported_interfaces or args.verbose: print("\texported reference interfaces:") - for reference_interfaces in c.reference_interfaces: - print(f"\t\t{reference_interfaces}") + for reference_interface in c.reference_interfaces: + print(f"\t\t{reference_interface}") + if args.reference_interfaces or args.exported_interfaces or args.verbose: + print("\texported state interfaces:") + for exported_state_interface in c.exported_state_interfaces: + print(f"\t\t{exported_state_interface}") class ListControllersVerb(VerbExtension): @@ -86,6 +91,11 @@ def add_arguments(self, parser, cli_name): action="store_true", help="List controller's exported references", ) + parser.add_argument( + "--exported-interfaces", + action="store_true", + help="List controller's exported state and reference interfaces", + ) parser.add_argument( "--verbose", "-v", @@ -98,6 +108,13 @@ def main(self, *, args): with NodeStrategy(args).direct_node as node: response = list_controllers(node, args.controller_manager) + if args.reference_interfaces: + warnings.filterwarnings("always") + warnings.warn( + "The '--reference-interfaces' argument is deprecated and will be removed in future releases. Use '--exported-interfaces' instead.", + DeprecationWarning, + ) + if not response.controller: print("No controllers are currently loaded!") return 0 diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index 4cb6c01e09..20b2f47a1a 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -260,7 +260,7 @@ def _on_ctrl_menu(self, pos): if action is action_configure: configure_controller(self._node, self._cm_name, ctrl.name) elif action is action_spawn: - load_controller(self._node, self._cm_name, ctrl.name) + configure_controller(self._node, self._cm_name, ctrl.name) self._activate_controller(ctrl.name) else: # Assume controller isn't loaded