Add cli interface

ros2controlcli/README.md

@@ -0,0 +1,146 @@
+# ros2controlcli
+Command line interface for controller manager
+
+## Verbs 
+
+
+### list
+
+```bash
+$ ros2 control list -h
+usage: ros2 control list [-h] [--spin-time SPIN_TIME] [--no-daemon] [-c CONTROLLER_MANAGER] [--include-hidden-nodes]
+
+Output the list of loaded controllers, their type and status
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --spin-time SPIN_TIME
+                        Spin time in seconds to wait for discovery (only applies when not using an already running daemon)
+  -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                        Name of the controller manager ROS node
+  --include-hidden-nodes
+                        Consider hidden nodes as well
+```
+
+```bash
+$ ros2 control list 
+test_controller_name[test_controller]    active 
+```
+
+### list_types
+
+```bash
+$ ros2 control list_types -h
+usage: ros2 control list_types [-h] [--spin-time SPIN_TIME] [--no-daemon] [-c CONTROLLER_MANAGER] [--include-hidden-nodes]
+
+Output the available controller types and their base classes
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --spin-time SPIN_TIME
+                        Spin time in seconds to wait for discovery (only applies when not using an already running daemon)
+  -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                        Name of the controller manager ROS node
+  --include-hidden-nodes
+                        Consider hidden nodes as well
+```
+```bash
+$ ros2 control list_types 
+diff_drive_controller/DiffDriveController                              controller_interface::ControllerInterface
+joint_state_controller/JointStateController                            controller_interface::ControllerInterface
+joint_trajectory_controller/JointTrajectoryController                  controller_interface::ControllerInterface
+test_controller      
+```
+
+### load
+
+```bash
+$ ros2 control load -h
+usage: ros2 control load [-h] [--spin-time SPIN_TIME] [--no-daemon] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name
+
+Load a controller in a controller manager
+
+positional arguments:
+  controller_name       Name of the controller
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --spin-time SPIN_TIME
+                        Spin time in seconds to wait for discovery (only applies when not using an already running daemon)
+  -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                        Name of the controller manager ROS node
+  --include-hidden-nodes
+                        Consider hidden nodes as well
+```
+
+### reload_libraries
+
+```bash
+$ ros2 control reload_libraries -h
+usage: ros2 control reload_libraries [-h] [--spin-time SPIN_TIME] [--force-kill] [-c CONTROLLER_MANAGER] [--include-hidden-nodes]
+
+Reload controller libraries
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --spin-time SPIN_TIME
+                        Spin time in seconds to wait for discovery (only applies when not using an already running daemon)
+  --force-kill          Force stop of loaded controllers
+  -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                        Name of the controller manager ROS node
+  --include-hidden-nodes
+                        Consider hidden nodes as well
+```
+
+### switch
+
+```bash
+$ ros2 control switch -h
+usage: ros2 control switch [-h] [--spin-time SPIN_TIME] [--no-daemon] [--stop-controllers [STOP_CONTROLLERS [STOP_CONTROLLERS ...]]]
+                           [--start-controllers [START_CONTROLLERS [START_CONTROLLERS ...]]] [--strict] [--start-asap]
+                           [--switch-timeout SWITCH_TIMEOUT] [-c CONTROLLER_MANAGER] [--include-hidden-nodes]
+
+Switch controllers in a controller manager
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --spin-time SPIN_TIME
+                        Spin time in seconds to wait for discovery (only applies when not using an already running daemon)
+  --stop-controllers [STOP_CONTROLLERS [STOP_CONTROLLERS ...]]
+                        Name of the controllers to be stopped
+  --start-controllers [START_CONTROLLERS [START_CONTROLLERS ...]]
+                        Name of the controllers to be started
+  --strict              Strict switch
+  --start-asap          Start asap controllers
+  --switch-timeout SWITCH_TIMEOUT
+                        Timeout for switching controllers
+  -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                        Name of the controller manager ROS node
+  --include-hidden-nodes
+                        Consider hidden nodes as well
+```
+
+
+
+
+
+### unload
+
+```bash
+$ ros2 control unload -h
+usage: ros2 control unload [-h] [--spin-time SPIN_TIME] [--no-daemon] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name
+
+Unload a controller in a controller manager
+
+positional arguments:
+  controller_name       Name of the controller
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --spin-time SPIN_TIME
+                        Spin time in seconds to wait for discovery (only applies when not using an already running daemon)
+  -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
+                        Name of the controller manager ROS node
+  --include-hidden-nodes
+                        Consider hidden nodes as well
+```

ros2controlcli/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>ros2controlcli</name>
+  <version>0.0.0</version>
+  <description>
+    The ROS 2 command line tools for ROS2 Control.
+  </description>
+  <maintainer email="victor.lopez@pal-robotics.com">Victor Lopez</maintainer>
+  <license>Apache License 2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>ros2cli</depend>
+  <depend>ros2node</depend>
+  <depend>ros2param</depend>
+  <depend>controller_manager_msgs</depend>
+  <exec_depend>rosidl_runtime_py</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>ament_xmllint</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros2controlcli/ros2controlcli/api/__init__.py

