From 373d5dde77f66bc930d37800cdfb613d73ce4bcb Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 15:53:48 +0200 Subject: [PATCH 01/11] update the load controller to reuse things from the spawner --- ros2controlcli/ros2controlcli/api/__init__.py | 2 +- .../ros2controlcli/verb/load_controller.py | 104 ++++++++++++++---- 2 files changed, 85 insertions(+), 21 deletions(-) diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index 2b4b805980..ef814bb99b 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -95,7 +95,7 @@ def add_controller_mgr_parsers(parser): "-c", "--controller-manager", help="Name of the controller manager ROS node", - default="/controller_manager", + default="controller_manager", required=False, ) arg.completer = NodeNameCompleter(include_hidden_nodes_key="include_hidden_nodes") diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index b5a155de94..f57e781588 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -12,13 +12,21 @@ # 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 controller_manager import ( + configure_controller, + load_controller, + list_controllers, + switch_controllers, + set_controller_parameters_from_param_file, + bcolors, +) 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 +import os class LoadControllerVerb(VerbExtension): @@ -28,37 +36,93 @@ def add_arguments(self, parser, cli_name): add_arguments(parser) arg = parser.add_argument("controller_name", help="Name of the controller") arg.completer = ControllerNameCompleter() + arg = parser.add_argument( + "param_file", + help="The YAML file with the controller parameters", + nargs="?", + default=None, + ) arg = parser.add_argument( "--set-state", choices=["inactive", "active"], help="Set the state of the loaded controller", + default=None, ) add_controller_mgr_parsers(parser) def main(self, *, args): 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" + controllers = list_controllers(node, args.controller_manager, 20.0).controller + if any(c.name == args.controller_name for c in controllers): + print( + bcolors.WARNING + + f"Controller : {args.controller_name} already loaded, skipping load_controller!" + + bcolors.ENDC + ) + else: + if args.param_file: + if not os.path.exists(args.param_file): + print( + bcolors.FAIL + + f"Controller parameter file : {args.param_file} does not exist, Aborting!" + + bcolors.ENDC + ) + return 1 + if not os.path.isabs(args.param_file): + args.param_file = os.path.join(os.getcwd(), args.param_file) - if not args.set_state: - print(f"Successfully loaded controller {args.controller_name}") - return 0 + if not set_controller_parameters_from_param_file( + node, + args.controller_manager, + args.controller_name, + args.param_file, + node.get_namespace(), + ): + return 1 - # 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" + ret = load_controller(node, args.controller_manager, args.controller_name) + if not ret.ok: + print( + bcolors.FAIL + + f"Failed loading controller {args.controller_name} check controller_manager logs" + + bcolors.ENDC + ) + return 1 + print( + bcolors.OKBLUE + + f"Successfully loaded controller {args.controller_name}" + + bcolors.ENDC + ) - if args.set_state == "active": - response = switch_controllers( - node, args.controller_manager, [], [args.controller_name], True, True, 5.0 + if args.set_state: + + # we in any case configure the controller + response = configure_controller( + node, args.controller_manager, args.controller_name ) if not response.ok: - return "Error activating controller, check controller_manager logs" + print( + bcolors.FAIL + + f"Error configuring controller : {args.controller_name}" + + bcolors.ENDC + ) + return 1 + + if args.set_state == "active": + response = switch_controllers( + node, args.controller_manager, [], [args.controller_name], True, True, 5.0 + ) + if not response.ok: + print( + bcolors.FAIL + + f"Error activating controller : {args.controller_name}, check controller_manager logs" + + bcolors.ENDC + ) + return 1 - print( - f"Successfully loaded controller {args.controller_name} into " - f'state {"inactive" if args.set_state == "inactive" else "active"}' - ) - return 0 + print( + bcolors.OKBLUE + + f"Successfully loaded controller {args.controller_name} into state {args.set_state}" + + bcolors.ENDC + ) + return 0 From 1109598c590de7d43d9455ae4aaab0c3a4b1be99 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 16:10:27 +0200 Subject: [PATCH 02/11] update switch and unload controller to use bcolors --- .../ros2controlcli/verb/switch_controllers.py | 11 ++++++++--- .../ros2controlcli/verb/unload_controller.py | 17 +++++++++++++---- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index 1c798c29b8..840dd96f02 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import switch_controllers +from controller_manager import switch_controllers, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -63,7 +63,12 @@ def main(self, *, args): args.switch_timeout, ) if not response.ok: - return "Error switching controllers, check controller_manager logs" + print( + bcolors.FAIL + + "Error switching controllers, check controller_manager logs" + + bcolors.ENDC + ) + return 1 - print("Successfully switched controllers") + print(bcolors.OKBLUE + "Successfully switched controllers" + bcolors.ENDC) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/unload_controller.py b/ros2controlcli/ros2controlcli/verb/unload_controller.py index 81612eb3ad..bbd8ea081c 100644 --- a/ros2controlcli/ros2controlcli/verb/unload_controller.py +++ b/ros2controlcli/ros2controlcli/verb/unload_controller.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import unload_controller +from controller_manager import unload_controller, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -34,7 +34,16 @@ def main(self, *, args): 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" - - print(f"Successfully unloaded controller {args.controller_name}") + print( + bcolors.FAIL + + f"Error unloading controller {args.controller_name}, check controller_manager logs" + + bcolors.ENDC + ) + return 1 + + print( + bcolors.OKBLUE + + f"Successfully unloaded controller {args.controller_name}" + + bcolors.ENDC + ) return 0 From 0b9cd03acfcf4ad49c5f6d3e026cf79347137408 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 17:39:42 +0200 Subject: [PATCH 03/11] Use fully qualified service name for better logging and debugging --- .../controller_manager_services.py | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 7b0f726d68..9d151a43dc 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -88,15 +88,23 @@ def service_caller( @return The service response """ - cli = node.create_client(service_type, service_name) + namespace = "" if node.get_namespace() == "/" else node.get_namespace() + fully_qualified_service_name = ( + f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name + ) + cli = node.create_client(service_type, fully_qualified_service_name) while not cli.service_is_ready(): - node.get_logger().info(f"waiting for service {service_name} to become available...") + node.get_logger().info( + f"waiting for service {fully_qualified_service_name} to become available..." + ) if service_timeout: if not cli.wait_for_service(service_timeout): - raise ServiceNotFoundError(f"Could not contact service {service_name}") + raise ServiceNotFoundError( + f"Could not contact service {fully_qualified_service_name}" + ) elif not cli.wait_for_service(10.0): - node.get_logger().warn(f"Could not contact service {service_name}") + node.get_logger().warn(f"Could not contact service {fully_qualified_service_name}") node.get_logger().debug(f"requester: making request: {request}\n") future = None @@ -105,13 +113,13 @@ def service_caller( rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) if future.result() is None: node.get_logger().warning( - f"Failed getting a result from calling {service_name} in " + f"Failed getting a result from calling {fully_qualified_service_name} in " f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: return future.result() raise RuntimeError( - f"Could not successfully call service {service_name} after {max_attempts} attempts." + f"Could not successfully call service {fully_qualified_service_name} after {max_attempts} attempts." ) From 6583657163bf3d09bdc6fa6ff6635e6f0691c740 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 17:40:10 +0200 Subject: [PATCH 04/11] Use argparse OPTIONAL variable --- ros2controlcli/ros2controlcli/verb/load_controller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index f57e781588..13199f37e1 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -27,6 +27,7 @@ from ros2controlcli.api import add_controller_mgr_parsers, ControllerNameCompleter import os +from argparse import OPTIONAL class LoadControllerVerb(VerbExtension): @@ -39,7 +40,7 @@ def add_arguments(self, parser, cli_name): arg = parser.add_argument( "param_file", help="The YAML file with the controller parameters", - nargs="?", + nargs=OPTIONAL, default=None, ) arg = parser.add_argument( From 46d70ad900f28cfebcf19991efd737b6d5b8d49d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 17:59:00 +0200 Subject: [PATCH 05/11] Remove unused service caller --- ros2controlcli/ros2controlcli/api/__init__.py | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index ef814bb99b..aa3afcc42f 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -15,8 +15,6 @@ from controller_manager import list_controllers, list_hardware_components -import rclpy - from ros2cli.node.direct import DirectNode from ros2node.api import NodeNameCompleter @@ -24,31 +22,6 @@ from ros2param.api import call_list_parameters -def service_caller(service_name, service_type, request): - try: - rclpy.init() - - node = rclpy.create_node(f"ros2controlcli_{service_name.replace('/', '')}_requester") - - cli = node.create_client(service_type, service_name) - - if not cli.service_is_ready(): - node.get_logger().debug(f"waiting for service {service_name} to become available...") - - if not cli.wait_for_service(2.0): - raise RuntimeError(f"Could not contact service {service_name}") - - node.get_logger().debug(f"requester: making request: {repr(request)}\n") - future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - return future.result() - else: - future_exception = future.exception() - raise RuntimeError(f"Exception while calling service: {repr(future_exception)}") - finally: - node.destroy_node() - rclpy.shutdown() class ControllerNameCompleter: From baef43fec52827053f13c1c94e40908a2c147fd3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 18:44:59 +0200 Subject: [PATCH 06/11] Add ability to parse ros args to the ros2controlcli --- ros2controlcli/ros2controlcli/api/__init__.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index aa3afcc42f..8475620b72 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -21,7 +21,7 @@ from ros2param.api import call_list_parameters - +import argparse class ControllerNameCompleter: @@ -62,8 +62,14 @@ def __call__(self, prefix, parsed_args, **kwargs): return [c.name for c in hardware_components if c.state.label in self.valid_states] +class ParserROSArgs(argparse.Action): + def __call__(self, parser, namespace, values, option_string=None): + values = [option_string] + values + setattr(namespace, "argv", values) + + def add_controller_mgr_parsers(parser): - """Parser arguments to get controller manager node name, defaults to /controller_manager.""" + """Parser arguments to get controller manager node name, defaults to controller_manager.""" arg = parser.add_argument( "-c", "--controller-manager", @@ -75,3 +81,9 @@ def add_controller_mgr_parsers(parser): parser.add_argument( "--include-hidden-nodes", action="store_true", help="Consider hidden nodes as well" ) + parser.add_argument( + "--ros-args", + nargs=argparse.REMAINDER, + help="Pass arbitrary arguments to the executable", + action=ParserROSArgs, + ) From 4594e36da109e565e91fad29397dbe541dbc99a1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 18:45:26 +0200 Subject: [PATCH 07/11] use direct_node from NodeStrategy to be able to use the node namespacing properly --- ros2controlcli/ros2controlcli/verb/list_controller_types.py | 2 +- ros2controlcli/ros2controlcli/verb/list_controllers.py | 2 +- ros2controlcli/ros2controlcli/verb/list_hardware_components.py | 2 +- ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py | 2 +- ros2controlcli/ros2controlcli/verb/load_controller.py | 2 +- .../ros2controlcli/verb/reload_controller_libraries.py | 2 +- ros2controlcli/ros2controlcli/verb/set_controller_state.py | 2 +- .../ros2controlcli/verb/set_hardware_component_state.py | 2 +- ros2controlcli/ros2controlcli/verb/switch_controllers.py | 2 +- ros2controlcli/ros2controlcli/verb/unload_controller.py | 2 +- ros2controlcli/ros2controlcli/verb/view_controller_chains.py | 2 +- 11 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/list_controller_types.py b/ros2controlcli/ros2controlcli/verb/list_controller_types.py index 086b820124..370c8cdc0d 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controller_types.py +++ b/ros2controlcli/ros2controlcli/verb/list_controller_types.py @@ -29,7 +29,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node 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: diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 3daf752b8c..f50bad1345 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -95,7 +95,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = list_controllers(node, args.controller_manager) # Structure data as table for nicer output diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index e3e2269920..3194fee73f 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -36,7 +36,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 4510998ad9..a0bf8a8403 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -28,7 +28,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: hardware_interfaces = list_hardware_interfaces(node, args.controller_manager) command_interfaces = sorted( hardware_interfaces.command_interfaces, key=lambda hwi: hwi.name diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index 13199f37e1..5a6d7b643b 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -52,7 +52,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: controllers = list_controllers(node, args.controller_manager, 20.0).controller if any(c.name == args.controller_name for c in controllers): print( diff --git a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py index 82bc2e480f..e60c9987a0 100644 --- a/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py +++ b/ros2controlcli/ros2controlcli/verb/reload_controller_libraries.py @@ -32,7 +32,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = reload_controller_libraries( node, args.controller_manager, force_kill=args.force_kill ) diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py index 7cc44775a1..830d309b13 100644 --- a/ros2controlcli/ros2controlcli/verb/set_controller_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py @@ -37,7 +37,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: controllers = list_controllers(node, args.controller_manager).controller try: diff --git a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py index 4b1093f4f7..00f62f3e4c 100644 --- a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py @@ -39,7 +39,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: if args.state == "unconfigured": diff --git a/ros2controlcli/ros2controlcli/verb/switch_controllers.py b/ros2controlcli/ros2controlcli/verb/switch_controllers.py index 840dd96f02..60ceb000c4 100644 --- a/ros2controlcli/ros2controlcli/verb/switch_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/switch_controllers.py @@ -52,7 +52,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = switch_controllers( node, args.controller_manager, diff --git a/ros2controlcli/ros2controlcli/verb/unload_controller.py b/ros2controlcli/ros2controlcli/verb/unload_controller.py index bbd8ea081c..2adf08c100 100644 --- a/ros2controlcli/ros2controlcli/verb/unload_controller.py +++ b/ros2controlcli/ros2controlcli/verb/unload_controller.py @@ -31,7 +31,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: response = unload_controller(node, args.controller_manager, args.controller_name) if not response.ok: print( diff --git a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py index f838a96e1c..dafe067539 100644 --- a/ros2controlcli/ros2controlcli/verb/view_controller_chains.py +++ b/ros2controlcli/ros2controlcli/verb/view_controller_chains.py @@ -202,7 +202,7 @@ def add_arguments(self, parser, cli_name): add_controller_mgr_parsers(parser) def main(self, *, args): - with NodeStrategy(args) as node: + with NodeStrategy(args).direct_node as node: list_controllers_response = list_controllers(node, args.controller_manager) list_hardware_response = list_hardware_interfaces(node, args.controller_manager) parse_response(list_controllers_response, list_hardware_response) From bf6bfd5c5156dbf2e8e8131e891636f64a810aa1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 20 Aug 2024 17:50:06 +0200 Subject: [PATCH 08/11] update print formatting --- .../ros2controlcli/verb/load_controller.py | 28 +++++-------------- .../ros2controlcli/verb/unload_controller.py | 8 ++---- 2 files changed, 9 insertions(+), 27 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/load_controller.py b/ros2controlcli/ros2controlcli/verb/load_controller.py index 5a6d7b643b..e47540a1df 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -56,17 +56,13 @@ def main(self, *, args): controllers = list_controllers(node, args.controller_manager, 20.0).controller if any(c.name == args.controller_name for c in controllers): print( - bcolors.WARNING - + f"Controller : {args.controller_name} already loaded, skipping load_controller!" - + bcolors.ENDC + f"{bcolors.WARNING}Controller : {args.controller_name} already loaded, skipping load_controller!{bcolors.ENDC}" ) else: if args.param_file: if not os.path.exists(args.param_file): print( - bcolors.FAIL - + f"Controller parameter file : {args.param_file} does not exist, Aborting!" - + bcolors.ENDC + f"{bcolors.FAIL}Controller parameter file : {args.param_file} does not exist, Aborting!{bcolors.ENDC}" ) return 1 if not os.path.isabs(args.param_file): @@ -84,15 +80,11 @@ def main(self, *, args): ret = load_controller(node, args.controller_manager, args.controller_name) if not ret.ok: print( - bcolors.FAIL - + f"Failed loading controller {args.controller_name} check controller_manager logs" - + bcolors.ENDC + f"{bcolors.FAIL}Failed loading controller {args.controller_name} check controller_manager logs{bcolors.ENDC}" ) return 1 print( - bcolors.OKBLUE - + f"Successfully loaded controller {args.controller_name}" - + bcolors.ENDC + f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name}{bcolors.ENDC}" ) if args.set_state: @@ -103,9 +95,7 @@ def main(self, *, args): ) if not response.ok: print( - bcolors.FAIL - + f"Error configuring controller : {args.controller_name}" - + bcolors.ENDC + f"{bcolors.FAIL}Error configuring controller : {args.controller_name}{bcolors.ENDC}" ) return 1 @@ -115,15 +105,11 @@ def main(self, *, args): ) if not response.ok: print( - bcolors.FAIL - + f"Error activating controller : {args.controller_name}, check controller_manager logs" - + bcolors.ENDC + f"{bcolors.FAIL}Error activating controller : {args.controller_name}, check controller_manager logs{bcolors.ENDC}" ) return 1 print( - bcolors.OKBLUE - + f"Successfully loaded controller {args.controller_name} into state {args.set_state}" - + bcolors.ENDC + f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name} into state {args.set_state}{bcolors.ENDC}" ) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/unload_controller.py b/ros2controlcli/ros2controlcli/verb/unload_controller.py index 2adf08c100..cd445cbb70 100644 --- a/ros2controlcli/ros2controlcli/verb/unload_controller.py +++ b/ros2controlcli/ros2controlcli/verb/unload_controller.py @@ -35,15 +35,11 @@ def main(self, *, args): response = unload_controller(node, args.controller_manager, args.controller_name) if not response.ok: print( - bcolors.FAIL - + f"Error unloading controller {args.controller_name}, check controller_manager logs" - + bcolors.ENDC + f"{bcolors.FAIL}Error unloading controller {args.controller_name}, check controller_manager logs{bcolors.ENDC}" ) return 1 print( - bcolors.OKBLUE - + f"Successfully unloaded controller {args.controller_name}" - + bcolors.ENDC + f"{bcolors.OKBLUE}Successfully unloaded controller {args.controller_name}{bcolors.ENDC}" ) return 0 From 14297c223625f3f74da91247617ab5e05816a767 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 20 Aug 2024 18:09:25 +0200 Subject: [PATCH 09/11] update release notes --- doc/release_notes.rst | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index caa28ca9d0..797db16ecc 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -114,3 +114,15 @@ ros2controlcli .. code-block:: bash ros2 control set_hardware_component_state + +* The ``load_controller`` now supports parsing of the params file (`#1703 `_). + + .. code-block:: bash + + ros2 control load_controller + +* All the ros2controlcli verbs now support the namespacing through the ROS 2 standard way (`#1703 `_). + + .. code-block:: bash + + ros2 control --ros-args -r __ns:= From ee62259355e8ae9f41deb9c42f77f1247d2ce16f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 26 Aug 2024 09:44:12 +0200 Subject: [PATCH 10/11] update ros2controlcli documentation --- ros2controlcli/doc/userdoc.rst | 117 ++++++++++++++++++++------------- 1 file changed, 72 insertions(+), 45 deletions(-) diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5dd88cd6d9..e310f9336c 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -28,18 +28,30 @@ list_controllers .. code-block:: console $ ros2 control list_controllers -h - usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_controllers [-h] [--spin-time SPIN_TIME] [-s] [--claimed-interfaces] [--required-state-interfaces] [--required-command-interfaces] [--chained-interfaces] [--reference-interfaces] [--verbose] + [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the list of loaded controllers, their type and status - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --claimed-interfaces List controller's claimed interfaces + --required-state-interfaces + List controller's required state interfaces + --required-command-interfaces + List controller's required command interfaces + --chained-interfaces List interfaces that the controllers are chained to + --reference-interfaces + List controller's exported references + --verbose, -v List controller's claimed interfaces, required state interfaces and required command interfaces -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable Example output: @@ -55,18 +67,20 @@ list_controller_types .. code-block:: console $ ros2 control list_controller_types -h - usage: ros2 control list_controller_types [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_controller_types [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the available controller types and their base classes - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable Example output: @@ -84,21 +98,21 @@ list_hardware_components .. code-block:: console $ ros2 control list_hardware_components -h - usage: ros2 control list_hardware_components [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_hardware_components [-h] [--spin-time SPIN_TIME] [-s] [--verbose] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the list of available hardware components options: - -h, --help show this help message and exit - --spin-time SPIN_TIME + -h, --help show this help message and exit + --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - -s, --use-sim-time Enable ROS simulation time - --verbose, -v List hardware components with command and state interfaces - -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + -s, --use-sim-time Enable ROS simulation time + --verbose, -v List hardware components with command and state interfaces + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node - --include-hidden-nodes + --include-hidden-nodes Consider hidden nodes as well - + --ros-args ... Pass arbitrary arguments to the executable Example output: @@ -118,18 +132,20 @@ list_hardware_interfaces .. code-block:: console $ ros2 control list_hardware_interfaces -h - usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control list_hardware_interfaces [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Output the list of available command and state interfaces - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable .. code-block:: console @@ -149,23 +165,26 @@ load_controller .. code-block:: console $ ros2 control load_controller -h - usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [-s] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name [param_file] Load a controller in a controller manager positional arguments: controller_name Name of the controller + param_file The YAML file with the controller parameters - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time --set-state {inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable reload_controller_libraries --------------------------- @@ -173,19 +192,21 @@ reload_controller_libraries .. code-block:: console $ ros2 control reload_controller_libraries -h - usage: ros2 control reload_controller_libraries [-h] [--spin-time SPIN_TIME] [--force-kill] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] + usage: ros2 control reload_controller_libraries [-h] [--spin-time SPIN_TIME] [-s] [--force-kill] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Reload controller libraries - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time --force-kill Force stop of loaded controllers -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable set_controller_state -------------------- @@ -193,23 +214,24 @@ set_controller_state .. code-block:: console $ ros2 control set_controller_state -h - usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name {inactive,active} + usage: ros2 control set_controller_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name {inactive,active} Adjust the state of the controller positional arguments: controller_name Name of the controller to be changed - {inactive,active} - State in which the controller should be changed to + {inactive,active} State in which the controller should be changed to - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable set_hardware_component_state ---------------------------- @@ -217,8 +239,7 @@ set_hardware_component_state .. code-block:: console $ ros2 control set_hardware_component_state -h - usage: ros2 control set_hardware_component_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] - hardware_component_name {unconfigured,inactive,active} + usage: ros2 control set_hardware_component_state [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] hardware_component_name {unconfigured,inactive,active} Adjust the state of the hardware component @@ -237,6 +258,7 @@ set_hardware_component_state Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable switch_controllers ------------------ @@ -244,27 +266,29 @@ switch_controllers .. code-block:: console $ ros2 control switch_controllers -h - usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [--deactivate [CTRL1 [CTRL2 ...]]] [--activate [CTRL1 [CTRL2 ...]]] [--strict] [--activate-asap] [--switch-timeout SWITCH_TIMEOUT] [-c CONTROLLER_MANAGER] - [--include-hidden-nodes] + usage: ros2 control switch_controllers [-h] [--spin-time SPIN_TIME] [-s] [--deactivate [DEACTIVATE ...]] [--activate [ACTIVATE ...]] [--strict] [--activate-asap] [--switch-timeout SWITCH_TIMEOUT] + [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Switch controllers in a controller manager - optional arguments: - -h, --help show this help message and exit - --spin-time SPIN_TIME - Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --deactivate [CTRL1 [CTRL2 ...]] - Name of the controllers to be deactivate - --activate [CTRL1 [CTRL2 ...]] - Name of the controllers to be activated - --strict Strict switch - --activate-asap Activate asap controllers - --switch-timeout SWITCH_TIMEOUT - Timeout for switching controllers - -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node - --include-hidden-nodes - Consider hidden nodes as well + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + --deactivate [DEACTIVATE ...] + Name of the controllers to be deactivated + --activate [ACTIVATE ...] + Name of the controllers to be activated + --strict Strict switch + --activate-asap Start asap controllers + --switch-timeout SWITCH_TIMEOUT + Timeout for switching controllers + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node + --include-hidden-nodes + Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable unload_controller ----------------- @@ -272,21 +296,23 @@ unload_controller .. code-block:: console $ ros2 control unload_controller -h - usage: ros2 control unload_controller [-h] [--spin-time SPIN_TIME] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control unload_controller [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] controller_name Unload a controller in a controller manager positional arguments: controller_name Name of the controller - optional arguments: + options: -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable view_controller_chains ---------------------- @@ -294,7 +320,7 @@ view_controller_chains .. code-block:: console $ ros2 control view_controller_chains -h - usage: ros2 view_controller_chains + usage: ros2 control view_controller_chains [-h] [--spin-time SPIN_TIME] [-s] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] [--ros-args ...] Generates a diagram of the loaded chained controllers into /tmp/controller_diagram.gv.pdf @@ -307,3 +333,4 @@ view_controller_chains Name of the controller manager ROS node --include-hidden-nodes Consider hidden nodes as well + --ros-args ... Pass arbitrary arguments to the executable From db66aa564bb5aee714e98b58da2f6f560120797e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 26 Aug 2024 09:57:18 +0200 Subject: [PATCH 11/11] Add default info of the controller_manager --- ros2controlcli/doc/userdoc.rst | 22 +++++++++---------- ros2controlcli/ros2controlcli/api/__init__.py | 2 +- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index e310f9336c..8449583d61 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -48,7 +48,7 @@ list_controllers List controller's exported references --verbose, -v List controller's claimed interfaces, required state interfaces and required command interfaces -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -77,7 +77,7 @@ list_controller_types Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -109,7 +109,7 @@ list_hardware_components -s, --use-sim-time Enable ROS simulation time --verbose, -v List hardware components with command and state interfaces -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -142,7 +142,7 @@ list_hardware_interfaces Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -181,7 +181,7 @@ load_controller --set-state {inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -203,7 +203,7 @@ reload_controller_libraries -s, --use-sim-time Enable ROS simulation time --force-kill Force stop of loaded controllers -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -228,7 +228,7 @@ set_controller_state Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -255,7 +255,7 @@ set_hardware_component_state Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -285,7 +285,7 @@ switch_controllers --switch-timeout SWITCH_TIMEOUT Timeout for switching controllers -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -309,7 +309,7 @@ unload_controller Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable @@ -330,7 +330,7 @@ view_controller_chains Spin time in seconds to wait for discovery (only applies when not using an already running daemon) -s, --use-sim-time Enable ROS simulation time -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER - Name of the controller manager ROS node + Name of the controller manager ROS node (default: controller_manager) --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index 8475620b72..7ff78c3ed0 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -73,7 +73,7 @@ def add_controller_mgr_parsers(parser): arg = parser.add_argument( "-c", "--controller-manager", - help="Name of the controller manager ROS node", + help="Name of the controller manager ROS node (default: controller_manager)", default="controller_manager", required=False, )