diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 1c2ba9b11b..719bbf97e6 100644
--- a/.github/workflows/ci.yml
+++ b/.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: |
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
index 1014e3eab9..02b1ab2478 100644
--- a/.github/workflows/lint.yml
+++ b/.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
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index 3c45729631..239c860ecf 100644
--- a/controller_manager/CMakeLists.txt
+++ b/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
diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py
index 80d3726e25..bfde65470d 100644
--- a/controller_manager/controller_manager/__init__.py
+++ b/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',
]
diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py
new file mode 100644
index 0000000000..1fe84ae821
--- /dev/null
+++ b/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)
diff --git a/controller_manager/controller_manager/launch_utils.py b/controller_manager/controller_manager/launch_utils.py
index 4268b376b7..f33518973f 100644
--- a/controller_manager/controller_manager/launch_utils.py
+++ b/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')
diff --git a/controller_manager/package.xml b/controller_manager/package.xml
index e2f90bbbb9..c3e44b44cb 100644
--- a/controller_manager/package.xml
+++ b/controller_manager/package.xml
@@ -21,8 +21,6 @@
ros2param
ros2run
- ros2controlcli
-
ament_cmake_gmock
ament_cmake_gtest
ament_lint_auto
diff --git a/controller_manager/scripts/spawner.py b/controller_manager/scripts/spawner.py
old mode 100644
new mode 100755
index 37b90cb4de..d119d65123
--- a/controller_manager/scripts/spawner.py
+++ b/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()
diff --git a/controller_manager/scripts/unspawner.py b/controller_manager/scripts/unspawner.py
old mode 100644
new mode 100755
index 37fe6eb5e0..a9201cba9d
--- a/controller_manager/scripts/unspawner.py
+++ b/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()
diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml
index 418fb9fa94..84d9002bfa 100644
--- a/ros2controlcli/package.xml
+++ b/ros2controlcli/package.xml
@@ -13,6 +13,7 @@
ros2cli
ros2node
ros2param
+ controller_manager
controller_manager_msgs
rosidl_runtime_py
diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py
index 1afddbe0a4..8d7f7521ae 100644
--- a/ros2controlcli/ros2controlcli/api/__init__.py
+++ b/ros2controlcli/ros2controlcli/api/__init__.py
@@ -13,13 +13,17 @@
# limitations under the License.
-from controller_manager_msgs.srv import ConfigureController, ConfigureStartController, \
- ListControllers, ListControllerTypes, ListHardwareInterfaces, LoadController, \
- LoadConfigureController, LoadStartController, \
- ReloadControllerLibraries, SwitchController, UnloadController
+from controller_manager import list_controllers
+
+from controller_manager_msgs.srv import \
+ ConfigureStartController, LoadConfigureController, LoadStartController
+
import rclpy
+
from ros2cli.node.direct import DirectNode
+
from ros2node.api import NodeNameCompleter
+
from ros2param.api import call_list_parameters
@@ -52,47 +56,6 @@ def service_caller(service_name, service_type, request):
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 list_hardware_interfaces(controller_manager_name):
- request = ListHardwareInterfaces.Request()
- return service_caller('{}/list_hardware_interfaces'.format(controller_manager_name),
- ListHardwareInterfaces, 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 configure_controller(controller_manager_name, controller_name):
- request = ConfigureController.Request()
- request.name = controller_name
- return service_caller('{}/configure_controller'.format(controller_manager_name),
- ConfigureController, request)
-
-
def load_configure_controller(controller_manager_name, controller_name):
request = LoadConfigureController.Request()
request.name = controller_name
@@ -114,28 +77,6 @@ def configure_start_controller(controller_manager_name, controller_name):
ConfigureStartController, 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."""
@@ -156,10 +97,11 @@ def __init__(self, valid_states=['active', 'inactive', 'configured', 'unconfigur
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]
+ with DirectNode(parsed_args) as node:
+ controllers = list_controllers(node, 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):
diff --git a/ros2controlcli/ros2controlcli/verb/configure_controller.py b/ros2controlcli/ros2controlcli/verb/configure_controller.py
index f6427632ff..a7a1b9fc10 100644
--- a/ros2controlcli/ros2controlcli/verb/configure_controller.py
+++ b/ros2controlcli/ros2controlcli/verb/configure_controller.py
@@ -12,12 +12,13 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+from controller_manager import configure_controller
+
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, \
- configure_controller
-import sys
+from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter
class ConfigureControllerVerb(VerbExtension):
@@ -31,7 +32,8 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
- response = configure_controller(args.controller_manager, args.controller_name)
- if not response.ok:
- print('Error configuring controller, check controller_manager logs', file=sys.stderr)
- return not response.ok
+ with NodeStrategy(args) as node:
+ response = configure_controller(node, args.controller_manager, args.controller_name)
+ if not response.ok:
+ 'Error configuring controller, check controller_manager logs'
+ return 'Successfully configured controller {}'.format(args.controller_name)
diff --git a/ros2controlcli/ros2controlcli/verb/configure_start_controller.py b/ros2controlcli/ros2controlcli/verb/configure_start_controller.py
index 896029e131..b06d9c2cf8 100644
--- a/ros2controlcli/ros2controlcli/verb/configure_start_controller.py
+++ b/ros2controlcli/ros2controlcli/verb/configure_start_controller.py
@@ -14,10 +14,9 @@
from ros2cli.node.direct import add_arguments
from ros2cli.verb import VerbExtension
-from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter, \
- configure_start_controller
-import sys
+from ros2controlcli.api import add_controller_mgr_parsers, configure_start_controller, \
+ ControllerNameCompleter
class ConfigureStartControllerVerb(VerbExtension):
@@ -31,8 +30,8 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
+ print("deprecated warning: Please use either 'load --state' or 'set_state'")
response = configure_start_controller(args.controller_manager, args.controller_name)
if not response.ok:
- print('Error configuring and starting controller, check '
- 'controller_manager logs', file=sys.stderr)
- return not response.ok
+ return 'Error configuring and starting controller, check controller_manager logs'
+ return 'Successfully configured and started controller {}'.format(args.controller_name)
diff --git a/ros2controlcli/ros2controlcli/verb/list.py b/ros2controlcli/ros2controlcli/verb/list.py
index 4a37193297..85b13891e6 100644
--- a/ros2controlcli/ros2controlcli/verb/list.py
+++ b/ros2controlcli/ros2controlcli/verb/list.py
@@ -12,9 +12,13 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+from controller_manager import list_controllers
+
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, list_controllers
+
+from ros2controlcli.api import add_controller_mgr_parsers
class ListVerb(VerbExtension):
@@ -25,7 +29,8 @@ def add_arguments(self, parser, cli_name):
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))
- return 0
+ with NodeStrategy(args) as node:
+ controllers = list_controllers(node, args.controller_manager).controller
+ for c in controllers:
+ print('{:20s}{:20s} {:10s}'.format(c.name, '[' + c.type + ']', c.state))
+ return 0
diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py
index 620724cf5b..9050afb9d4 100644
--- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py
+++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py
@@ -12,9 +12,12 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+from controller_manager import list_hardware_interfaces
+
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, list_hardware_interfaces
+from ros2controlcli.api import add_controller_mgr_parsers
class ListHardwareInterfacesVerb(VerbExtension):
@@ -25,17 +28,18 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
- hardware_interfaces = list_hardware_interfaces(args.controller_manager)
- command_interfaces = sorted(
- hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name)
- state_interfaces = sorted(
- hardware_interfaces.state_interfaces, key=lambda hwi: hwi.name)
- print('command interfaces')
- for command_interface in command_interfaces:
- print('\t%s [%s]' %
- (command_interface.name,
- 'claimed' if command_interface.is_claimed else 'unclaimed'))
- print('state interfaces')
- for state_interface in state_interfaces:
- print('\t', state_interface.name)
- return 0
+ with NodeStrategy(args) as node:
+ hardware_interfaces = list_hardware_interfaces(node, args.controller_manager)
+ command_interfaces = sorted(
+ hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name)
+ state_interfaces = sorted(
+ hardware_interfaces.state_interfaces, key=lambda hwi: hwi.name)
+ print('command interfaces')
+ for command_interface in command_interfaces:
+ print('\t%s [%s]' %
+ (command_interface.name,
+ 'claimed' if command_interface.is_claimed else 'unclaimed'))
+ print('state interfaces')
+ for state_interface in state_interfaces:
+ print('\t', state_interface.name)
+ return 0
diff --git a/ros2controlcli/ros2controlcli/verb/list_types.py b/ros2controlcli/ros2controlcli/verb/list_types.py
index b031eb664a..ba751f4baa 100644
--- a/ros2controlcli/ros2controlcli/verb/list_types.py
+++ b/ros2controlcli/ros2controlcli/verb/list_types.py
@@ -12,9 +12,13 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+from controller_manager import list_controller_types
+
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, list_controller_types
+
+from ros2controlcli.api import add_controller_mgr_parsers
class ListTypesVerb(VerbExtension):
@@ -25,8 +29,9 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
- response = list_controller_types(args.controller_manager)
- types_and_classes = zip(response.types, response.base_classes)
- for c in types_and_classes:
- print('{:70s} {}'.format(c[0], c[1]))
- return 0
+ with NodeStrategy(args) 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:
+ print('{:70s} {}'.format(c[0], c[1]))
+ return 0
diff --git a/ros2controlcli/ros2controlcli/verb/load.py b/ros2controlcli/ros2controlcli/verb/load.py
index d97fa2845a..9b761fe5e7 100644
--- a/ros2controlcli/ros2controlcli/verb/load.py
+++ b/ros2controlcli/ros2controlcli/verb/load.py
@@ -12,11 +12,13 @@
# 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 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, load_controller
-import sys
+from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter
class LoadVerb(VerbExtension):
@@ -27,10 +29,38 @@ def add_arguments(self, parser, cli_name):
arg = parser.add_argument(
'controller_name', help='Name of the controller')
arg.completer = ControllerNameCompleter()
+ arg = parser.add_argument(
+ '--state',
+ choices=['configure', 'start'],
+ help='Set the state of the loaded controller')
add_controller_mgr_parsers(parser)
def main(self, *, args):
- response = load_controller(args.controller_manager, args.controller_name)
- if not response.ok:
- print('Error loading controller, check controller_manager logs', file=sys.stderr)
- return not response.ok
+ 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'
+
+ if not args.state:
+ return 'Successfully loaded controller {}'.format(args.controller_name)
+
+ # 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'
+
+ if args.state == 'start':
+ response = switch_controllers(
+ node,
+ args.controller_manager,
+ [],
+ [args.controller_name],
+ True,
+ True,
+ 5.0)
+ if not response.ok:
+ return 'Error starting controller, check controller_manager logs'
+
+ return 'Sucessfully loaded controller {} into state {}'.format(
+ args.controller_name, ('inactive' if args.state == 'configure' else 'active'))
diff --git a/ros2controlcli/ros2controlcli/verb/load_configure_controller.py b/ros2controlcli/ros2controlcli/verb/load_configure_controller.py
index eccd52cabc..54c1d9a3f1 100644
--- a/ros2controlcli/ros2controlcli/verb/load_configure_controller.py
+++ b/ros2controlcli/ros2controlcli/verb/load_configure_controller.py
@@ -14,11 +14,10 @@
from ros2cli.node.direct import add_arguments
from ros2cli.verb import VerbExtension
+
from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter, \
load_configure_controller
-import sys
-
class LoadConfigureControllerVerb(VerbExtension):
"""Load and Configure a controller in a controller manager."""
@@ -31,8 +30,8 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
+ print("deprecated warning: Please use either 'load --state' or 'set_state'")
response = load_configure_controller(args.controller_manager, args.controller_name)
if not response.ok:
- print('Error loading and configuring controller, check '
- 'controller_manager logs', file=sys.stderr)
- return not response.ok
+ return 'Error loading and configuring controller, check controller_manager logs'
+ return 'Successfully loaded and configured controller {}'.format(args.controller_name)
diff --git a/ros2controlcli/ros2controlcli/verb/load_start_controller.py b/ros2controlcli/ros2controlcli/verb/load_start_controller.py
index 13e97da0d6..d5d85b6ad4 100644
--- a/ros2controlcli/ros2controlcli/verb/load_start_controller.py
+++ b/ros2controlcli/ros2controlcli/verb/load_start_controller.py
@@ -14,11 +14,10 @@
from ros2cli.node.direct import add_arguments
from ros2cli.verb import VerbExtension
+
from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter, \
load_start_controller
-import sys
-
class LoadStartControllerVerb(VerbExtension):
"""Load, Configure and Start a controller in a controller manager."""
@@ -31,8 +30,8 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
+ print("deprecated warning: Please use either 'load --state' or 'set_state'")
response = load_start_controller(args.controller_manager, args.controller_name)
if not response.ok:
- print('Error loading and starting controller, check '
- 'controller_manager logs', file=sys.stderr)
- return not response.ok
+ return 'Error loading and starting controller, check controller_manager logs'
+ return 'Successfully loaded and started controller {}'.format(args.controller_name)
diff --git a/ros2controlcli/ros2controlcli/verb/reload_libraries.py b/ros2controlcli/ros2controlcli/verb/reload_libraries.py
index 8e4693cedc..356462796c 100644
--- a/ros2controlcli/ros2controlcli/verb/reload_libraries.py
+++ b/ros2controlcli/ros2controlcli/verb/reload_libraries.py
@@ -12,11 +12,13 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+from controller_manager import reload_controller_libraries
+
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, reload_controller_libraries
-import sys
+from ros2controlcli.api import add_controller_mgr_parsers
class ReloadLibrariesVerb(VerbExtension):
@@ -30,9 +32,9 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
- response = reload_controller_libraries(args.controller_manager, force_kill=args.force_kill)
- if response.ok:
- print('Reload successful')
- else:
- print('Error reloading libraries, check controller_manager logs', file=sys.stderr)
- return not response.ok
+ with NodeStrategy(args) as node:
+ response = reload_controller_libraries(
+ node, args.controller_manager, force_kill=args.force_kill)
+ if response.ok:
+ return 'Reload successful'
+ return 'Error reloading libraries, check controller_manager logs'
diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py
new file mode 100644
index 0000000000..d737ee74a3
--- /dev/null
+++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py
@@ -0,0 +1,89 @@
+# 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 import configure_controller, list_controllers, switch_controllers
+
+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, LoadedControllerNameCompleter
+
+
+class SetControllerStateVerb(VerbExtension):
+ """Adjust the state of the controller."""
+
+ def add_arguments(self, parser, cli_name):
+ add_arguments(parser)
+ arg = parser.add_argument(
+ 'controller_name',
+ help='Name of the controller to be changed')
+ arg.completer = LoadedControllerNameCompleter()
+ arg = parser.add_argument(
+ 'state',
+ choices=['configure', 'start', 'stop'],
+ help='State in which the controller should be changed to')
+ add_controller_mgr_parsers(parser)
+
+ def main(self, *, args):
+ with NodeStrategy(args) as node:
+ controllers = list_controllers(node, args.controller_manager).controller
+
+ try:
+ matched_controller = [c for c in controllers if c.name == args.controller_name][0]
+ except IndexError:
+ return 'controller {} does not seem to be loaded'.format(args.controller_name)
+
+ if args.state == 'configure':
+ if matched_controller.state != 'unconfigured':
+ return "can't configure {} from its current state {}{}".format(
+ matched_controller.name, matched_controller.state)
+
+ response = configure_controller(
+ node, args.controller_manager, args.controller_name)
+ if not response.ok:
+ return 'Error configuring controller, check controller_manager logs'
+ return 'successfully configured {}'.format(args.controller_name)
+
+ if args.state == 'start':
+ if matched_controller.state != 'inactive':
+ return "can't start {} from its current state {}".format(
+ matched_controller.name, matched_controller.state)
+ response = switch_controllers(
+ node,
+ args.controller_manager,
+ [],
+ [args.controller_name],
+ True,
+ True,
+ 5.0)
+ if not response.ok:
+ return 'Error starting controller, check controller_manager logs'
+ return 'successfully started {}'.format(args.controller_name)
+
+ if args.state == 'stop':
+ if matched_controller.state != 'active':
+ return "can't stop {} from its current state {}".format(
+ matched_controller.name, matched_controller.state)
+ response = switch_controllers(
+ node,
+ args.controller_manager,
+ [args.controller_name],
+ [],
+ True,
+ True,
+ 5.0)
+ if not response.ok:
+ return 'Error stopping controller, check controller_manager logs'
+ return 'successfully stopped {}'.format(args.controller_name)
diff --git a/ros2controlcli/ros2controlcli/verb/switch.py b/ros2controlcli/ros2controlcli/verb/switch.py
index c732993a0b..c747be7744 100644
--- a/ros2controlcli/ros2controlcli/verb/switch.py
+++ b/ros2controlcli/ros2controlcli/verb/switch.py
@@ -12,12 +12,13 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+from controller_manager import switch_controllers
+
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, LoadedControllerNameCompleter, \
- switch_controllers
-import sys
+from ros2controlcli.api import add_controller_mgr_parsers, LoadedControllerNameCompleter
class SwitchVerb(VerbExtension):
@@ -50,13 +51,15 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
- response = switch_controllers(
- args.controller_manager,
- args.stop_controllers,
- args.start_controllers,
- args.strict,
- args.start_asap,
- args.switch_timeout)
- if not response.ok:
- print('Error switching controllers, check controller_manager logs', file=sys.stderr)
- return not response.ok
+ with NodeStrategy(args) as node:
+ response = switch_controllers(
+ node,
+ args.controller_manager,
+ args.stop_controllers,
+ args.start_controllers,
+ args.strict,
+ args.start_asap,
+ args.switch_timeout)
+ if not response.ok:
+ return 'Error switching controllers, check controller_manager logs'
+ return 'Successfully switched controllers'
diff --git a/ros2controlcli/ros2controlcli/verb/unload.py b/ros2controlcli/ros2controlcli/verb/unload.py
index bd6010eeef..ff6ea5674d 100644
--- a/ros2controlcli/ros2controlcli/verb/unload.py
+++ b/ros2controlcli/ros2controlcli/verb/unload.py
@@ -12,12 +12,13 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+from controller_manager import unload_controller
+
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, LoadedControllerNameCompleter, \
- unload_controller
-import sys
+from ros2controlcli.api import add_controller_mgr_parsers, LoadedControllerNameCompleter
class UnloadVerb(VerbExtension):
@@ -31,7 +32,8 @@ def add_arguments(self, parser, cli_name):
add_controller_mgr_parsers(parser)
def main(self, *, args):
- response = unload_controller(args.controller_manager, args.controller_name)
- if not response.ok:
- print('Error unloading controllers, check controller_manager logs', file=sys.stderr)
- return not response.ok
+ with NodeStrategy(args) as node:
+ response = unload_controller(node, args.controller_manager, args.controller_name)
+ if not response.ok:
+ return 'Error unloading controllers, check controller_manager logs'
+ return 'Successfully unloaded controller {}'.format(args.controller_name)
diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py
index 8fa93dbb5d..432c966d0c 100644
--- a/ros2controlcli/setup.py
+++ b/ros2controlcli/setup.py
@@ -38,7 +38,7 @@
'ros2controlcli.verb': [
'list_controllers = ros2controlcli.verb.list:ListVerb',
'list_hardware_interfaces = \
- ros2controlcli.verb.list_hardware_interfaces:ListHardwareInterfacesVerb',
+ ros2controlcli.verb.list_hardware_interfaces:ListHardwareInterfacesVerb',
'list_controller_types = ros2controlcli.verb.list_types:ListTypesVerb',
'load_controller = ros2controlcli.verb.load:LoadVerb',
'configure_controller = \
@@ -51,6 +51,8 @@
ros2controlcli.verb.configure_start_controller:ConfigureStartControllerVerb',
'reload_controller_libraries = \
ros2controlcli.verb.reload_libraries:ReloadLibrariesVerb',
+ 'set_controller_state = \
+ ros2controlcli.verb.set_controller_state:SetControllerStateVerb',
'switch_controllers = ros2controlcli.verb.switch:SwitchVerb',
'unload_controller = ros2controlcli.verb.unload:UnloadVerb',
],