Remodel ros2controlcli

.github/workflows/ci.yml

@@ -40,6 +40,7 @@ jobs:
             controller_manager
             controller_manager_msgs
             hardware_interface
+            ros2controlcli
             ros2_control
             ros2_control_test_assets
           vcs-repo-file-url: |

.github/workflows/lint.yml

@@ -22,6 +22,7 @@ jobs:
           controller_manager
           controller_manager_msgs
           hardware_interface
+          ros2controlcli
           ros2_control
           ros2_control_test_assets
           transmission_interface

controller_manager/CMakeLists.txt

@@ -132,12 +132,12 @@ if(BUILD_TESTING)
   target_include_directories(test_release_interfaces PRIVATE include)
   target_link_libraries(test_release_interfaces controller_manager test_controller_with_interfaces)
 
-  # ament_add_gmock(
-  #   test_spawner_unspawner
-  #   test/test_spawner_unspawner.cpp
-  # )
-  # target_include_directories(test_spawner_unspawner PRIVATE include)
-  # target_link_libraries(test_spawner_unspawner controller_manager test_controller)
+  ament_add_gmock(
+    test_spawner_unspawner
+    test/test_spawner_unspawner.cpp
+  )
+  target_include_directories(test_spawner_unspawner PRIVATE include)
+  target_link_libraries(test_spawner_unspawner controller_manager test_controller)
 endif()
 
 # Install Python modules

controller_manager/controller_manager/__init__.py

@@ -12,8 +12,17 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from . import launch_utils
+from .controller_manager_services import configure_controller, \
+        list_controller_types, list_controllers, list_hardware_interfaces, \
+        load_controller, reload_controller_libraries, switch_controllers, unload_controller
 
 __all__ = [
-    'launch_utils',
+    'configure_controller',
+    'list_controller_types',
+    'list_controllers',
+    'list_hardware_interfaces',
+    'load_controller',
+    'reload_controller_libraries',
+    'switch_controllers',
+    'unload_controller',
 ]

controller_manager/controller_manager/controller_manager_services.py

