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..52666531c1 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,50 @@ 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" ) +<<<<<<< HEAD if controller_type: if not set_controller_parameters( node, controller_manager_name, controller_name, "type", controller_type +======= + if controller_type and not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type + ): + return False + + fallback_controllers = get_parameter_from_param_files( + node, + controller_name, + spawner_namespace, + controller_parameter_files, + "fallback_controllers", + ) + if fallback_controllers: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "fallback_controllers", + fallback_controllers, +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) ): return False return True diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py index 4cef8743ac..3c4bd47544 100644 --- a/controller_manager/controller_manager/launch_utils.py +++ b/controller_manager/controller_manager/launch_utils.py @@ -20,10 +20,14 @@ def generate_controllers_spawner_launch_description( +<<<<<<< HEAD controller_names: list, controller_type=None, controller_params_file=None, extra_spawner_args=[], +======= + controller_names: list, controller_params_files=None, extra_spawner_args=[] +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) ): """ Generate launch description for loading a controller using spawner. @@ -40,9 +44,14 @@ def generate_controllers_spawner_launch_description( # Passing controller type and parameter file to load the controller generate_controllers_spawner_launch_description( ['joint_state_broadcaster'], +<<<<<<< HEAD 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')], +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) extra_spawner_args=[--load-only] ) @@ -66,11 +75,18 @@ def generate_controllers_spawner_launch_description( ] ) +<<<<<<< HEAD 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] +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) # Setting --unload-on-kill if launch arg unload_on_kill is "true" # See https://github.com/ros2/launch/issues/290 @@ -105,12 +121,56 @@ 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], +<<<<<<< HEAD controller_type=controller_type, controller_params_file=controller_params_file, +======= + controller_params_file=controller_params_files, +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) extra_spawner_args=extra_spawner_args, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 83b1e2f89d..0f57b57525 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -26,8 +26,12 @@ load_controller, switch_controllers, unload_controller, +<<<<<<< HEAD set_controller_parameters, set_controller_parameters_from_param_file, +======= + set_controller_parameters_from_param_files, +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -81,7 +85,15 @@ def main(args=None): parser.add_argument( "-p", "--param-file", +<<<<<<< HEAD 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", +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) required=False, ) parser.add_argument( @@ -146,12 +158,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]) @@ -183,6 +197,7 @@ def main(args=None): + bcolors.ENDC ) else: +<<<<<<< HEAD if args.controller_type: if not set_controller_parameters( node, @@ -194,10 +209,14 @@ def main(args=None): return 1 if param_file: if not set_controller_parameters_from_param_file( +======= + if param_files: + if not set_controller_parameters_from_param_files( +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) 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..5d7a8f02bd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -594,16 +594,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,6 +2564,7 @@ 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(); +<<<<<<< HEAD const std::string ros_args_arg = "--ros-args"; if (controller.info.parameters_file.has_value()) { @@ -2573,6 +2574,36 @@ rclcpp::NodeOptions ControllerManager::determine_controller_node_options( } node_options_arguments.push_back("--params-file"); node_options_arguments.push_back(controller.info.parameters_file.value()); +======= + + for (const std::string & arg : cm_node_options_.arguments()) + { + if (arg.find("__ns") != std::string::npos || arg.find("__node") != std::string::npos) + { + if ( + node_options_arguments.back() == RCL_REMAP_FLAG || + node_options_arguments.back() == RCL_SHORT_REMAP_FLAG) + { + node_options_arguments.pop_back(); + } + continue; + } + + node_options_arguments.push_back(arg); + } + + if (controller.info.parameters_files.has_value()) + { + for (const auto & parameters_file : controller.info.parameters_files.value()) + { + 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); + } +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) } // 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 55beafaa7e..9769ac5c63 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); } @@ -338,6 +340,65 @@ TEST_F(TestLoadController, unload_on_kill_activate_as_group) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +<<<<<<< HEAD +======= +TEST_F(TestLoadController, spawner_test_fallback_controllers) +{ + const std::string 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)); + cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); + } + + // Try to spawn now the controller with fallback controllers inside the yaml + EXPECT_EQ( + call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); + ASSERT_EQ( + ctrl_1.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_1.params_file").as_string_array()[0], test_file_path); + + auto ctrl_2 = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); + ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT( + ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); + ASSERT_EQ( + ctrl_2.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_2.params_file").as_string_array()[0], test_file_path); + + auto ctrl_3 = cm_->get_loaded_controllers()[2]; + ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); + ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); + ASSERT_EQ( + ctrl_3.c->get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + ASSERT_EQ(cm_->get_parameter("ctrl_3.params_file").as_string_array()[0], test_file_path); + } +} + +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) TEST_F(TestLoadController, spawner_with_many_controllers) { std::stringstream ss; @@ -618,7 +679,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 +693,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( @@ -646,14 +709,28 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD 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( + 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_array()[0], test_file_path); +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) 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); +<<<<<<< HEAD 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( + 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_array()[0], test_file_path); +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) } TEST_F( @@ -702,7 +779,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 +793,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( @@ -731,14 +810,28 @@ TEST_F( auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ns_ctrl_with_parameters_and_type"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); +<<<<<<< HEAD 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( + 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_array()[0], test_file_path); +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) 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); +<<<<<<< HEAD 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( + 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_array()[0], test_file_path); +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) } TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_with_wildcard_entries_in_params_file) @@ -845,3 +938,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..2b8f7cd659 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -16,6 +16,51 @@ controller_interface controller_manager ****************** +<<<<<<< HEAD +======= + * Configured chainable controller: Listed exported interfaces are unavailable and unclaimed + * Active chainable controller (not in chained mode): Listed exported interfaces are available but unclaimed + * Active chainable controller (in chained mode): Listed exported interfaces are available and claimed +* Try using SCHED_FIFO on any kernel (`#1142 `_) +* A method to get node options to setup the controller node was added (`#1169 `_): ``get_node_options`` can be overridden by controllers, this would make it easy for other controllers to be able to setup their own custom node options +* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 `_). +* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 `_) +* Changes from `(PR #1256) `__ + + * All ``joints`` defined in the ````-tag have to be present in the URDF received :ref:`by the controller manager `, otherwise the following error is shown: + + The published robot description file (URDF) seems not to be genuine. The following error was caught: not found in URDF. + + This is to ensure that the URDF and the ````-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead. + + * The syntax for mimic joints is changed to the `official URDF specification `__. + + .. code-block:: xml + + + + + + + + + + + + + + + + + + The parameters within the ``ros2_control`` tag are not supported any more. +* 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 `_). +* 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 `_). +* ``--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 `_). +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) * ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). 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; diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index f28c9c1eb2..8321242588 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -12,7 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. +<<<<<<< HEAD from controller_manager import configure_controller, load_controller, switch_controllers +======= +from controller_manager import ( + configure_controller, + load_controller, + list_controllers, + switch_controllers, + set_controller_parameters_from_param_files, + bcolors, +) +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -41,9 +52,20 @@ def main(self, *, args): if not response.ok: return "Error loading controller, check controller_manager logs" +<<<<<<< HEAD if not args.set_state: print(f"Successfully loaded controller {args.controller_name}") return 0 +======= + if not set_controller_parameters_from_param_files( + node, + args.controller_manager, + args.controller_name, + [args.param_file], + node.get_namespace(), + ): + return 1 +>>>>>>> 73fe227 ([Spawner] Accept parsing multiple `--param-file` arguments to spawner (#1805)) # we in any case configure the controller response = configure_controller(node, args.controller_manager, args.controller_name)