Add spawner and unspawner scripts

controller_manager/CMakeLists.txt

@@ -60,6 +60,12 @@ install(DIRECTORY include/
   DESTINATION include
 )
 
+install(PROGRAMS
+  scripts/spawner.py
+  scripts/unspawner.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
@@ -125,8 +131,18 @@ 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)
 endif()
 
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME})
+
 ament_export_libraries(
   controller_manager
 )

controller_manager/controller_manager/__init__.py

@@ -0,0 +1,19 @@
+# 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 . import launch_utils
+
+__all__ = [
+    'launch_utils',
+]

controller_manager/controller_manager/launch_utils.py

@@ -0,0 +1,81 @@
+# 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 launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PythonExpression
+
+from launch_ros.actions import Node
+
+
+def generate_load_controller_launch_description(controller_name,
+                                                controller_type=None,
+                                                controller_params_file=None):
+    """
+    Generate launch description for loading a controller using spawner.py.
+
+    Returns a list of LaunchDescription actions adding the 'controller_manager_name' and
+    'unload_on_kill' LaunchArguments and a Node action that runs the controller_manager
+    spawner.py node to load and start a controller
+
+    Examples
+    --------
+      # Assuming the controller type and controller parameters are known to the controller_manager
+      generate_load_controller_launch_description('joint_state_controller')
+
+      # Passing controller type and controller parameter file to load
+      generate_load_controller_launch_description(
+        'joint_state_controller',
+        controller_type='joint_state_controller/JointStateController',
+        controller_params_file=os.path.join(get_package_share_directory('my_pkg'),
+                                            'config', 'controller_params.yaml')
+        )
+
+    """
+    declare_controller_mgr_name = DeclareLaunchArgument(
+        'controller_manager_name', default_value='controller_manager',
+        description='Controller manager node name'
+    )
+    declare_unload_on_kill = DeclareLaunchArgument(
+        'unload_on_kill', default_value='false',
+        description='Wait until the node is interrupted and then unload controller'
+    )
+
+    spawner_arguments = [
+        controller_name,
+        '--controller-manager',
+        LaunchConfiguration('controller_manager_name')
+    ]
+
+    if controller_type:
+        spawner_arguments += ['--controller-type', controller_type]
+
+    if controller_params_file:
+        spawner_arguments += ['--param-file', controller_params_file]
+
+    # Setting --unload-on-kill if launch arg unload_on_kill is "true"
+    # See https://github.com/ros2/launch/issues/290
+    spawner_arguments += [PythonExpression(
+        ['"--unload-on-kill"', ' if "true" == "',
+            LaunchConfiguration('unload_on_kill'), '" else ""']
+    )]
+
+    spawner = Node(package='controller_manager', executable='spawner.py',
+                   arguments=spawner_arguments, shell=True, output='screen')
+
+    return LaunchDescription([
+        declare_controller_mgr_name,
+        declare_unload_on_kill,
+        spawner,
+    ])

controller_manager/package.xml

@@ -13,9 +13,15 @@
   <depend>controller_interface</depend>
   <depend>controller_manager_msgs</depend>
   <depend>hardware_interface</depend>
+  <depend>launch</depend>
+  <depend>launch_ros</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rcpputils</depend>
+  <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>

controller_manager/scripts/spawner.py

@@ -0,0 +1,138 @@
+#!/usr/bin/env python3
+# Copyright 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.
+
+import argparse
+import subprocess
+import sys
+import time
+
+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]:
+            return True
+    return False
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        'controller_name', help='Name of the controller')
+    parser.add_argument(
+        '-t', '--controller-type',
+        help='If not provided it should exist in the controller manager namespace',
+        default=None, required=False)
+    parser.add_argument(
+        '-c', '--controller-manager', help='Name of the controller manager ROS node',
+        default='/controller_manager', required=False)
+    parser.add_argument(
+        '-p', '--param-file', type=argparse.FileType('r'),
+        help='Controller param file to be loaded into controller node before configure',
+        required=False)
+    parser.add_argument(
+        '-u', '--unload-on-kill',
+        help='Wait until this application is interrupted and unload controller',
+        action='store_true')
+
+    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
+    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):
+            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:
+                # Error message printed by ros2 control
+                return ret.returncode
+            node.get_logger().info('Loaded ' + controller_name)
+
+        if param_file:
+            ret = subprocess.run(['ros2', 'param', 'load', controller_name,
+                                  param_file])
+            if ret.returncode != 0:
+                # Error message printed by ros2 param
+                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
+        node.get_logger().info('Configured and started ' + controller_name)
+
+        if not args.unload_on_kill:
+            return 0
+        try:
+            node.get_logger().info('Waiting until interrupt to unload controllers')
+            while True:
+                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])
+            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')
+        return 0
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    sys.exit(main())

controller_manager/scripts/unspawner.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+# Copyright 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.
+
+
+import argparse
+import subprocess
+import sys
+
+import rclpy
+from rclpy.node import Node
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        'controller_name', help='Name of the controller')
+    parser.add_argument(
+        '-c', '--controller-manager', help='Name of the controller manager ROS node',
+        default='/controller_manager', required=False)
+
+    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
+
+    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])
+        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')
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

controller_manager/src/controller_manager.cpp

@@ -773,7 +773,7 @@ void ControllerManager::load_controller_service_cb(
   std::lock_guard<std::mutex> guard(services_lock_);
   RCLCPP_DEBUG(get_logger(), "loading service locked");
 
-  response->ok = load_controller(request->name).get();
+  response->ok = load_controller(request->name).get() != nullptr;
 
   RCLCPP_DEBUG(
     get_logger(), "loading service finished for controller '%s' ", request->name.c_str());