From cecaebaa1bcb170dbce4d52493e8ec34f9b64dc8 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Sat, 11 Mar 2023 19:34:52 +0100 Subject: [PATCH 1/4] add spawner for hardware (#941) * add spwaner for hardware * aplly suggestions from code review * move comment (cherry picked from commit 937817c4c492ed158ccac683360d95176b8dab9d) # Conflicts: # controller_manager/controller_manager/__init__.py # controller_manager/controller_manager/controller_manager_services.py --- .../controller_manager/__init__.py | 14 ++ .../controller_manager_services.py | 39 +++ .../controller_manager/hardware_spawner.py | 227 ++++++++++++++++++ controller_manager/setup.cfg | 1 + controller_manager/src/controller_manager.cpp | 37 ++- 5 files changed, 307 insertions(+), 11 deletions(-) create mode 100644 controller_manager/controller_manager/hardware_spawner.py diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index ee5cbdd3f0..58a0ca2867 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -20,11 +20,13 @@ list_hardware_interfaces, load_controller, reload_controller_libraries, + set_hardware_component_state, switch_controllers, unload_controller ) __all__ = [ +<<<<<<< HEAD 'configure_controller', 'list_controller_types', 'list_controllers', @@ -34,4 +36,16 @@ 'reload_controller_libraries', 'switch_controllers', 'unload_controller', +======= + "configure_controller", + "list_controller_types", + "list_controllers", + "list_hardware_components", + "list_hardware_interfaces", + "load_controller", + "reload_controller_libraries", + "set_hardware_component_state", + "switch_controllers", + "unload_controller", +>>>>>>> 937817c (add spawner for hardware (#941)) ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 64e91a08ba..a3948f6159 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -12,9 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. +<<<<<<< HEAD from controller_manager_msgs.srv import ConfigureController, \ ListControllers, ListControllerTypes, ListHardwareComponents, ListHardwareInterfaces, \ LoadController, ReloadControllerLibraries, SwitchController, UnloadController +======= +from controller_manager_msgs.srv import ( + ConfigureController, + ListControllers, + ListControllerTypes, + ListHardwareComponents, + ListHardwareInterfaces, + LoadController, + ReloadControllerLibraries, + SetHardwareComponentState, + SwitchController, + UnloadController, +) +>>>>>>> 937817c (add spawner for hardware (#941)) import rclpy @@ -117,8 +132,32 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ) +<<<<<<< HEAD def switch_controllers(node, controller_manager_name, deactivate_controllers, activate_controllers, strict, activate_asap, timeout): +======= +def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): + request = SetHardwareComponentState.Request() + request.name = component_name + request.target_state = lifecyle_state + return service_caller( + node, + f"{controller_manager_name}/set_hardware_component_state", + SetHardwareComponentState, + request, + ) + + +def switch_controllers( + node, + controller_manager_name, + deactivate_controllers, + activate_controllers, + strict, + activate_asap, + timeout, +): +>>>>>>> 937817c (add spawner for hardware (#941)) request = SwitchController.Request() request.activate_controllers = activate_controllers request.deactivate_controllers = deactivate_controllers diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py new file mode 100644 index 0000000000..c95fb6181e --- /dev/null +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +# Copyright 2023 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + +import argparse +import sys +import time + +from controller_manager import set_hardware_component_state + +from lifecycle_msgs.msg import State +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.signals import SignalHandlerOptions + + +# from https://stackoverflow.com/a/287944 +class bcolors: + HEADER = "\033[95m" + OKBLUE = "\033[94m" + OKCYAN = "\033[96m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" + + +def first_match(iterable, predicate): + return next((n for n in iterable if predicate(n)), None) + + +def wait_for_value_or(function, node, timeout, default, description): + while node.get_clock().now() < timeout: + if result := function(): + return result + node.get_logger().info( + f"Waiting for {description}", throttle_duration_sec=2, skip_first=True + ) + time.sleep(0.2) + return default + + +def combine_name_and_namespace(name_and_namespace): + node_name, namespace = name_and_namespace + return namespace + ("" if namespace.endswith("/") else "/") + node_name + + +def find_node_and_namespace(node, full_node_name): + node_names_and_namespaces = node.get_node_names_and_namespaces() + return first_match( + node_names_and_namespaces, + lambda n: combine_name_and_namespace(n) == full_node_name, + ) + + +def has_service_names(node, node_name, node_namespace, service_names): + client_names_and_types = node.get_service_names_and_types_by_node(node_name, node_namespace) + if not client_names_and_types: + return False + client_names, _ = zip(*client_names_and_types) + return all(service in client_names for service in service_names) + + +def wait_for_controller_manager(node, controller_manager, timeout_duration): + # List of service names from controller_manager we wait for + service_names = ( + f"{controller_manager}/list_hardware_components", + f"{controller_manager}/set_hardware_component_state", + ) + + # Wait for controller_manager + timeout = node.get_clock().now() + Duration(seconds=timeout_duration) + node_and_namespace = wait_for_value_or( + lambda: find_node_and_namespace(node, controller_manager), + node, + timeout, + None, + f"'{controller_manager}' node to exist", + ) + + # Wait for the services if the node was found + if node_and_namespace: + node_name, namespace = node_and_namespace + return wait_for_value_or( + lambda: has_service_names(node, node_name, namespace, service_names), + node, + timeout, + False, + f"'{controller_manager}' services to be available", + ) + + return False + + +def handle_set_component_state_service_call( + node, controller_manager_name, component, target_state, action +): + response = set_hardware_component_state(node, controller_manager_name, component, target_state) + if response.ok and response.state == target_state: + node.get_logger().info( + bcolors.OKGREEN + + f"{action} component '{component}'. Hardware now in state: {response.state}." + ) + elif response.ok and not response.state == target_state: + node.get_logger().warn( + bcolors.WARNING + + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'." + ) + else: + node.get_logger().warn( + bcolors.WARNING + + f"Could not {action} component '{component}'. Service call failed. Wrong component name?" + ) + + +def activate_components(node, controller_manager_name, components_to_activate): + active_state = State() + active_state.id = State.PRIMARY_STATE_ACTIVE + active_state.label = "active" + for component in components_to_activate: + handle_set_component_state_service_call( + node, controller_manager_name, component, active_state, "activated" + ) + + +def configure_components(node, controller_manager_name, components_to_configure): + inactive_state = State() + inactive_state.id = State.PRIMARY_STATE_INACTIVE + inactive_state.label = "inactive" + for component in components_to_configure: + handle_set_component_state_service_call( + node, controller_manager_name, component, inactive_state, "configured" + ) + + +def main(args=None): + rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) + parser = argparse.ArgumentParser() + activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True) + + parser.add_argument( + "hardware_component_name", + help="The name of the hardware component which should be activated.", + ) + parser.add_argument( + "-c", + "--controller-manager", + help="Name of the controller manager ROS node", + default="controller_manager", + required=False, + ) + parser.add_argument( + "--controller-manager-timeout", + help="Time to wait for the controller manager", + required=False, + default=10, + type=int, + ) + + # add arguments which are mutually exclusive + activate_or_confiigure_grp.add_argument( + "--activate", + help="Activates the given components. Note: Components are by default configured before activated. ", + action="store_true", + required=False, + ) + activate_or_confiigure_grp.add_argument( + "--configure", + help="Configures the given components.", + action="store_true", + required=False, + ) + + command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] + args = parser.parse_args(command_line_args) + controller_manager_name = args.controller_manager + controller_manager_timeout = args.controller_manager_timeout + hardware_component = [args.hardware_component_name] + activate = args.activate + configure = args.configure + + node = Node("hardware_spawner") + if not controller_manager_name.startswith("/"): + spawner_namespace = node.get_namespace() + if spawner_namespace != "/": + controller_manager_name = f"{spawner_namespace}/{controller_manager_name}" + else: + controller_manager_name = f"/{controller_manager_name}" + + try: + if not wait_for_controller_manager( + node, controller_manager_name, controller_manager_timeout + ): + node.get_logger().error("Controller manager not available") + return 1 + + if activate: + activate_components(node, controller_manager_name, hardware_component) + elif configure: + configure_components(node, controller_manager_name, hardware_component) + else: + node.get_logger().error( + 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + ) + parser.print_help() + return 0 + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + ret = main() + sys.exit(ret) diff --git a/controller_manager/setup.cfg b/controller_manager/setup.cfg index ef02ee65bd..92c5581e92 100644 --- a/controller_manager/setup.cfg +++ b/controller_manager/setup.cfg @@ -2,3 +2,4 @@ console_scripts = spawner = controller_manager.spawner:main unspawner = controller_manager.unspawner:main + hardware_spawner = controller_manager.hardware_spawner:main diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 81792c9e7d..551c6d58a6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -362,27 +362,42 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; std::vector configure_components_on_start = std::vector({}); - get_parameter("configure_components_on_start", configure_components_on_start); - rclcpp_lifecycle::State inactive_state( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); - for (const auto & component : configure_components_on_start) + if (get_parameter("configure_components_on_start", configure_components_on_start)) { - resource_manager_->set_component_state(component, inactive_state); + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " + "hardware_spawner instead."); + rclcpp_lifecycle::State inactive_state( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); + for (const auto & component : configure_components_on_start) + { + resource_manager_->set_component_state(component, inactive_state); + } } std::vector activate_components_on_start = std::vector({}); - get_parameter("activate_components_on_start", activate_components_on_start); - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - for (const auto & component : activate_components_on_start) + if (get_parameter("activate_components_on_start", activate_components_on_start)) { - resource_manager_->set_component_state(component, active_state); + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " + "hardware_spawner instead."); + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + for (const auto & component : activate_components_on_start) + { + resource_manager_->set_component_state(component, active_state); + } } - // if both parameter are empty or non-existing preserve behavior where all components are // activated per default if (configure_components_on_start.empty() && activate_components_on_start.empty()) { + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Automatic activation of all hardware components will not be supported in the " + "future anymore. Use hardware_spawner instead."); resource_manager_->activate_all_components(); } } From ffd5de5caf1f9b0b17aaaad41cb33e7614b9c72d Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 12 Dec 2023 10:03:19 +0100 Subject: [PATCH 2/4] Resolve conflicts. --- controller_manager/controller_manager/__init__.py | 14 +------------- .../controller_manager_services.py | 11 ----------- 2 files changed, 1 insertion(+), 24 deletions(-) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index 58a0ca2867..849dee7fc0 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -26,7 +26,6 @@ ) __all__ = [ -<<<<<<< HEAD 'configure_controller', 'list_controller_types', 'list_controllers', @@ -34,18 +33,7 @@ 'list_hardware_interfaces', 'load_controller', 'reload_controller_libraries', + 'set_hardware_component_state', 'switch_controllers', 'unload_controller', -======= - "configure_controller", - "list_controller_types", - "list_controllers", - "list_hardware_components", - "list_hardware_interfaces", - "load_controller", - "reload_controller_libraries", - "set_hardware_component_state", - "switch_controllers", - "unload_controller", ->>>>>>> 937817c (add spawner for hardware (#941)) ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index a3948f6159..91c7ab7517 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -12,11 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -<<<<<<< HEAD -from controller_manager_msgs.srv import ConfigureController, \ - ListControllers, ListControllerTypes, ListHardwareComponents, ListHardwareInterfaces, \ - LoadController, ReloadControllerLibraries, SwitchController, UnloadController -======= from controller_manager_msgs.srv import ( ConfigureController, ListControllers, @@ -29,7 +24,6 @@ SwitchController, UnloadController, ) ->>>>>>> 937817c (add spawner for hardware (#941)) import rclpy @@ -132,10 +126,6 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ) -<<<<<<< HEAD -def switch_controllers(node, controller_manager_name, deactivate_controllers, - activate_controllers, strict, activate_asap, timeout): -======= def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state): request = SetHardwareComponentState.Request() request.name = component_name @@ -157,7 +147,6 @@ def switch_controllers( activate_asap, timeout, ): ->>>>>>> 937817c (add spawner for hardware (#941)) request = SwitchController.Request() request.activate_controllers = activate_controllers request.deactivate_controllers = deactivate_controllers From 3d4004f0bc1f112649d601b459aa77bf9353d7e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Tue, 12 Dec 2023 10:08:02 +0100 Subject: [PATCH 3/4] Revert changes on controller manager to humble. --- controller_manager/src/controller_manager.cpp | 37 ++++++------------- 1 file changed, 11 insertions(+), 26 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 551c6d58a6..81792c9e7d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -362,42 +362,27 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; std::vector configure_components_on_start = std::vector({}); - if (get_parameter("configure_components_on_start", configure_components_on_start)) + get_parameter("configure_components_on_start", configure_components_on_start); + rclcpp_lifecycle::State inactive_state( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); + for (const auto & component : configure_components_on_start) { - RCLCPP_WARN_STREAM( - get_logger(), - "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " - "hardware_spawner instead."); - rclcpp_lifecycle::State inactive_state( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); - for (const auto & component : configure_components_on_start) - { - resource_manager_->set_component_state(component, inactive_state); - } + resource_manager_->set_component_state(component, inactive_state); } std::vector activate_components_on_start = std::vector({}); - if (get_parameter("activate_components_on_start", activate_components_on_start)) + get_parameter("activate_components_on_start", activate_components_on_start); + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + for (const auto & component : activate_components_on_start) { - RCLCPP_WARN_STREAM( - get_logger(), - "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " - "hardware_spawner instead."); - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - for (const auto & component : activate_components_on_start) - { - resource_manager_->set_component_state(component, active_state); - } + resource_manager_->set_component_state(component, active_state); } + // if both parameter are empty or non-existing preserve behavior where all components are // activated per default if (configure_components_on_start.empty() && activate_components_on_start.empty()) { - RCLCPP_WARN_STREAM( - get_logger(), - "[Deprecated]: Automatic activation of all hardware components will not be supported in the " - "future anymore. Use hardware_spawner instead."); resource_manager_->activate_all_components(); } } From dbc8d3b85f825f7e669be045c4c8a4d35d2c29cc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 3 Jan 2024 20:11:21 +0000 Subject: [PATCH 4/4] Revert "Revert changes on controller manager to humble." This reverts commit fca604d138da4606c824fb26f4ecf63009a732dc. --- controller_manager/src/controller_manager.cpp | 37 +++++++++++++------ 1 file changed, 26 insertions(+), 11 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 81792c9e7d..551c6d58a6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -362,27 +362,42 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript using lifecycle_msgs::msg::State; std::vector configure_components_on_start = std::vector({}); - get_parameter("configure_components_on_start", configure_components_on_start); - rclcpp_lifecycle::State inactive_state( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); - for (const auto & component : configure_components_on_start) + if (get_parameter("configure_components_on_start", configure_components_on_start)) { - resource_manager_->set_component_state(component, inactive_state); + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " + "hardware_spawner instead."); + rclcpp_lifecycle::State inactive_state( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); + for (const auto & component : configure_components_on_start) + { + resource_manager_->set_component_state(component, inactive_state); + } } std::vector activate_components_on_start = std::vector({}); - get_parameter("activate_components_on_start", activate_components_on_start); - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - for (const auto & component : activate_components_on_start) + if (get_parameter("activate_components_on_start", activate_components_on_start)) { - resource_manager_->set_component_state(component, active_state); + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " + "hardware_spawner instead."); + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + for (const auto & component : activate_components_on_start) + { + resource_manager_->set_component_state(component, active_state); + } } - // if both parameter are empty or non-existing preserve behavior where all components are // activated per default if (configure_components_on_start.empty() && activate_components_on_start.empty()) { + RCLCPP_WARN_STREAM( + get_logger(), + "[Deprecated]: Automatic activation of all hardware components will not be supported in the " + "future anymore. Use hardware_spawner instead."); resource_manager_->activate_all_components(); } }