Merge branch 'master' into add/hw_components/update_rate

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",
 ]

controller_manager/controller_manager/controller_manager_services.py

@@ -284,57 +284,90 @@ def unload_controller(
     )
 
 
-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(
@@ -378,26 +411,36 @@ 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
 
-        fallback_controllers = get_parameter_from_param_file(
-            node, controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+        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(

controller_manager/controller_manager/launch_utils.py

@@ -20,7 +20,7 @@
 
 
 def generate_controllers_spawner_launch_description(
-    controller_names: list, controller_params_file=None, extra_spawner_args=[]
+    controller_names: list, controller_params_files=None, extra_spawner_args=[]
 ):
     """
     Generate launch description for loading a controller using spawner.
@@ -37,8 +37,8 @@ def generate_controllers_spawner_launch_description(
       # Passing controller parameter file to load the controller (Controller type is retrieved from config file)
       generate_controllers_spawner_launch_description(
         ['joint_state_broadcaster'],
-        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]
         )
 
@@ -62,8 +62,10 @@ def generate_controllers_spawner_launch_description(
         ]
     )
 
-    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
@@ -98,11 +100,51 @@ 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_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_params_file=controller_params_file,
+        controller_params_file=controller_params_files,
         extra_spawner_args=extra_spawner_args,
     )

controller_manager/controller_manager/spawner.py

@@ -26,7 +26,7 @@
     load_controller,
     switch_controllers,
     unload_controller,
-    set_controller_parameters_from_param_file,
+    set_controller_parameters_from_param_files,
     bcolors,
 )
 from controller_manager.controller_manager_services import ServiceNotFoundError
@@ -83,8 +83,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(
@@ -147,13 +150,15 @@ 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
     service_call_timeout = args.service_call_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])
 
@@ -198,12 +203,12 @@ def main(args=None):
                     + bcolors.ENDC
                 )
             else:
-                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