diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index e2681e5471..239c860ecf 100644
--- a/controller_manager/CMakeLists.txt
+++ b/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
)
diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py
new file mode 100644
index 0000000000..80d3726e25
--- /dev/null
+++ b/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',
+]
diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py
new file mode 100644
index 0000000000..4268b376b7
--- /dev/null
+++ b/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,
+ ])
diff --git a/controller_manager/package.xml b/controller_manager/package.xml
index b6cbce19d4..e2f90bbbb9 100644
--- a/controller_manager/package.xml
+++ b/controller_manager/package.xml
@@ -13,9 +13,15 @@
controller_interface
controller_manager_msgs
hardware_interface
+ launch
+ launch_ros
pluginlib
rclcpp
rcpputils
+ ros2param
+ ros2run
+
+ ros2controlcli
ament_cmake_gmock
ament_cmake_gtest
diff --git a/controller_manager/scripts/spawner.py b/controller_manager/scripts/spawner.py
new file mode 100644
index 0000000000..37b90cb4de
--- /dev/null
+++ b/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())
diff --git a/controller_manager/scripts/unspawner.py b/controller_manager/scripts/unspawner.py
new file mode 100644
index 0000000000..37fe6eb5e0
--- /dev/null
+++ b/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()
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index b6cc1f648f..7167f83aa7 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -773,7 +773,7 @@ void ControllerManager::load_controller_service_cb(
std::lock_guard 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());
diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp
new file mode 100644
index 0000000000..465b94de5a
--- /dev/null
+++ b/controller_manager/test/test_spawner_unspawner.cpp
@@ -0,0 +1,149 @@
+// 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.
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include "controller_manager_test_common.hpp"
+#include "controller_interface/controller_interface.hpp"
+#include "controller_manager/controller_manager.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+
+using ::testing::_;
+using ::testing::Return;
+
+using namespace std::chrono_literals;
+class TestLoadController : public ControllerManagerFixture
+{
+ void SetUp() override
+ {
+ ControllerManagerFixture::SetUp();
+
+ update_timer_ = cm_->create_wall_timer(
+ std::chrono::milliseconds(10), [&]() {
+ cm_->read();
+ cm_->update();
+ cm_->write();
+ }
+ );
+
+ update_executor_ = std::make_shared(
+ rclcpp::ExecutorOptions(), 2);
+
+ update_executor_->add_node(cm_);
+ update_executor_spin_future_ = std::async(
+ std::launch::async, [this]() -> void {
+ update_executor_->spin();
+ });
+ // This sleep is needed to prevent a too fast test from ending before the
+ // executor has began to spin, which causes it to hang
+ std::this_thread::sleep_for(50ms);
+ }
+
+ void TearDown() override
+ {
+ update_executor_->cancel();
+ }
+
+protected:
+ rclcpp::TimerBase::SharedPtr update_timer_;
+
+ // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
+ std::shared_ptr update_executor_;
+ std::future update_executor_spin_future_;
+};
+
+int call_spawner(const std::string extra_args)
+{
+ std::string spawner_script = "ros2 run controller_manager spawner.py ";
+ return std::system((spawner_script + extra_args).c_str());
+}
+
+int call_unspawner(const std::string extra_args)
+{
+ std::string spawner_script = "ros2 run controller_manager unspawner.py ";
+ return std::system((spawner_script + extra_args).c_str());
+}
+
+TEST_F(TestLoadController, spawner_test_type_in_param)
+{
+ EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments";
+ EXPECT_NE(call_spawner("ctrl_1"), 0) << "Wrong controller manager name";
+ EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Missing .type parameter";
+
+ cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+ EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+ auto ctrl_1 = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+ ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(ctrl_1.c->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+ // Try to spawn again, it should fail because already active
+ EXPECT_NE(call_spawner("ctrl_1 -c test_controller_manager"), 0) << "Cannot configure from active";
+ ctrl_1.c->deactivate();
+ // We should be able to reconfigure and start a configured controller
+ EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0);
+ ctrl_1.c->cleanup();
+ // We should be able to reconfigure and start am unconfigured loaded controller
+ EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0);
+ ASSERT_EQ(ctrl_1.c->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+
+ // Unload and reload
+ EXPECT_EQ(call_unspawner("ctrl_1 -c test_controller_manager"), 0);
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul) << "Controller should have been unloaded";
+ EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0);
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul) << "Controller should have been loaded";
+ ctrl_1 = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_1.c->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+}
+
+TEST_F(TestLoadController, spawner_test_type_in_arg)
+{
+ // Provide controller type via -t argument
+ EXPECT_EQ(
+ call_spawner(
+ "ctrl_2 -c test_controller_manager -t " +
+ std::string(test_controller::TEST_CONTROLLER_CLASS_NAME)), 0);
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+ auto ctrl_2 = cm_->get_loaded_controllers()[0];
+ ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
+ ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+ ASSERT_EQ(ctrl_2.c->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+}
+
+TEST_F(TestLoadController, unload_on_kill)
+{
+ // Launch spawner.py with unload on kill
+ // timeout command will kill it after the specified time with signal SIGINT
+ std::stringstream ss;
+ ss << "timeout --signal=INT 5 " <<
+ "ros2 run controller_manager spawner.py " <<
+ "ctrl_3 -c test_controller_manager -t " <<
+ std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) <<
+ " --unload-on-kill";
+
+ EXPECT_NE(
+ std::system(ss.str().c_str()),
+ 0) << "timeout should have killed spawner and returned non 0 code";
+
+ ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
+}