[CLI] Add set_hardware_component_state verb (#1248) (#1471)

Co-authored-by: Dr. Denis <[email protected]>
Co-authored-by: Mateus Menezes <[email protected]>
(cherry picked from commit a902e27)

Co-authored-by: Christoph Fröhlich <[email protected]>

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
 ------------------
 

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(

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}"
                     )
 

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

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