[CLI] Add set_hardware_component_state verb

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