diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index ca90f4a2c6..f49bed4d34 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -20,6 +20,7 @@ list_hardware_interfaces, load_controller, reload_controller_libraries, + set_hardware_component_state, switch_controllers, unload_controller, ) @@ -32,6 +33,7 @@ "list_hardware_interfaces", "load_controller", "reload_controller_libraries", + "set_hardware_component_state", "switch_controllers", "unload_controller", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index b5a712778e..5fe78c69b8 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -20,6 +20,7 @@ ListHardwareInterfaces, LoadController, ReloadControllerLibraries, + SetHardwareComponentState, SwitchController, UnloadController, ) @@ -107,6 +108,18 @@ def reload_controller_libraries(node, controller_manager_name, force_kill): ) +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, 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 e97cd889eb..46e9f36ec3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -196,27 +196,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(); } }