From dc2162a089f41e62740cf6d61446390ca7b32601 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 28 Nov 2024 12:58:40 +0100 Subject: [PATCH 1/6] [Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805) --- .../controller_manager/__init__.py | 8 +- .../controller_manager_services.py | 157 ++++++++++------ .../controller_manager/launch_utils.py | 58 ++++-- .../controller_manager/spawner.py | 22 ++- controller_manager/doc/userdoc.rst | 2 +- controller_manager/src/controller_manager.cpp | 29 +-- .../test/test_spawner_unspawner.cpp | 169 ++++++++++++++++-- doc/release_notes.rst | 1 + .../hardware_interface/controller_info.hpp | 2 +- 9 files changed, 340 insertions(+), 108 deletions(-) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index 4a8d7daee5..638a28ce86 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,9 +23,9 @@ set_hardware_component_state, switch_controllers, unload_controller, - get_parameter_from_param_file, + get_parameter_from_param_files, set_controller_parameters, - set_controller_parameters_from_param_file, + set_controller_parameters_from_param_files, bcolors, ) @@ -40,8 +40,8 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", - "get_parameter_from_param_file", + "get_parameter_from_param_files", "set_controller_parameters", - "set_controller_parameters_from_param_file", + "set_controller_parameters_from_param_files", "bcolors", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index cc0b6f4645..a5148098f0 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -244,57 +244,90 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti ) -def get_parameter_from_param_file( - node, controller_name, namespace, parameter_file, parameter_name +def get_params_files_with_controller_parameters( + node, controller_name: str, namespace: str, parameter_files: list ): - with open(parameter_file) as f: - namespaced_controller = ( - f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" - ) - WILDCARD_KEY = "/**" - ROS_PARAMS_KEY = "ros__parameters" - parameters = yaml.safe_load(f) - 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}" + controller_parameter_files = [] + for parameter_file in parameter_files: + if parameter_file in controller_parameter_files: + continue + with open(parameter_file) as f: + namespaced_controller = ( + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" + ) + WILDCARD_KEY = "/**" + parameters = yaml.safe_load(f) + # 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_parameter_files.append(parameter_file) + + if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]: + controller_parameter_files.append(parameter_file) + return controller_parameter_files + + +def get_parameter_from_param_files( + node, controller_name: str, namespace: str, parameter_files: list, parameter_name: str +): + for parameter_file in parameter_files: + with open(parameter_file) as f: + namespaced_controller = ( + f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}" + ) + WILDCARD_KEY = "/**" + ROS_PARAMS_KEY = "ros__parameters" + parameters = yaml.safe_load(f) + 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 node : {namespaced_controller}" ) + if ( + controller_param_dict + and ROS_PARAMS_KEY in controller_param_dict + and parameter_name in controller_param_dict[ROS_PARAMS_KEY] + ): 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 node : {namespaced_controller}" - ) - 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 + if controller_param_dict and parameter_name in controller_param_dict[ROS_PARAMS_KEY]: + return controller_param_dict[ROS_PARAMS_KEY][parameter_name] + if controller_param_dict is None: + node.get_logger().fatal( + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter files : {parameter_files}{bcolors.ENDC}" + ) + return None def set_controller_parameters( @@ -338,21 +371,27 @@ def set_controller_parameters( return True -def set_controller_parameters_from_param_file( - node, controller_manager_name, controller_name, parameter_file, namespace=None +def set_controller_parameters_from_param_files( + node, controller_manager_name: str, controller_name: str, parameter_files: list, namespace=None ): - if parameter_file: - spawner_namespace = namespace if namespace else node.get_namespace() + spawner_namespace = namespace if namespace else node.get_namespace() + controller_parameter_files = get_params_files_with_controller_parameters( + node, controller_name, spawner_namespace, parameter_files + ) + if controller_parameter_files: set_controller_parameters( - node, controller_manager_name, controller_name, "params_file", parameter_file + node, + controller_manager_name, + controller_name, + "params_file", + controller_parameter_files, ) - controller_type = get_parameter_from_param_file( - node, controller_name, spawner_namespace, parameter_file, "type" + controller_type = get_parameter_from_param_files( + node, controller_name, spawner_namespace, controller_parameter_files, "type" ) - if controller_type: - if not set_controller_parameters( - node, controller_manager_name, controller_name, "type", controller_type - ): - return False + if controller_type and not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type + ): + return False return True diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 4cef8743ac..823181a0ee 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -22,7 +22,7 @@ def generate_controllers_spawner_launch_description( controller_names: list, controller_type=None, - controller_params_file=None, + controller_params_files=None, extra_spawner_args=[], ): """ @@ -40,9 +40,8 @@ def generate_controllers_spawner_launch_description( # Passing controller type and parameter file to load the controller generate_controllers_spawner_launch_description( ['joint_state_broadcaster'], - controller_type='joint_state_broadcaster/JointStateBroadcaster', - controller_params_file=os.path.join(get_package_share_directory('my_pkg'), - 'config', 'controller_params.yaml'), + controller_params_files=[os.path.join(get_package_share_directory('my_pkg'), + 'config', 'controller_params.yaml')], extra_spawner_args=[--load-only] ) @@ -66,11 +65,10 @@ def generate_controllers_spawner_launch_description( ] ) - if controller_type: - spawner_arguments += ["--controller-type", controller_type] - - if controller_params_file: - spawner_arguments += ["--param-file", controller_params_file] + if controller_params_files: + for controller_params_file in controller_params_files: + if controller_params_file: + spawner_arguments += ["--param-file", controller_params_file] # Setting --unload-on-kill if launch arg unload_on_kill is "true" # See https://github.com/ros2/launch/issues/290 @@ -105,12 +103,52 @@ def generate_controllers_spawner_launch_description( ) +def generate_controllers_spawner_launch_description_from_dict( + controller_info_dict: dict, extra_spawner_args=[] +): + """ + Generate launch description for loading a controller using spawner. + + controller_info_dict: dict + A dictionary with the following info: + - controller_name: str + The name of the controller to load as the key + - controller_params_file: str or list or None + The path to the controller parameter file or a list of paths to multiple parameter files + or None if no parameter file is needed as the value of the key + If a list is passed, the controller parameters will be overloaded in same order + extra_spawner_args: list + A list of extra arguments to pass to the controller spawner + """ + if not type(controller_info_dict) is dict: + raise ValueError(f"Invalid controller_info_dict type parsed {controller_info_dict}") + controller_names = controller_info_dict.keys() + controller_params_files = [] + for controller_name in controller_names: + controller_params_file = controller_info_dict[controller_name] + if controller_params_file: + if type(controller_params_file) is list: + controller_params_files.extend(controller_params_file) + elif type(controller_params_file) is str: + controller_params_files.append(controller_params_file) + else: + raise ValueError( + f"Invalid controller_params_file type parsed in the dict {controller_params_file}" + ) + return generate_controllers_spawner_launch_description( + controller_names=controller_names, + controller_params_files=controller_params_files, + extra_spawner_args=extra_spawner_args, + ) + + def generate_load_controller_launch_description( controller_name: str, controller_type=None, controller_params_file=None, extra_spawner_args=[] ): + controller_params_files = [controller_params_file] if controller_params_file else None return generate_controllers_spawner_launch_description( controller_names=[controller_name], controller_type=controller_type, - controller_params_file=controller_params_file, + controller_params_files=controller_params_files, extra_spawner_args=extra_spawner_args, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 83b1e2f89d..968a4d3d44 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -27,7 +27,7 @@ switch_controllers, unload_controller, set_controller_parameters, - set_controller_parameters_from_param_file, + set_controller_parameters_from_param_files, bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -81,7 +81,11 @@ def main(args=None): parser.add_argument( "-p", "--param-file", - help="Controller param file to be loaded into controller node before configure", + help="Controller param file to be loaded into controller node before configure. " + "Pass multiple times to load different files for different controllers or to " + "override the parameters of the same controller.", + default=None, + action="append", required=False, ) parser.add_argument( @@ -146,12 +150,14 @@ def main(args=None): args = parser.parse_args(command_line_args) controller_names = args.controller_names controller_manager_name = args.controller_manager - param_file = args.param_file + param_files = args.param_file controller_manager_timeout = args.controller_manager_timeout switch_timeout = args.switch_timeout - if param_file and not os.path.isfile(param_file): - raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) + if param_files: + for param_file in param_files: + if not os.path.isfile(param_file): + raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) node = Node("spawner_" + controller_names[0]) @@ -192,12 +198,12 @@ def main(args=None): args.controller_type, ): return 1 - if param_file: - if not set_controller_parameters_from_param_file( + if param_files: + if not set_controller_parameters_from_param_files( node, controller_manager_name, controller_name, - param_file, + param_files, spawner_namespace, ): return 1 diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 8f1c57161c..d21cd0a3b2 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -102,7 +102,7 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node -p PARAM_FILE, --param-file PARAM_FILE - Controller param file to be loaded into controller node before configure + Controller param file to be loaded into controller node before configure. Pass multiple times to load different files for different controllers or to override the parameters of the same controller. -n NAMESPACE, --namespace NAMESPACE Namespace for the controller --load-only Only load the controller and leave unconfigured. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1624b47c2d..5f21c8e4ac 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -24,6 +24,7 @@ #include "controller_manager_msgs/msg/hardware_component_state.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "rcl/arguments.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -594,16 +595,16 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c // read_only params, dynamic maps lists etc // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::string parameters_file; + std::vector parameters_files; // Check if parameter has been declared if (!has_parameter(param_name)) { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING); + declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); } - if (get_parameter(param_name, parameters_file) && !parameters_file.empty()) + if (get_parameter(param_name, parameters_files) && !parameters_files.empty()) { - controller_spec.info.parameters_file = parameters_file; + controller_spec.info.parameters_files = parameters_files; } return add_controller_impl(controller_spec); @@ -2564,26 +2565,28 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true); std::vector node_options_arguments = controller_node_options.arguments(); - const std::string ros_args_arg = "--ros-args"; - if (controller.info.parameters_file.has_value()) + if (controller.info.parameters_files.has_value()) { - if (!check_for_element(node_options_arguments, ros_args_arg)) + for (const auto & parameters_file : controller.info.parameters_files.value()) { - node_options_arguments.push_back(ros_args_arg); + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) + { + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + } + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); + node_options_arguments.push_back(parameters_file); } - node_options_arguments.push_back("--params-file"); - node_options_arguments.push_back(controller.info.parameters_file.value()); } // ensure controller's `use_sim_time` parameter matches controller_manager's const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); if (use_sim_time.as_bool()) { - if (!check_for_element(node_options_arguments, ros_args_arg)) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - node_options_arguments.push_back(ros_args_arg); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } - node_options_arguments.push_back("-p"); + node_options_arguments.push_back(RCL_PARAM_FLAG); node_options_arguments.push_back("use_sim_time:=true"); } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 55beafaa7e..43018b7a43 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -260,7 +260,8 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ctrl_with_parameters_and_type.c->get_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.params_file").as_string_array()[0], + test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; ASSERT_EQ( @@ -272,7 +273,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_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("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); EXPECT_EQ( @@ -289,14 +290,15 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ(ctrl_1.c->get_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.params_file").as_string_array()[0], + 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.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ(ctrl_2.c->get_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("chainable_ctrl_with_parameters_and_type.params_file").as_string_array()[0], test_file_path); } @@ -618,7 +620,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -631,7 +634,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) chain_ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); EXPECT_EQ( @@ -647,13 +651,15 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) 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_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); + ASSERT_EQ( + cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], 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(ctrl_2.c->get_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); + ASSERT_EQ( + cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } TEST_F( @@ -702,7 +708,8 @@ TEST_F( ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; @@ -715,7 +722,8 @@ TEST_F( chain_ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ASSERT_EQ( - cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file").as_string(), + cm_->get_parameter(chain_ctrl_with_parameters_and_type.info.name + ".params_file") + .as_string_array()[0], test_file_path); EXPECT_EQ( @@ -732,13 +740,15 @@ TEST_F( 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_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); + ASSERT_EQ( + cm_->get_parameter(ctrl_1.info.name + ".params_file").as_string_array()[0], 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(ctrl_2.c->get_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); + ASSERT_EQ( + cm_->get_parameter(ctrl_2.info.name + ".params_file").as_string_array()[0], test_file_path); } TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) @@ -845,3 +855,138 @@ TEST_F( ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } + +TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " -p" + fallback_test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); + + 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.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 params_file_info = + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], 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"); + 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); + params_file_info = + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); + + auto ctrl_1 = cm_->get_loaded_controllers()[3]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + 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); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +} + +TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + const std::string fallback_test_file_path = + ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_fallback_controllers.yaml"; + + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " + "--load-only -c " + "test_controller_manager -p " + + test_file_path + " -p" + fallback_test_file_path + " -p" + fallback_test_file_path + " -p" + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); + + 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.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 params_file_info = + cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], 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"); + 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); + params_file_info = + cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); + + auto ctrl_1 = cm_->get_loaded_controllers()[3]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + 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); + params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); + ASSERT_EQ(params_file_info.size(), 1ul); + ASSERT_EQ(params_file_info[0], fallback_test_file_path); +} diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 400e234d23..9291a4dbc4 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,3 +22,4 @@ controller_manager * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). * ``--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 `_). +* The spawner now supports parsing multiple ``-p`` or ``--param-file`` arguments, this should help in loading multiple parameter files for a controller or for multiple controllers (`#1805 `_). diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 61a51a134a..d684596d55 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,7 +34,7 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional parameters_file; + std::optional> parameters_files; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; From d41c2a00f015d1d6f5502170cb88834470bc4e06 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 10:41:45 +0100 Subject: [PATCH 2/6] add logic for 'params_file' to handle both string and string_array (#1898) --- controller_manager/src/controller_manager.cpp | 41 +++++++++++-------- .../test/test_spawner_unspawner.cpp | 38 +++++++++++++++++ .../hardware_interface/controller_info.hpp | 2 +- 3 files changed, 64 insertions(+), 17 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5f21c8e4ac..1f387b89ee 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -595,16 +595,28 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c // read_only params, dynamic maps lists etc // Now check if the parameters_file parameter exist const std::string param_name = controller_name + ".params_file"; - std::vector parameters_files; + controller_spec.info.parameters_files.clear(); - // Check if parameter has been declared - if (!has_parameter(param_name)) + // get_parameter checks if parameter has been declared/set + rclcpp::Parameter params_files_parameter; + if (get_parameter(param_name, params_files_parameter)) { - declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING_ARRAY); - } - if (get_parameter(param_name, parameters_files) && !parameters_files.empty()) - { - controller_spec.info.parameters_files = parameters_files; + if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { + controller_spec.info.parameters_files = params_files_parameter.as_string_array(); + } + else if (params_files_parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { + controller_spec.info.parameters_files.push_back(params_files_parameter.as_string()); + } + else + { + RCLCPP_ERROR( + get_logger(), + "The 'params_file' param needs to be a string or a string array for '%s', but it is of " + "type %s", + controller_name.c_str(), params_files_parameter.get_type_name().c_str()); + } } return add_controller_impl(controller_spec); @@ -2565,17 +2577,14 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( .allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true); std::vector node_options_arguments = controller_node_options.arguments(); - if (controller.info.parameters_files.has_value()) + for (const auto & parameters_file : controller.info.parameters_files) { - for (const auto & parameters_file : controller.info.parameters_files.value()) + if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) { - if (!check_for_element(node_options_arguments, RCL_ROS_ARGS_FLAG)) - { - node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); - } - node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); - node_options_arguments.push_back(parameters_file); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); } + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); + node_options_arguments.push_back(parameters_file); } // ensure controller's `use_sim_time` parameter matches controller_manager's diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 43018b7a43..ecb238d78d 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -220,6 +220,44 @@ TEST_F(TestLoadController, multi_ctrls_test_type_in_param) } } +TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + cm_->set_parameter(rclcpp::Parameter( + "ctrl_with_parameters_and_type.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter( + rclcpp::Parameter("ctrl_with_parameters_and_type.params_file", test_file_path)); + + 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"), 0); + + 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, "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); + auto ctrl_node = ctrl_with_parameters_and_type.c->get_node(); + ASSERT_THAT( + ctrl_with_parameters_and_type.info.parameters_files, + std::vector({test_file_path})); + if (!ctrl_node->has_parameter("joint_names")) + { + ctrl_node->declare_parameter("joint_names", std::vector({"random_joint"})); + } + ASSERT_THAT( + ctrl_node->get_parameter("joint_names").as_string_array(), + std::vector({"joint0"})); +} + TEST_F(TestLoadController, spawner_test_type_in_arg) { ControllerManagerRunner cm_runner(this); diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index d684596d55..3e25ca731e 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -34,7 +34,7 @@ struct ControllerInfo std::string type; /// Controller param file - std::optional> parameters_files; + std::vector parameters_files; /// List of claimed interfaces by the controller. std::vector claimed_interfaces; From 825ebfa9b03c8d21c3bfea003420c4b592241234 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 29 Nov 2024 13:12:46 +0100 Subject: [PATCH 3/6] Add more parameter overriding tests by parsing multiple parameter files (#1899) --- controller_manager/CMakeLists.txt | 3 +++ .../test/test_controller_overriding_parameters.yaml | 5 +++++ .../test/test_controller_spawner_with_type.yaml | 1 + controller_manager/test/test_spawner_unspawner.cpp | 6 ++++++ 4 files changed, 15 insertions(+) create mode 100644 controller_manager/test/test_controller_overriding_parameters.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 8919020177..e31f0b5917 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -186,6 +186,9 @@ if(BUILD_TESTING) install(FILES test/test_controller_spawner_with_type.yaml DESTINATION test) + install(FILES test/test_controller_overriding_parameters.yaml + DESTINATION test) + ament_add_gmock( test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/controller_manager/test/test_controller_overriding_parameters.yaml b/controller_manager/test/test_controller_overriding_parameters.yaml new file mode 100644 index 0000000000..115d0ed993 --- /dev/null +++ b/controller_manager/test/test_controller_overriding_parameters.yaml @@ -0,0 +1,5 @@ +ctrl_with_parameters_and_type: + ros__parameters: + interface_name: "impedance" + joint_offset: 0.2 + joint_names: ["joint10"] diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 087994bd23..23fd69b216 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -2,6 +2,7 @@ ctrl_with_parameters_and_type: ros__parameters: type: "controller_manager/test_controller" joint_names: ["joint0"] + interface_name: "position" /**: chainable_ctrl_with_parameters_and_type: diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index ecb238d78d..74568c1332 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -256,6 +256,12 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) ASSERT_THAT( ctrl_node->get_parameter("joint_names").as_string_array(), std::vector({"joint0"})); + + if (!ctrl_node->has_parameter("interface_name")) + { + ctrl_node->declare_parameter("interface_name", "invalid_interface"); + } + ASSERT_EQ(ctrl_node->get_parameter("interface_name").as_string(), "position"); } TEST_F(TestLoadController, spawner_test_type_in_arg) From 274d81e80e47c8b0b11ffdc8852aa9f299c07fcd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Dec 2024 20:57:21 +0100 Subject: [PATCH 4/6] change get_lifecycle_state to get_state --- .../test/test_spawner_unspawner.cpp | 22 ++++++++----------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 74568c1332..1f4164ad32 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -241,7 +241,7 @@ TEST_F(TestLoadController, spawner_test_with_params_file_string_parameter) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "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(), + ctrl_with_parameters_and_type.c->get_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); @@ -927,7 +927,7 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "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(), + ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto params_file_info = cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -941,7 +941,7 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) 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(), + chain_ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -951,8 +951,7 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) auto ctrl_2 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ( - ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], fallback_test_file_path); @@ -960,8 +959,7 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[3]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); 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(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], fallback_test_file_path); @@ -995,7 +993,7 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "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(), + ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto params_file_info = cm_->get_parameter("ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -1009,7 +1007,7 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) 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(), + chain_ctrl_with_parameters_and_type.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("chainable_ctrl_with_parameters_and_type.params_file").as_string_array(); @@ -1019,8 +1017,7 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) auto ctrl_2 = cm_->get_loaded_controllers()[2]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ( - ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], fallback_test_file_path); @@ -1028,8 +1025,7 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) auto ctrl_1 = cm_->get_loaded_controllers()[3]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); 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(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); ASSERT_EQ(params_file_info[0], fallback_test_file_path); From 55cf6c56618fbe68c580af3fc6733062be42f62c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Dec 2024 21:49:26 +0100 Subject: [PATCH 5/6] adapt the tests for humble --- ...roller_spawner_with_basic_controllers.yaml | 13 +++++++++++ .../test/test_spawner_unspawner.cpp | 22 +++++++++---------- 2 files changed, 24 insertions(+), 11 deletions(-) create mode 100644 controller_manager/test/test_controller_spawner_with_basic_controllers.yaml diff --git a/controller_manager/test/test_controller_spawner_with_basic_controllers.yaml b/controller_manager/test/test_controller_spawner_with_basic_controllers.yaml new file mode 100644 index 0000000000..bc93f162c1 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_basic_controllers.yaml @@ -0,0 +1,13 @@ +ctrl_1: + ros__parameters: + joint_names: ["joint1"] + +ctrl_2: + ros__parameters: + joint_names: ["joint2"] + fallback_controllers: ["ctrl_6", "ctrl_7", "ctrl_8"] + +ctrl_3: + ros__parameters: + joint_names: ["joint3"] + fallback_controllers: ["ctrl_9"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 1f4164ad32..a21c3b3b2c 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -904,9 +904,9 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; - const std::string fallback_test_file_path = + const std::string basic_ctrls_test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_fallback_controllers.yaml"; + "/test/test_controller_spawner_with_basic_controllers.yaml"; cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); @@ -918,7 +918,7 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " "--load-only -c " "test_controller_manager -p " + - test_file_path + " -p" + fallback_test_file_path), + test_file_path + " -p" + basic_ctrls_test_file_path), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); @@ -954,7 +954,7 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); - ASSERT_EQ(params_file_info[0], fallback_test_file_path); + ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); auto ctrl_1 = cm_->get_loaded_controllers()[3]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); @@ -962,16 +962,16 @@ TEST_F(TestLoadController, spawner_test_parsing_multiple_params_file) ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); - ASSERT_EQ(params_file_info[0], fallback_test_file_path); + ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); } TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; - const std::string fallback_test_file_path = + const std::string basic_ctrls_test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_fallback_controllers.yaml"; + "/test/test_controller_spawner_with_basic_controllers.yaml"; cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); @@ -983,8 +983,8 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type ctrl_2 ctrl_1 " "--load-only -c " "test_controller_manager -p " + - test_file_path + " -p" + fallback_test_file_path + " -p" + fallback_test_file_path + " -p" + - test_file_path), + test_file_path + " -p" + basic_ctrls_test_file_path + " -p" + basic_ctrls_test_file_path + + " -p" + test_file_path), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 4ul); @@ -1020,7 +1020,7 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_2.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); - ASSERT_EQ(params_file_info[0], fallback_test_file_path); + ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); auto ctrl_1 = cm_->get_loaded_controllers()[3]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); @@ -1028,5 +1028,5 @@ TEST_F(TestLoadController, spawner_test_parsing_same_params_file_multiple_times) ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); params_file_info = cm_->get_parameter("ctrl_1.params_file").as_string_array(); ASSERT_EQ(params_file_info.size(), 1ul); - ASSERT_EQ(params_file_info[0], fallback_test_file_path); + ASSERT_EQ(params_file_info[0], basic_ctrls_test_file_path); } From faf2e153e201d10bdb4aad6b4c2edc807a8a1fe7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 3 Dec 2024 22:12:24 +0100 Subject: [PATCH 6/6] add missing install rule --- controller_manager/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e31f0b5917..2d46561492 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -184,7 +184,10 @@ if(BUILD_TESTING) ament_target_dependencies(test_hardware_spawner ros2_control_test_assets) install(FILES test/test_controller_spawner_with_type.yaml - DESTINATION test) + DESTINATION test) + + install(FILES test/test_controller_spawner_with_basic_controllers.yaml + DESTINATION test) install(FILES test/test_controller_overriding_parameters.yaml DESTINATION test)