From 447228704ecdf953fefd40d4ea64f8c14d4c5f8a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 11 Sep 2024 16:46:15 +0200 Subject: [PATCH] [ros2controlcli] add params file parsing to load_controller verb and add namespacing support (#1703) --- .../controller_manager_services.py | 20 ++- doc/release_notes.rst | 12 ++ ros2controlcli/doc/userdoc.rst | 137 +++++++++++------- ros2controlcli/ros2controlcli/api/__init__.py | 47 ++---- .../verb/list_controller_types.py | 2 +- .../ros2controlcli/verb/list_controllers.py | 2 +- .../verb/list_hardware_components.py | 2 +- .../verb/list_hardware_interfaces.py | 2 +- .../ros2controlcli/verb/load_controller.py | 93 +++++++++--- .../verb/reload_controller_libraries.py | 2 +- .../verb/set_controller_state.py | 2 +- .../verb/set_hardware_component_state.py | 2 +- .../ros2controlcli/verb/switch_controllers.py | 13 +- .../ros2controlcli/verb/unload_controller.py | 15 +- .../verb/view_controller_chains.py | 2 +- 15 files changed, 223 insertions(+), 130 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a09be5d14b..a1d3c0f5ad 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." ) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 097ab208b0..bb9c10abe0 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -118,3 +118,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:= diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5dd88cd6d9..8449583d61 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 + 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 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 + 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 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 - Name of the controller manager ROS node - --include-hidden-nodes + -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 (default: controller_manager) + --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 + 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 .. 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 + 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 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 + 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 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 + 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 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 @@ -234,9 +255,10 @@ 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 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 (default: controller_manager) + --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 + 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 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 @@ -304,6 +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 2b4b805980..7ff78c3ed0 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -15,40 +15,13 @@ from controller_manager import list_controllers, list_hardware_components -import rclpy - from ros2cli.node.direct import DirectNode from ros2node.api import NodeNameCompleter 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() +import argparse class ControllerNameCompleter: @@ -89,16 +62,28 @@ 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", - help="Name of the controller manager ROS node", - default="/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") 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, + ) 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 b4ebff94cd..6b6a42e3ce 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) if not response.controller: 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 b5a155de94..e47540a1df 100644 --- a/ros2controlcli/ros2controlcli/verb/load_controller.py +++ b/ros2controlcli/ros2controlcli/verb/load_controller.py @@ -12,13 +12,22 @@ # 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 +from argparse import OPTIONAL class LoadControllerVerb(VerbExtension): @@ -28,37 +37,79 @@ 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=OPTIONAL, + 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" + 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( + 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( + 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): + 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( + f"{bcolors.FAIL}Failed loading controller {args.controller_name} check controller_manager logs{bcolors.ENDC}" + ) + return 1 + print( + f"{bcolors.OKBLUE}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( + f"{bcolors.FAIL}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( + f"{bcolors.FAIL}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( + f"{bcolors.OKBLUE}Successfully loaded controller {args.controller_name} into state {args.set_state}{bcolors.ENDC}" + ) + return 0 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 1c798c29b8..60ceb000c4 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 @@ -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, @@ -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..cd445cbb70 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 @@ -31,10 +31,15 @@ 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: - return "Error unloading controllers, check controller_manager logs" - - print(f"Successfully unloaded controller {args.controller_name}") + print( + f"{bcolors.FAIL}Error unloading controller {args.controller_name}, check controller_manager logs{bcolors.ENDC}" + ) + return 1 + + print( + f"{bcolors.OKBLUE}Successfully unloaded controller {args.controller_name}{bcolors.ENDC}" + ) return 0 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)