[Spawner] Allow parsing the parameter files parsed from spawner to controllers (#3136)

(cherry picked from commit a583699)

# Conflicts:
#	controller_manager/controller_manager/spawner.py
#	controller_manager/doc/userdoc.rst
#	controller_manager/test/test_advanced_spawner.cpp
#	doc/release_notes.rst

Author: saikishor

controller_manager/controller_manager/spawner.py

@@ -158,6 +158,55 @@ def main(args=None):
         required=False,
     )
 
+<<<<<<< HEAD
+=======
+    global_namespace_args = parser.parse_args(args)
+
+    controllers_info = []
+    for controller_name in global_namespace_args.controller_names:
+        controllers_info.append(
+            {
+                "name": controller_name,
+                "param_files": global_namespace_args.param_file,
+                "load_only": global_namespace_args.load_only,
+                "inactive": global_namespace_args.inactive,
+                "controller_ros_args": global_namespace_args.controller_ros_args,
+            }
+        )
+
+    return global_namespace_args, controllers_info
+
+
+def main(args=None):
+    rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
+
+    def append_unique_preserve_order(base, to_append):
+        if base is None:
+            base = []
+        existing = set(base)
+        for item in to_append:
+            if item not in existing:
+                base.append(item)
+                existing.add(item)
+        return base
+
+    def get_ros_params_files(argv):
+        params_files = []
+        index = 0
+        while index < len(argv):
+            if argv[index] == "--params-file":
+                if index + 1 >= len(argv):
+                    raise ValueError("--params-file requires a path argument")
+                params_files.append(argv[index + 1])
+                index += 2
+                continue
+            index += 1
+        return params_files
+
+    spawner_ros_params_files = get_ros_params_files(sys.argv[1:])
+
+    # Remove ROS args
+>>>>>>> a583699 ([Spawner] Allow parsing the parameter files parsed from spawner to controllers (#3136))
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
     controller_names = args.controller_names
@@ -171,11 +220,30 @@ def main(args=None):
     switch_asap = args.switch_asap
     node = None
 
+<<<<<<< HEAD
     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)
     logger = rclpy.logging.get_logger("ros2_control_controller_spawner_" + controller_names[0])
+=======
+    if spawner_ros_params_files:
+        for controller in controllers:
+            controller["param_files"] = append_unique_preserve_order(
+                controller["param_files"], spawner_ros_params_files
+            )
+
+    # Check param files existence
+    for c in controllers:
+        if c["param_files"]:
+            for param_file in c["param_files"]:
+                if not os.path.isfile(param_file):
+                    raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
+
+    # Use the first controller name for the logger/lock
+    first_controller_name = controllers[0]["name"]
+    logger = rclpy.logging.get_logger("ros2_control_controller_spawner_" + first_controller_name)
+>>>>>>> a583699 ([Spawner] Allow parsing the parameter files parsed from spawner to controllers (#3136))
 
     try:
         spawner_node_name = "spawner_" + controller_names[0]

controller_manager/doc/userdoc.rst

@@ -194,6 +194,98 @@ There are two scripts to interact with controller manager from launch files:
       --controller-ros-args CONTROLLER_ROS_ARGS
                             The --ros-args to be passed to the controller node, e.g., for remapping topics. Pass multiple times for every argument.
 
+<<<<<<< HEAD
+=======
+    When launching ``spawner`` with ROS parameter files that use substitutions (for example, launch ``allow_substs=True``),
+    the resolved ``--params-file`` path(s) used by the spawner node are automatically forwarded to each controller along
+    with any explicit ``--param-file`` arguments passed to the spawner command.
+
+.. note::
+  If a single parameter file is used for multiple controllers, the spawner will automatically forward the resolved path(s) to each controller. The following methods are recommended:
+
+  .. code-block:: console
+
+    Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "my_controller",
+            "--param-file",
+            PathSubstitution(FindPackageShare("my_config_pkg"))
+            / "config"
+            / "controllers.yaml",
+        ],
+    ),
+
+  (or)
+
+  .. code-block:: console
+
+    Node(
+        package="controller_manager",
+        executable="spawner",
+        parameters=[
+            ParameterFile(
+                PathSubstitution(FindPackageShare("my_config_pkg"))
+                / "config"
+                / "controllers.yaml",
+            ),
+        ],
+        arguments=[
+            "my_controller"
+        ],
+    ),
+
+The ``spawner`` now supports per controller arguments, while parsing the arguments for multiple controllers using ``--controller`` option. For example, to spawn two controllers with different parameter files and remapping topics, the following command can be used:
+
+.. code-block:: console
+
+    $ ros2 run controller_manager spawner --controller position_trajectory_controller \
+        --param-file /path/to/position_trajectory_controller_params.yaml \
+        --controller-ros-args "--ros-args --remap /joint_states:=/rrbot/joint_states" \
+        --controller velocity_trajectory_controller \
+        --param-file /path/to/velocity_trajectory_controller_params.yaml \
+        --controller-ros-args "--ros-args --remap /joint_states:=/rrbot/joint_states"
+
+.. code-block:: console
+
+    $ ros2 run  controller_manager spawner --controller -h
+    Usage: spawner [global_options] --controller <name> [controller_options] --controller <name> ...
+
+    Global Options:
+    usage: spawner [-c CONTROLLER_MANAGER] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group]
+                  [--switch-asap | --no-switch-asap] [-u] [-h]
+
+    options:
+      -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                            Name of the controller manager
+      --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
+                            Timeout for controller manager services
+      --switch-timeout SWITCH_TIMEOUT
+                            Timeout for switch controller service
+      --service-call-timeout SERVICE_CALL_TIMEOUT
+                            Timeout for service calls
+      --activate-as-group   Activate controllers as a group
+      --switch-asap, --no-switch-asap
+                            Switch controllers as soon as possible
+      -u, --unload-on-kill  Deactivate the active controllers and unload them on kill
+      -h, --help            Show help
+
+    Controller Options:
+    usage: spawner [-p PARAM_FILE] [--load-only] [--inactive] [--controller-ros-args CONTROLLER_ROS_ARGS] controller_name
+
+    positional arguments:
+      controller_name       Name of the controller
+
+    options:
+      -p PARAM_FILE, --param-file PARAM_FILE
+                            Parameter files to load for the controller
+      --load-only           Load the controller but do not configure/activate it
+      --inactive            Configure the controller but do not switch it
+      --controller-ros-args CONTROLLER_ROS_ARGS
+                            ROS arguments to pass to the controller
+
+>>>>>>> a583699 ([Spawner] Allow parsing the parameter files parsed from spawner to controllers (#3136))
 
 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: