[Spawner] Add support for wildcard entries in the controller param fi…

…les (#1724)

(cherry picked from commit 83fff77)

# Conflicts:
#	controller_manager/controller_manager/controller_manager_services.py
#	controller_manager/test/test_spawner_unspawner.cpp
#	doc/release_notes.rst

controller_manager/controller_manager/controller_manager_services.py

@@ -244,24 +244,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(
@@ -315,11 +348,27 @@ def set_controller_parameters_from_param_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(
                 node, controller_manager_name, controller_name, "type", controller_type
             ):
                 return False
+<<<<<<< HEAD
+=======
+
+        fallback_controllers = get_parameter_from_param_file(
+            node, controller_name, spawner_namespace, parameter_file, "fallback_controllers"
+        )
+        if fallback_controllers:
+            if not set_controller_parameters(
+                node,
+                controller_manager_name,
+                controller_name,
+                "fallback_controllers",
+                fallback_controllers,
+            ):
+                return False
+>>>>>>> 83fff77 ([Spawner] Add support for wildcard entries in the controller param files  (#1724))
     return True

controller_manager/doc/userdoc.rst

@@ -112,6 +112,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/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"]