From 3d44d4e5d399d1e23c6385a94dc4763b100c069a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 23 Mar 2024 15:30:32 +0000 Subject: [PATCH 1/4] Add set_hardware_component_state_verb --- ros2controlcli/doc/userdoc.rst | 28 ++++ ros2controlcli/ros2controlcli/api/__init__.py | 16 ++- .../verb/set_controller_state.py | 2 +- .../verb/set_hardware_component_state.py | 127 ++++++++++++++++++ ros2controlcli/setup.py | 2 + 5 files changed, 173 insertions(+), 2 deletions(-) create mode 100644 ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 6dadd31226..0ff831732f 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -16,6 +16,7 @@ Currently supported commands are - ros2 control load_controller - ros2 control reload_controller_libraries - ros2 control set_controller_state + - ros2 control set_hardware_component_state - ros2 control switch_controllers - ros2 control unload_controller - ros2 control view_controller_chains @@ -210,6 +211,33 @@ set_controller_state --include-hidden-nodes Consider hidden nodes as well +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} + + Adjust the state of the hardware component + + positional arguments: + hardware_component_name + Name of the hardware_component to be changed + {unconfigured,inactive,active} + State in which the controller should be changed to + + 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 + switch_controllers ------------------ diff --git a/ros2controlcli/ros2controlcli/api/__init__.py b/ros2controlcli/ros2controlcli/api/__init__.py index df522670ec..c1a3d8cb5d 100644 --- a/ros2controlcli/ros2controlcli/api/__init__.py +++ b/ros2controlcli/ros2controlcli/api/__init__.py @@ -13,7 +13,7 @@ # limitations under the License. -from controller_manager import list_controllers +from controller_manager import list_controllers, list_hardware_components import rclpy @@ -75,6 +75,20 @@ def __call__(self, prefix, parsed_args, **kwargs): return [c.name for c in controllers if c.state in self.valid_states] +class LoadedHardwareComponentNameCompleter: + """Callable returning a list of loaded hardware components.""" + + def __init__(self, valid_states=["active", "inactive", "configured", "unconfigured"]): + self.valid_states = valid_states + + def __call__(self, prefix, parsed_args, **kwargs): + with DirectNode(parsed_args) as node: + hardware_components = list_hardware_components( + node, parsed_args.controller_manager + ).component + return [c.name for c in hardware_components if c.state.label in self.valid_states] + + def add_controller_mgr_parsers(parser): """Parser arguments to get controller manager node name, defaults to /controller_manager.""" arg = parser.add_argument( diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py index d584abe987..73bffcc5cd 100644 --- a/ros2controlcli/ros2controlcli/verb/set_controller_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py @@ -81,7 +81,7 @@ def main(self, *, args): else: return ( - f'cannot put {matched_controller.name} in "inactive" state' + f'cannot put {matched_controller.name} in "inactive" state ' f"from its current state {matched_controller.state}" ) diff --git a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py new file mode 100644 index 0000000000..819b49fcf5 --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py @@ -0,0 +1,127 @@ +# Copyright 2023 ROS2Control Developer Team +# +# 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 list_hardware_components, set_hardware_component_state + +from ros2cli.node.direct import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2cli.verb import VerbExtension +from lifecycle_msgs.msg import State + +from ros2controlcli.api import add_controller_mgr_parsers, LoadedHardwareComponentNameCompleter + + +class SetHardwareComponentStateVerb(VerbExtension): + """Adjust the state of the hardware component.""" + + def add_arguments(self, parser, cli_name): + add_arguments(parser) + arg = parser.add_argument( + "hardware_component_name", help="Name of the hardware_component to be changed" + ) + arg.completer = LoadedHardwareComponentNameCompleter() + arg = parser.add_argument( + "state", + choices=["unconfigured", "inactive", "active"], + help="State in which the hardware component should be changed to", + ) + add_controller_mgr_parsers(parser) + + def main(self, *, args): + with NodeStrategy(args) as node: + hardware_components = list_hardware_components(node, args.controller_manager).component + + try: + matched_hardware_components = [ + c for c in hardware_components if c.name == args.hardware_component_name + ][0] + except IndexError: + return f"component {args.hardware_component_name} does not seem to be loaded" + + if args.state == "unconfigured": + if matched_hardware_components.state.label != "inactive": + return ( + f"cannot cleanup {matched_hardware_components.name} " + f"from its current state {matched_hardware_components.state}" + ) + + unconfigured_state = State() + unconfigured_state.id = State.PRIMARY_STATE_UNCONFIGURED + unconfigured_state.label = "unconfigured" + + response = set_hardware_component_state( + node, args.controller_manager, args.hardware_component_name, unconfigured_state + ) + if not response.ok: + return "Error cleaning up hardware component, check controller_manager logs" + + print(f"Successfully set {args.hardware_component_name} to state {response.state}") + return 0 + + if args.state == "inactive": + inactive_state = State() + inactive_state.id = State.PRIMARY_STATE_INACTIVE + inactive_state.label = "inactive" + + if matched_hardware_components.state.label == "unconfigured": + response = set_hardware_component_state( + node, args.controller_manager, args.hardware_component_name, inactive_state + ) + if not response.ok: + return ( + "Error configuring hardware component, check controller_manager logs" + ) + + print( + f"Successfully set {args.hardware_component_name} to state {response.state}" + ) + return 0 + + elif matched_hardware_components.state.label == "active": + response = set_hardware_component_state( + node, args.controller_manager, args.hardware_component_name, inactive_state + ) + if not response.ok: + return "Error stopping hardware component, check controller_manager logs" + + print( + f"Successfully set {args.hardware_component_name} to state {response.state}" + ) + return 0 + + else: + return ( + f'cannot put {matched_hardware_components.name} in "inactive" state ' + f"from its current state {matched_hardware_components.state}" + ) + + if args.state == "active": + if matched_hardware_components.state.label != "inactive": + return ( + f"cannot activate {matched_hardware_components.name} " + f"from its current state {matched_hardware_components.state}" + ) + + active_state = State() + active_state.id = State.PRIMARY_STATE_ACTIVE + active_state.label = "active" + + response = set_hardware_component_state( + node, args.controller_manager, args.hardware_component_name, active_state + ) + if not response.ok: + return "Error activating hardware component, check controller_manager logs" + + print(f"Successfully set {args.hardware_component_name} to state {response.state}") + return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index d45c1125dd..c04ad508aa 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -62,6 +62,8 @@ ros2controlcli.verb.reload_controller_libraries:ReloadControllerLibrariesVerb", "set_controller_state = \ ros2controlcli.verb.set_controller_state:SetControllerStateVerb", + "set_hardware_component_state = \ + ros2controlcli.verb.set_hardware_component_state:SetHardwareComponentStateVerb", "switch_controllers = ros2controlcli.verb.switch_controllers:SwitchControllersVerb", "unload_controller = ros2controlcli.verb.unload_controller:UnloadControllerVerb", ], From 0effe5411c34e2bedc20788e26858d5fba0d82c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 23 Mar 2024 15:30:32 +0000 Subject: [PATCH 2/4] Apply suggestions from code review Co-authored-by: Dr. Denis --- ros2controlcli/ros2controlcli/verb/set_controller_state.py | 2 +- .../ros2controlcli/verb/set_hardware_component_state.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/set_controller_state.py b/ros2controlcli/ros2controlcli/verb/set_controller_state.py index 73bffcc5cd..7cc44775a1 100644 --- a/ros2controlcli/ros2controlcli/verb/set_controller_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_controller_state.py @@ -81,7 +81,7 @@ def main(self, *, args): else: return ( - f'cannot put {matched_controller.name} in "inactive" state ' + f"cannot put {matched_controller.name} in 'inactive' state " f"from its current state {matched_controller.state}" ) diff --git a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py index 819b49fcf5..7adfc0d7f9 100644 --- a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py @@ -1,4 +1,4 @@ -# Copyright 2023 ROS2Control Developer Team +# Copyright 2023 ros2_control Development Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From 41ddf8959215e1b43f77210f3d5e4c6b48125090 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 23 Mar 2024 15:30:32 +0000 Subject: [PATCH 3/4] Update ros2controlcli/doc/userdoc.rst Co-authored-by: Mateus Menezes --- ros2controlcli/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 0ff831732f..5dd88cd6d9 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -226,7 +226,7 @@ set_hardware_component_state hardware_component_name Name of the hardware_component to be changed {unconfigured,inactive,active} - State in which the controller should be changed to + State in which the hardware component should be changed to options: -h, --help show this help message and exit From 6fa349290783b2fdaed74f15448f6514789af196 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 23 Mar 2024 15:43:54 +0000 Subject: [PATCH 4/4] Remove checks and just call the service --- .../verb/set_hardware_component_state.py | 63 +++---------------- 1 file changed, 8 insertions(+), 55 deletions(-) diff --git a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py index 7adfc0d7f9..4b1093f4f7 100644 --- a/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py +++ b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_components, set_hardware_component_state +from controller_manager import set_hardware_component_state from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy @@ -40,21 +40,8 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args) as node: - hardware_components = list_hardware_components(node, args.controller_manager).component - - try: - matched_hardware_components = [ - c for c in hardware_components if c.name == args.hardware_component_name - ][0] - except IndexError: - return f"component {args.hardware_component_name} does not seem to be loaded" if args.state == "unconfigured": - if matched_hardware_components.state.label != "inactive": - return ( - f"cannot cleanup {matched_hardware_components.name} " - f"from its current state {matched_hardware_components.state}" - ) unconfigured_state = State() unconfigured_state.id = State.PRIMARY_STATE_UNCONFIGURED @@ -66,52 +53,18 @@ def main(self, *, args): if not response.ok: return "Error cleaning up hardware component, check controller_manager logs" - print(f"Successfully set {args.hardware_component_name} to state {response.state}") - return 0 - if args.state == "inactive": inactive_state = State() inactive_state.id = State.PRIMARY_STATE_INACTIVE inactive_state.label = "inactive" - if matched_hardware_components.state.label == "unconfigured": - response = set_hardware_component_state( - node, args.controller_manager, args.hardware_component_name, inactive_state - ) - if not response.ok: - return ( - "Error configuring hardware component, check controller_manager logs" - ) - - print( - f"Successfully set {args.hardware_component_name} to state {response.state}" - ) - return 0 - - elif matched_hardware_components.state.label == "active": - response = set_hardware_component_state( - node, args.controller_manager, args.hardware_component_name, inactive_state - ) - if not response.ok: - return "Error stopping hardware component, check controller_manager logs" - - print( - f"Successfully set {args.hardware_component_name} to state {response.state}" - ) - return 0 - - else: - return ( - f'cannot put {matched_hardware_components.name} in "inactive" state ' - f"from its current state {matched_hardware_components.state}" - ) + response = set_hardware_component_state( + node, args.controller_manager, args.hardware_component_name, inactive_state + ) + if not response.ok: + return "Error stopping hardware component, check controller_manager logs" if args.state == "active": - if matched_hardware_components.state.label != "inactive": - return ( - f"cannot activate {matched_hardware_components.name} " - f"from its current state {matched_hardware_components.state}" - ) active_state = State() active_state.id = State.PRIMARY_STATE_ACTIVE @@ -123,5 +76,5 @@ def main(self, *, args): if not response.ok: return "Error activating hardware component, check controller_manager logs" - print(f"Successfully set {args.hardware_component_name} to state {response.state}") - return 0 + print(f"Successfully set {args.hardware_component_name} to state {response.state}") + return 0