@@ -0,0 +1,138 @@
+# Copyright 2020 PAL Robotics S.L.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+from controller_manager_msgs.srv import ListControllers, ListControllerTypes, \
+    LoadController, ReloadControllerLibraries, SwitchController, UnloadController
+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(
+            'ros2controlcli_{}_requester'.format(
+                service_name.replace(
+                    '/', '')))
+
+        cli = node.create_client(service_type, service_name)
+
+        if not cli.service_is_ready():
+            node.get_logger().debug('waiting for service {} to become available...'
+                                    .format(service_name))
+            if not cli.wait_for_service(0.2):
+                raise RuntimeError('Could not contact service {}'.format(service_name))
+
+        node.get_logger().debug('requester: making request: %r\n' % request)
+        future = cli.call_async(request)
+        rclpy.spin_until_future_complete(node, future)
+        if future.result() is not None:
+            return future.result()
+        else:
+            raise RuntimeError('Exception while calling service: %r' % future.exception())
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+def list_controllers(controller_manager_name):
+    request = ListControllers.Request()
+    return service_caller('{}/list_controllers'.format(controller_manager_name),
+                          ListControllers, request)
+
+
+def list_controller_types(controller_manager_name):
+    request = ListControllerTypes.Request()
+    return service_caller(
+        '{}/list_controller_types'.format(controller_manager_name), ListControllerTypes, request)
+
+
+def reload_controller_libraries(controller_manager_name, force_kill):
+    request = ReloadControllerLibraries.Request()
+    request.force_kill = force_kill
+    return service_caller(
+        '{}/reload_controller_libraries'.format(controller_manager_name),
+        ReloadControllerLibraries,
+        request)
+
+
+def load_controller(controller_manager_name, controller_name):
+    request = LoadController.Request()
+    request.name = controller_name
+    return service_caller('{}/load_controller'.format(controller_manager_name),
+                          LoadController, request)
+
+
+def switch_controllers(controller_manager_name, stop_controllers,
+                       start_controllers, strict, start_asap, timeout):
+    request = SwitchController.Request()
+    request.start_controllers = start_controllers
+    request.stop_controllers = stop_controllers
+    if strict:
+        request.strictness = SwitchController.Request.STRICT
+    else:
+        request.strictness = SwitchController.Request.BEST_EFFORT
+    request.start_asap = start_asap
+    request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg()
+    return service_caller('{}/switch_controller'.format(controller_manager_name),
+                          SwitchController, request)
+
+
+def unload_controller(controller_manager_name, controller_name):
+    request = UnloadController.Request()
+    request.name = controller_name
+    return service_caller('{}/unload_controller'.format(controller_manager_name),
+                          UnloadController, request)
+
+
+class ControllerNameCompleter:
+    """Callable returning a list of controllers parameter names."""
+
+    def __call__(self, prefix, parsed_args, **kwargs):
+        with DirectNode(parsed_args) as node:
+            parameter_names = call_list_parameters(
+                node=node, node_name=parsed_args.controller_manager)
+            suffix = '.type'
+            return [
+                n[:-len(suffix)] for n in parameter_names
+                if n.endswith(suffix)]
+
+
+class LoadedControllerNameCompleter:
+    """Callable returning a list of loaded controllers."""
+
+    def __init__(self, valid_states=['active', 'inactive', 'configured', 'unconfigured']):
+        self.valid_states = valid_states
+
+    def __call__(self, prefix, parsed_args, **kwargs):
+        controllers = list_controllers(parsed_args.controller_manager).controller
+        return [
+            c.name for c in controllers
+            if c.state in self.valid_states]
+
+
+def add_controller_mgr_parsers(parser):
+    """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', 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')

ros2controlcli/ros2controlcli/command/control.py

@@ -0,0 +1,38 @@
+# Copyright 2020 PAL Robotics S.L.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+from ros2cli.command import add_subparsers_on_demand
+from ros2cli.command import CommandExtension
+
+
+class ControlCommand(CommandExtension):
+    """Various control related sub-commands."""
+
+    def add_arguments(self, parser, cli_name):
+        self._subparser = parser
+        # get verb extensions and let them add their arguments
+        add_subparsers_on_demand(
+            parser, cli_name, '_verb', 'ros2controlcli.verb', required=False)
+
+    def main(self, *, parser, args):
+        if not hasattr(args, '_verb'):
+            # in case no verb was passed
+            self._subparser.print_help()
+            return 0
+
+        extension = getattr(args, '_verb')
+
+        # call the verb's main method
+        return extension.main(args=args)

ros2controlcli/ros2controlcli/verb/list.py

@@ -0,0 +1,30 @@
+# Copyright 2020 PAL Robotics S.L.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ros2cli.node.direct import add_arguments
+from ros2cli.verb import VerbExtension
+from ros2controlcli.api import add_controller_mgr_parsers, list_controllers
+
+
+class ListVerb(VerbExtension):
+    """Output the list of loaded controllers, their type and status."""
+
+    def add_arguments(self, parser, cli_name):
+        add_arguments(parser)
+        add_controller_mgr_parsers(parser)
+
+    def main(self, *, args):
+        controllers = list_controllers(args.controller_manager).controller
+        for c in controllers:
+            print('{:20s}{:20s} {:10s}'.format(c.name, '[' + c.type + ']', c.state))