From 40d080d9673b3fee22d240161b38e01c3db90659 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 1 Apr 2024 20:01:38 +0200 Subject: [PATCH] [CLI] Add `set_hardware_component_state` verb (#1248) Co-authored-by: Dr. Denis Co-authored-by: Mateus Menezes (cherry picked from commit a902e27968fa5983aab86d607aa83baa34531d01) --- ros2controlcli/doc/userdoc.rst | 28 +++++++ ros2controlcli/ros2controlcli/api/__init__.py | 16 +++- .../verb/set_controller_state.py | 2 +- .../verb/set_hardware_component_state.py | 80 +++++++++++++++++++ ros2controlcli/setup.py | 2 + 5 files changed, 126 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..5dd88cd6d9 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 hardware component 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..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 new file mode 100644 index 0000000000..4b1093f4f7 --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/set_hardware_component_state.py @@ -0,0 +1,80 @@ +# 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. +# 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 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: + + if args.state == "unconfigured": + + 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" + + if args.state == "inactive": + inactive_state = State() + inactive_state.id = State.PRIMARY_STATE_INACTIVE + inactive_state.label = "inactive" + + 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": + + 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 8e765a0c4a..da650c0219 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", ],