[ros2controlcli] add params file parsing to load_controller verb and add namespacing support (#1703)

Author: saikishor

controller_manager/controller_manager/controller_manager_services.py

@@ -88,15 +88,23 @@ def service_caller(
     @return The service response
 
     """
-    cli = node.create_client(service_type, service_name)
+    namespace = "" if node.get_namespace() == "/" else node.get_namespace()
+    fully_qualified_service_name = (
+        f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name
+    )
+    cli = node.create_client(service_type, fully_qualified_service_name)
 
     while not cli.service_is_ready():
-        node.get_logger().info(f"waiting for service {service_name} to become available...")
+        node.get_logger().info(
+            f"waiting for service {fully_qualified_service_name} to become available..."
+        )
         if service_timeout:
             if not cli.wait_for_service(service_timeout):
-                raise ServiceNotFoundError(f"Could not contact service {service_name}")
+                raise ServiceNotFoundError(
+                    f"Could not contact service {fully_qualified_service_name}"
+                )
         elif not cli.wait_for_service(10.0):
-            node.get_logger().warn(f"Could not contact service {service_name}")
+            node.get_logger().warn(f"Could not contact service {fully_qualified_service_name}")
 
     node.get_logger().debug(f"requester: making request: {request}\n")
     future = None
@@ -105,13 +113,13 @@ def service_caller(
         rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout)
         if future.result() is None:
             node.get_logger().warning(
-                f"Failed getting a result from calling {service_name} in "
+                f"Failed getting a result from calling {fully_qualified_service_name} in "
                 f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)"
             )
         else:
             return future.result()
     raise RuntimeError(
-        f"Could not successfully call service {service_name} after {max_attempts} attempts."
+        f"Could not successfully call service {fully_qualified_service_name} after {max_attempts} attempts."
     )
 
 

doc/release_notes.rst

@@ -118,3 +118,15 @@ ros2controlcli
   .. code-block:: bash
 
     ros2 control set_hardware_component_state <hardware_component_name> <state>
+
+* The ``load_controller`` now supports parsing of the params file (`#1703 <https://github.com/ros-controls/ros2_control/pull/1703>`_).
+
+  .. code-block:: bash
+
+    ros2 control load_controller <controller_name> <realtive_or_absolute_file_path>
+
+* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 <https://github.com/ros-controls/ros2_control/pull/1703>`_).
+
+  .. code-block:: bash
+
+    ros2 control <verb> <arguments> --ros-args -r __ns:=<namespace>

ros2controlcli/doc/userdoc.rst

@@ -15,40 +15,13 @@
 
 from controller_manager import list_controllers, list_hardware_components
 
-import rclpy
-
 from ros2cli.node.direct import DirectNode
 
 from ros2node.api import NodeNameCompleter
 
 from ros2param.api import call_list_parameters
 
-
-def service_caller(service_name, service_type, request):
-    try:
-        rclpy.init()
-
-        node = rclpy.create_node(f"ros2controlcli_{service_name.replace('/', '')}_requester")
-
-        cli = node.create_client(service_type, service_name)
-
-        if not cli.service_is_ready():
-            node.get_logger().debug(f"waiting for service {service_name} to become available...")
-
-            if not cli.wait_for_service(2.0):
-                raise RuntimeError(f"Could not contact service {service_name}")
-
-        node.get_logger().debug(f"requester: making request: {repr(request)}\n")
-        future = cli.call_async(request)
-        rclpy.spin_until_future_complete(node, future)
-        if future.result() is not None:
-            return future.result()
-        else:
-            future_exception = future.exception()
-            raise RuntimeError(f"Exception while calling service: {repr(future_exception)}")
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
+import argparse
 
 
 class ControllerNameCompleter:
@@ -89,16 +62,28 @@ def __call__(self, prefix, parsed_args, **kwargs):
             return [c.name for c in hardware_components if c.state.label in self.valid_states]
 
 
+class ParserROSArgs(argparse.Action):
+    def __call__(self, parser, namespace, values, option_string=None):
+        values = [option_string] + values
+        setattr(namespace, "argv", values)
+
+
 def add_controller_mgr_parsers(parser):
-    """Parser arguments to get controller manager node name, defaults to /controller_manager."""
+    """Parser arguments to get controller manager node name, defaults to controller_manager."""
     arg = parser.add_argument(
         "-c",
         "--controller-manager",
-        help="Name of the controller manager ROS node",
-        default="/controller_manager",
+        help="Name of the controller manager ROS node (default: controller_manager)",
+        default="controller_manager",
         required=False,
     )
     arg.completer = NodeNameCompleter(include_hidden_nodes_key="include_hidden_nodes")
     parser.add_argument(
         "--include-hidden-nodes", action="store_true", help="Consider hidden nodes as well"
     )
+    parser.add_argument(
+        "--ros-args",
+        nargs=argparse.REMAINDER,
+        help="Pass arbitrary arguments to the executable",
+        action=ParserROSArgs,
+    )

ros2controlcli/ros2controlcli/api/__init__.py

@@ -29,7 +29,7 @@ def add_arguments(self, parser, cli_name):
         add_controller_mgr_parsers(parser)
 
     def main(self, *, args):
-        with NodeStrategy(args) as node:
+        with NodeStrategy(args).direct_node as node:
             response = list_controller_types(node, args.controller_manager)
             types_and_classes = zip(response.types, response.base_classes)
             for c in types_and_classes:

ros2controlcli/ros2controlcli/verb/list_controller_types.py

@@ -95,7 +95,7 @@ def add_arguments(self, parser, cli_name):
         add_controller_mgr_parsers(parser)
 
     def main(self, *, args):
-        with NodeStrategy(args) as node:
+        with NodeStrategy(args).direct_node as node:
             response = list_controllers(node, args.controller_manager)
 
             if not response.controller:

ros2controlcli/ros2controlcli/verb/list_controllers.py

@@ -36,7 +36,7 @@ def add_arguments(self, parser, cli_name):
         add_controller_mgr_parsers(parser)
 
     def main(self, *, args):
-        with NodeStrategy(args) as node:
+        with NodeStrategy(args).direct_node as node:
             hardware_components = list_hardware_components(node, args.controller_manager)
 
             for idx, component in enumerate(hardware_components.component):

ros2controlcli/ros2controlcli/verb/list_hardware_components.py

@@ -28,7 +28,7 @@ def add_arguments(self, parser, cli_name):
         add_controller_mgr_parsers(parser)
 
     def main(self, *, args):
-        with NodeStrategy(args) as node:
+        with NodeStrategy(args).direct_node as node:
             hardware_interfaces = list_hardware_interfaces(node, args.controller_manager)
             command_interfaces = sorted(
                 hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name

ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py

@@ -12,13 +12,22 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from controller_manager import configure_controller, load_controller, switch_controllers
+from controller_manager import (
+    configure_controller,
+    load_controller,
+    list_controllers,
+    switch_controllers,
+    set_controller_parameters_from_param_file,
+    bcolors,
+)
 
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy
 from ros2cli.verb import VerbExtension
 
 from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter
+import os
+from argparse import OPTIONAL
 
 
 class LoadControllerVerb(VerbExtension):
@@ -28,37 +37,79 @@ def add_arguments(self, parser, cli_name):
         add_arguments(parser)
         arg = parser.add_argument("controller_name", help="Name of the controller")
         arg.completer = ControllerNameCompleter()
+        arg = parser.add_argument(
+            "param_file",
+            help="The YAML file with the controller parameters",
+            nargs=OPTIONAL,
+            default=None,
+        )
         arg = parser.add_argument(
             "--set-state",
             choices=["inactive", "active"],
             help="Set the state of the loaded controller",
+            default=None,
         )
         add_controller_mgr_parsers(parser)
 
     def main(self, *, args):
-        with NodeStrategy(args) as node:
-            response = load_controller(node, args.controller_manager, args.controller_name)
-            if not response.ok:
-                return "Error loading controller, check controller_manager logs"
+        with NodeStrategy(args).direct_node as node:
+            controllers = list_controllers(node, args.controller_manager, 20.0).controller
+            if any(c.name == args.controller_name for c in controllers):
+                print(
+                    f"{bcolors.WARNING}Controller : {args.controller_name} already loaded, skipping load_controller!{bcolors.ENDC}"
+                )
+            else:
+                if args.param_file:
+                    if not os.path.exists(args.param_file):
+                        print(
+                            f"{bcolors.FAIL}Controller parameter file : {args.param_file} does not exist, Aborting!{bcolors.ENDC}"
+                        )
+                        return 1
+                    if not os.path.isabs(args.param_file):
+                        args.param_file = os.path.join(os.getcwd(), args.param_file)
 
-            if not args.set_state:
-                print(f"Successfully loaded controller {args.controller_name}")
-                return 0
+                    if not set_controller_parameters_from_param_file(
+                        node,
+                        args.controller_manager,
+                        args.controller_name,
+                        args.param_file,
+                        node.get_namespace(),
+                    ):
+                        return 1
 
-            # we in any case configure the controller
-            response = configure_controller(node, args.controller_manager, args.controller_name)
-            if not response.ok:
-                return "Error configuring controller"
+                ret = load_controller(node, args.controller_manager, args.controller_name)
+                if not ret.ok:
+                    print(
+                        f"{bcolors.FAIL}Failed loading controller {args.controller_name} check controller_manager logs{bcolors.ENDC}"
+                    )
+                    return 1
+                print(
+                    f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name}{bcolors.ENDC}"
+                )
 
-            if args.set_state == "active":
-                response = switch_controllers(
-                    node, args.controller_manager, [], [args.controller_name], True, True, 5.0
+            if args.set_state:
+
+                # we in any case configure the controller
+                response = configure_controller(
+                    node, args.controller_manager, args.controller_name
                 )
                 if not response.ok:
-                    return "Error activating controller, check controller_manager logs"
+                    print(
+                        f"{bcolors.FAIL}Error configuring controller : {args.controller_name}{bcolors.ENDC}"
+                    )
+                    return 1
+
+                if args.set_state == "active":
+                    response = switch_controllers(
+                        node, args.controller_manager, [], [args.controller_name], True, True, 5.0
+                    )
+                    if not response.ok:
+                        print(
+                            f"{bcolors.FAIL}Error activating controller : {args.controller_name}, check controller_manager logs{bcolors.ENDC}"
+                        )
+                        return 1
 
-            print(
-                f"Successfully loaded controller {args.controller_name} into "
-                f'state {"inactive" if args.set_state == "inactive" else "active"}'
-            )
-            return 0
+                print(
+                    f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name} into state {args.set_state}{bcolors.ENDC}"
+                )
+                return 0

ros2controlcli/ros2controlcli/verb/load_controller.py

@@ -32,7 +32,7 @@ def add_arguments(self, parser, cli_name):
         add_controller_mgr_parsers(parser)
 
     def main(self, *, args):
-        with NodeStrategy(args) as node:
+        with NodeStrategy(args).direct_node as node:
             response = reload_controller_libraries(
                 node, args.controller_manager, force_kill=args.force_kill
             )