Merge branch 'iron' into mergify/bp/iron/pr-1759

.pre-commit-config.yaml

@@ -16,7 +16,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.6.0
+    rev: v5.0.0
     hooks:
       - id: check-added-large-files
       - id: check-ast
@@ -36,7 +36,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.17.0
+    rev: v3.19.0
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -49,7 +49,7 @@ repos:
       args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
 
   - repo: https://github.com/psf/black
-    rev: 24.8.0
+    rev: 24.10.0
     hooks:
       - id: black
         args: ["--line-length=99"]
@@ -62,7 +62,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.0
+    rev: v19.1.3
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -132,7 +132,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.3
+    rev: 0.29.4
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.29.0 (2024-11-02)
+-------------------
+* `PoseSensor` semantic component (backport `#1775 <https://github.com/ros-controls/ros2_control/issues/1775>`_) (`#1786 <https://github.com/ros-controls/ros2_control/issues/1786>`_)
+* Contributors: mergify[bot]
+
 3.28.1 (2024-09-11)
 -------------------
 

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>3.28.1</version>
+  <version>3.29.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,18 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+3.29.0 (2024-11-02)
+-------------------
+* add thread_priority option to the ros2_control_node (`#1820 <https://github.com/ros-controls/ros2_control/issues/1820>`_) (`#1825 <https://github.com/ros-controls/ros2_control/issues/1825>`_)
+* Fix timeout value in std output (backport `#1807 <https://github.com/ros-controls/ros2_control/issues/1807>`_) (`#1813 <https://github.com/ros-controls/ros2_control/issues/1813>`_)
+* Improve launch utils to support the multiple controller names (`#1782 <https://github.com/ros-controls/ros2_control/issues/1782>`_) (`#1784 <https://github.com/ros-controls/ros2_control/issues/1784>`_)
+* allow extra spawner arguments to not declare every argument in launch utils (`#1505 <https://github.com/ros-controls/ros2_control/issues/1505>`_) (`#1793 <https://github.com/ros-controls/ros2_control/issues/1793>`_)
+* Refactor spawner to be able to reuse code for ros2controlcli (backport `#1661 <https://github.com/ros-controls/ros2_control/issues/1661>`_) (`#1696 <https://github.com/ros-controls/ros2_control/issues/1696>`_)
+* [CM] Handle other exceptions while loading the controller plugin (`#1731 <https://github.com/ros-controls/ros2_control/issues/1731>`_) (`#1734 <https://github.com/ros-controls/ros2_control/issues/1734>`_)
+* [CM] Throw an exception when the components initially fail to be in the required state (backport `#1729 <https://github.com/ros-controls/ros2_control/issues/1729>`_) (`#1778 <https://github.com/ros-controls/ros2_control/issues/1778>`_)
+* Fix spawner tests timeout on source builds (backport `#1692 <https://github.com/ros-controls/ros2_control/issues/1692>`_) (`#1698 <https://github.com/ros-controls/ros2_control/issues/1698>`_)
+* Contributors: mergify[bot]
+
 3.28.1 (2024-09-11)
 -------------------
 * fix: the print of the information in control node was in wrong order (`#1726 <https://github.com/ros-controls/ros2_control/issues/1726>`_) (`#1727 <https://github.com/ros-controls/ros2_control/issues/1727>`_)

controller_manager/controller_manager/controller_manager_services.py

@@ -245,24 +245,57 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
     )
 
 
-def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
+def get_parameter_from_param_file(
+    node, controller_name, namespace, parameter_file, parameter_name
+):
     with open(parameter_file) as f:
         namespaced_controller = (
-            controller_name if namespace == "/" else f"{namespace}/{controller_name}"
+            f"/{controller_name}" if namespace == "/" else f"{namespace}/{controller_name}"
         )
+        WILDCARD_KEY = "/**"
+        ROS_PARAMS_KEY = "ros__parameters"
         parameters = yaml.safe_load(f)
-        if namespaced_controller in parameters:
-            value = parameters[namespaced_controller]
-            if not isinstance(value, dict) or "ros__parameters" not in value:
+        controller_param_dict = None
+        # check for the parameter in 'controller_name' or 'namespaced_controller' or '/**/namespaced_controller' or '/**/controller_name'
+        for key in [
+            controller_name,
+            namespaced_controller,
+            f"{WILDCARD_KEY}/{controller_name}",
+            f"{WILDCARD_KEY}{namespaced_controller}",
+        ]:
+            if key in parameters:
+                if key == controller_name and namespace != "/":
+                    node.get_logger().fatal(
+                        f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}"
+                    )
+                    break
+                controller_param_dict = parameters[key]
+
+            if WILDCARD_KEY in parameters and key in parameters[WILDCARD_KEY]:
+                controller_param_dict = parameters[WILDCARD_KEY][key]
+
+            if controller_param_dict and (
+                not isinstance(controller_param_dict, dict)
+                or ROS_PARAMS_KEY not in controller_param_dict
+            ):
                 raise RuntimeError(
-                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
+                    f"YAML file : {parameter_file} is not a valid ROS parameter file for controller node : {namespaced_controller}"
                 )