@@ -0,0 +1,100 @@
+# Copyright (c) 2021 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 ConfigureController, \
+    ListControllers, ListControllerTypes, ListHardwareInterfaces, \
+    LoadController, ReloadControllerLibraries, SwitchController, UnloadController
+
+import rclpy
+
+
+def service_caller(node, service_name, service_type, request):
+    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(2.0):
+            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())
+
+
+def configure_controller(node, controller_manager_name, controller_name):
+    request = ConfigureController.Request()
+    request.name = controller_name
+    return service_caller(node, '{}/configure_controller'.format(controller_manager_name),
+                          ConfigureController, request)
+
+
+def list_controllers(node, controller_manager_name):
+    request = ListControllers.Request()
+    return service_caller(node, '{}/list_controllers'.format(controller_manager_name),
+                          ListControllers, request)
+
+
+def list_controller_types(node, controller_manager_name):
+    request = ListControllerTypes.Request()
+    return service_caller(node,
+                          '{}/list_controller_types'.format(controller_manager_name),
+                          ListControllerTypes, request)
+
+
+def list_hardware_interfaces(node, controller_manager_name):
+    request = ListHardwareInterfaces.Request()
+    return service_caller(node, '{}/list_hardware_interfaces'.format(controller_manager_name),
+                          ListHardwareInterfaces, request)
+
+
+def load_controller(node, controller_manager_name, controller_name):
+    request = LoadController.Request()
+    request.name = controller_name
+    return service_caller(node, '{}/load_controller'.format(controller_manager_name),
+                          LoadController, request)
+
+
+def reload_controller_libraries(node, controller_manager_name, force_kill):
+    request = ReloadControllerLibraries.Request()
+    request.force_kill = force_kill
+    return service_caller(node,
+                          '{}/reload_controller_libraries'.format(controller_manager_name),
+                          ReloadControllerLibraries, request)
+
+
+def switch_controllers(node, 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(node, '{}/switch_controller'.format(controller_manager_name),
+                          SwitchController, request)
+
+
+def unload_controller(node, controller_manager_name, controller_name):
+    request = UnloadController.Request()
+    request.name = controller_name
+    return service_caller(node, '{}/unload_controller'.format(controller_manager_name),
+                          UnloadController, request)

controller_manager/controller_manager/launch_utils.py

@@ -29,7 +29,7 @@ def generate_load_controller_launch_description(controller_name,
     'unload_on_kill' LaunchArguments and a Node action that runs the controller_manager
     spawner.py node to load and start a controller
 
-    Examples
+    Examples # noqa: D416
     --------
       # Assuming the controller type and controller parameters are known to the controller_manager
       generate_load_controller_launch_description('joint_state_controller')

controller_manager/package.xml

@@ -21,8 +21,6 @@
   <depend>ros2param</depend>
   <depend>ros2run</depend>
 
-  <exec_depend>ros2controlcli</exec_depend>
-
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

controller_manager/scripts/spawner.py

@@ -18,22 +18,42 @@
 import sys
 import time
 
+from controller_manager import configure_controller, list_controllers, \
+        load_controller, switch_controllers, unload_controller
+
 import rclpy
 from rclpy.duration import Duration
 from rclpy.node import Node
 
 
-def is_controller_loaded(controller_manager_name, controller_name):
-    ret = subprocess.run(['ros2', 'control', 'list_controllers',
-                          '--controller-manager', controller_manager_name], capture_output=True,
-                         encoding='utf8')
-    output = str(ret.stdout)
-    for line in output.splitlines():
-        if controller_name in line.split('[')[0]:
+def wait_for_controller_manager(node, controller_manager):
+    def full_name(n):
+        return n[1] + ('' if n[1].endswith('/') else '/') + n[0]
+
+    # Wait for controller_manager
+    timeout = node.get_clock().now() + Duration(seconds=10)
+    while node.get_clock().now() < timeout:
+        node_names_and_namespaces = node.get_node_names_and_namespaces()
+        if any(full_name(n) == controller_manager for n in node_names_and_namespaces):
             return True
+
+        node.get_logger().info(
+            'Waiting for {} services'.format(controller_manager),
+            throttle_duration_sec=2)
+        time.sleep(0.2)
+
     return False
 
 
+def is_controller_loaded(node, controller_manager, controller_name):
+    controllers = list_controllers(node, controller_manager).controller
+    return any(c.name == controller_name for c in controllers)
+
+
+def make_absolute(name):
+    return name if name.startswith('/') else ('/' + name)
+
+
 def main(args=None):
 
     rclpy.init(args=args)
@@ -59,39 +79,26 @@ def main(args=None):
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
     controller_name = args.controller_name
-    controller_manager_name = args.controller_manager
+    controller_manager_name = make_absolute(args.controller_manager)
     param_file = args.param_file
     controller_type = args.controller_type
 
     node = Node('spawner_' + controller_name)
     try:
-        # Wait for controller_manager
-        timeout = node.get_clock().now() + Duration(seconds=10)
-        while node.get_clock().now() < timeout:
-            ret = subprocess.run(
-                ['ros2', 'service', 'type',
-                 '/' + controller_manager_name + '/load_and_start_controller'],
-                stdout=subprocess.DEVNULL,
-                stderr=subprocess.DEVNULL)
-            if ret.returncode == 0:
-                break
-            node.get_logger().info(
-                'Waiting for {} services'.format(controller_manager_name),
-                throttle_duration_sec=2)
-            time.sleep(0.2)
-
-        if is_controller_loaded(controller_manager_name, controller_name):
+        if not wait_for_controller_manager(node, controller_manager_name):
+            node.get_logger().error('Controller manager not available')
+            return 1
+
+        if is_controller_loaded(node, controller_manager_name, controller_name):
             node.get_logger().info('Controller already loaded, skipping load_controller')
         else:
             if controller_type:
                 ret = subprocess.run(['ros2', 'param', 'set', controller_manager_name,
                                       controller_name + '.type', controller_type])
-
-            ret = subprocess.run(['ros2', 'control', 'load_controller', controller_name,
-                                  '--controller-manager', controller_manager_name])
-            if ret.returncode != 0:
+            ret = load_controller(node, controller_manager_name, controller_name)
+            if not ret.ok:
                 # Error message printed by ros2 control
-                return ret.returncode
+                return 1
             node.get_logger().info('Loaded ' + controller_name)
 
         if param_file:
@@ -102,11 +109,23 @@ def main(args=None):
                 return ret.returncode
             node.get_logger().info('Loaded ' + param_file + ' into ' + controller_name)
 
-        ret = subprocess.run(['ros2', 'control', 'configure_start_controller', controller_name,
-                              '--controller-manager', controller_manager_name])
-        if ret.returncode != 0:
-            # Error message printed by ros2 control
-            return ret.returncode
+        ret = configure_controller(node, controller_manager_name, controller_name)
+        if not ret.ok:
+            node.get_logger().info('Failed to configure controller')
+            return 1
+
+        ret = switch_controllers(
+            node,
+            controller_manager_name,
+            [],
+            [controller_name],
+            True,
+            True,
+            5.0)
+        if not ret.ok:
+            node.get_logger().info('Failed to start controller')
+            return 1
+
         node.get_logger().info('Configured and started ' + controller_name)
 
         if not args.unload_on_kill:
@@ -117,18 +136,26 @@ def main(args=None):
                 time.sleep(1)
         except KeyboardInterrupt:
             node.get_logger().info('Interrupt captured, stopping and unloading controller')
-            ret = subprocess.run(['ros2', 'control', 'switch_controllers', '--stop-controllers',
-                                  controller_name,
-                                  '--controller-manager', controller_manager_name])
+            ret = switch_controllers(
+                node,
+                controller_manager_name,
+                [controller_name],
+                [],
+                True,
+                True,
+                5.0)
+            if not ret.ok:
+                node.get_logger().info('Failed to stop controller')
+                return 1
+
             node.get_logger().info('Stopped controller')
 
-            # Ignore returncode, because message is already printed and we'll try to unload anyway
-            ret = subprocess.run(['ros2', 'control', 'unload_controller', controller_name,
-                                  '--controller-manager', controller_manager_name])
-            if ret.returncode != 0:
-                return ret.returncode
-            else:
-                node.get_logger().info('Unloaded controller')
+            ret = unload_controller(node, controller_manager_name, controller_name)
+            if not ret.ok:
+                node.get_logger().info('Failed to unload controller')
+                return 1
+
+            node.get_logger().info('Unloaded controller')
         return 0
     finally:
         rclpy.shutdown()

controller_manager/scripts/unspawner.py

@@ -15,9 +15,10 @@
 
 
 import argparse
-import subprocess
 import sys
 
+from controller_manager import switch_controllers, unload_controller
+
 import rclpy
 from rclpy.node import Node
 
@@ -40,16 +41,23 @@ def main(args=None):
     node = Node('unspawner_' + controller_name)
     try:
         # Ignore returncode, because message is already printed and we'll try to unload anyway
-        ret = subprocess.run(['ros2', 'control', 'switch_controllers', '--stop-controllers',
-                              controller_name, '--controller-manager', controller_manager_name])
+        ret = switch_controllers(
+            node,
+            controller_manager_name,
+            [controller_name],
+            [],
+            True,
+            True,
+            5.0)
         node.get_logger().info('Stopped controller')
 
-        ret = subprocess.run(['ros2', 'control', 'unload_controller', controller_name,
-                              '--controller-manager', controller_manager_name])
-        if ret.returncode != 0:
-            return ret.returncode
-        else:
-            node.get_logger().info('Unloaded controller')
+        ret = unload_controller(node, controller_manager_name, controller_name)
+        if not ret.ok:
+            node.get_logger().info('Failed to unload controller')
+            return 1
+        node.get_logger().info('Unloaded controller')
+
+        return 0
     finally:
         rclpy.shutdown()
 

ros2controlcli/package.xml

@@ -13,6 +13,7 @@
   <depend>ros2cli</depend>
   <depend>ros2node</depend>
   <depend>ros2param</depend>
+  <depend>controller_manager</depend>
   <depend>controller_manager_msgs</depend>
   <exec_depend>rosidl_runtime_py</exec_depend>