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', ],