-            if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
-                return parameters[namespaced_controller]["ros__parameters"][parameter_name]
-            else:
-                return None
-        else:
-            return None
+            if (
+                controller_param_dict
+                and ROS_PARAMS_KEY in controller_param_dict
+                and parameter_name in controller_param_dict[ROS_PARAMS_KEY]
+            ):
+                break
+
+        if controller_param_dict is None:
+            node.get_logger().fatal(
+                f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter file : {parameter_file}{bcolors.ENDC}"
+            )
+        if parameter_name in controller_param_dict[ROS_PARAMS_KEY]:
+            return controller_param_dict[ROS_PARAMS_KEY][parameter_name]
+
+        return None
 
 
 def set_controller_parameters(
@@ -312,11 +345,11 @@ def set_controller_parameters_from_param_file(
     if parameter_file:
         spawner_namespace = namespace if namespace else node.get_namespace()
         set_controller_parameters(
-            node, controller_manager_name, controller_name, "param_file", parameter_file
+            node, controller_manager_name, controller_name, "params_file", parameter_file
         )
 
         controller_type = get_parameter_from_param_file(
-            controller_name, spawner_namespace, parameter_file, "type"
+            node, controller_name, spawner_namespace, parameter_file, "type"
         )
         if controller_type:
             if not set_controller_parameters(

controller_manager/controller_manager/spawner.py

@@ -237,7 +237,7 @@ def main(args=None):
 
             node.get_logger().info(
                 bcolors.OKGREEN
-                + "Configured and activated all the parsed controllers list!"
+                + f"Configured and activated all the parsed controllers list : {controller_names}!"
                 + bcolors.ENDC
             )
 
@@ -261,16 +261,25 @@ def main(args=None):
                     )
                     return 1
 
-                node.get_logger().info("Deactivated controller")
-
-            ret = unload_controller(node, controller_manager_name, controller_name)
-            if not ret.ok:
-                node.get_logger().error(
-                    bcolors.FAIL + "Failed to unload controller" + bcolors.ENDC
+                node.get_logger().info(
+                    f"Successfully deactivated controllers : {controller_names}"
                 )
-                return 1
 
-            node.get_logger().info("Unloaded controller")
+            unload_status = True
+            for controller_name in controller_names:
+                ret = unload_controller(node, controller_manager_name, controller_name)
+                if not ret.ok:
+                    unload_status = False
+                    node.get_logger().error(
+                        bcolors.FAIL
+                        + f"Failed to unload controller : {controller_name}"
+                        + bcolors.ENDC
+                    )
+
+            if unload_status:
+                node.get_logger().info(f"Successfully unloaded controllers : {controller_names}")
+            else:
+                return 1
         return 0
     except KeyboardInterrupt:
         pass

controller_manager/doc/userdoc.rst

@@ -114,6 +114,60 @@ There are two scripts to interact with controller manager from launch files:
                             Time to wait for the controller manager
 
 
+The parsed controller config file can follow the same conventions as the typical ROS 2 parameter file format. Now, the spawner can handle config files with wildcard entries and also the controller name in the absolute namespace. See the following examples on the config files:
+
+ .. code-block:: yaml
+
+    /**/position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
+ .. code-block:: yaml
+
+    /position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
+ .. code-block:: yaml
+
+    position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
+ .. code-block:: yaml
+
+    /rrbot_1/position_trajectory_controller:
+    ros__parameters:
+      type: joint_trajectory_controller/JointTrajectoryController
+      joints:
+        - joint1
+        - joint2
+
+      command_interfaces:
+        - position
+        .....
+
 ``unspawner``
 ^^^^^^^^^^^^^^^^
 

controller_manager/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_manager</name>
-  <version>3.28.1</version>
+  <version>3.29.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/test/test_controller_spawner_with_type.yaml

@@ -3,25 +3,40 @@ ctrl_with_parameters_and_type:
     type: "controller_manager/test_controller"
     joint_names: ["joint0"]
 
-chainable_ctrl_with_parameters_and_type:
-  ros__parameters:
-    type: "controller_manager/test_chainable_controller"
-    joint_names: ["joint1"]
+/**:
+  chainable_ctrl_with_parameters_and_type:
+    ros__parameters:
+      type: "controller_manager/test_chainable_controller"
+      joint_names: ["joint1"]
+
+  wildcard_ctrl_with_parameters_and_type:
+    ros__parameters:
+      type: "controller_manager/test_controller"
+      joint_names: ["joint1"]
 
 ctrl_with_parameters_and_no_type:
   ros__parameters:
     joint_names: ["joint2"]
 
-/foo_namespace/ctrl_with_parameters_and_type:
+/foo_namespace/ns_ctrl_with_parameters_and_type:
   ros__parameters:
     type: "controller_manager/test_controller"
     joint_names: ["joint1"]
 
-/foo_namespace/chainable_ctrl_with_parameters_and_type:
+/foo_namespace/ns_chainable_ctrl_with_parameters_and_type:
+  ros__parameters:
+    type: "controller_manager/test_chainable_controller"
+    joint_names: ["joint1"]
+
+/foo_namespace/ns_ctrl_with_parameters_and_no_type:
+  ros__parameters:
+    joint_names: ["joint2"]
+
+/**/wildcard_chainable_ctrl_with_parameters_and_type:
   ros__parameters:
     type: "controller_manager/test_chainable_controller"
     joint_names: ["joint1"]
 
-/foo_namespace/ctrl_with_parameters_and_no_type:
+/**/wildcard_ctrl_with_parameters_and_no_type:
   ros__parameters:
     joint_names: ["joint2"]