Merge branch 'humble' into mergify/bp/humble/pr-1890

.pre-commit-config.yaml

@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.3
+    rev: v19.1.4
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.4
+    rev: 0.30.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.45.0 (2024-12-03)
+-------------------
+
 2.44.0 (2024-11-09)
 -------------------
 * Add few warning compiler options to error (backport `#1181 <https://github.com/ros-controls/ros2_control/issues/1181>`_) (`#1816 <https://github.com/ros-controls/ros2_control/issues/1816>`_)

controller_interface/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>2.44.0</version>
+  <version>2.45.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.45.0 (2024-12-03)
+-------------------
+* Add CM `switch_controller` service timeout as parameter to spawner.py (backport `#1790 <https://github.com/ros-controls/ros2_control/issues/1790>`_) (`#1879 <https://github.com/ros-controls/ros2_control/issues/1879>`_)
+* Fix Hardware spawner and add tests for it (backport `#1759 <https://github.com/ros-controls/ros2_control/issues/1759>`_) (`#1827 <https://github.com/ros-controls/ros2_control/issues/1827>`_)
+* Contributors: mergify[bot]
+
 2.44.0 (2024-11-09)
 -------------------
 * [ros2_control_node] Handle simulation environment clocks (backport `#1810 <https://github.com/ros-controls/ros2_control/issues/1810>`_) (`#1862 <https://github.com/ros-controls/ros2_control/issues/1862>`_)

controller_manager/CMakeLists.txt

@@ -184,7 +184,13 @@ 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)
 
   ament_add_gmock(
     test_hardware_management_srvs

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

@@ -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

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,
     )