From cce79ebb8c5b509b8bd97e1be26f0061a7227a70 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 14 Aug 2024 15:15:11 +0200 Subject: [PATCH 1/7] Make list controller and list hardware components immediately visualize the state. (#1606) --- .../controller_manager/spawner.py | 2 +- .../ros2controlcli/verb/list_controllers.py | 14 +++++++++++--- .../verb/list_hardware_components.py | 17 ++++++++++++++--- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index bc8d218ea8..a8b5d25eff 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -47,7 +47,7 @@ class bcolors: - HEADER = "\033[95m" + MAGENTA = "\033[95m" OKBLUE = "\033[94m" OKCYAN = "\033[96m" OKGREEN = "\033[92m" diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index 31ce814865..a26a1168a1 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -22,7 +22,7 @@ from ros2controlcli.api import add_controller_mgr_parsers -def print_controller_state(c, args): +def print_controller_state(c, args, col_width_name, col_width_state, col_width_type): state_color = "" if c.state == "active": state_color = bcolors.OKGREEN @@ -31,7 +31,9 @@ def print_controller_state(c, args): elif c.state == "unconfigured": state_color = bcolors.WARNING - print(f"{c.name:20s}[{c.type:20s}] {state_color}{c.state:10s}{bcolors.ENDC}") + print( + f"{state_color}{c.name:<{col_width_name}}{bcolors.ENDC} {c.type:<{col_width_type}} {state_color}{c.state:<{col_width_state}}{bcolors.ENDC}" + ) if args.claimed_interfaces or args.verbose: print("\tclaimed interfaces:") for claimed_interface in c.claimed_interfaces: @@ -96,7 +98,13 @@ def add_arguments(self, parser, cli_name): def main(self, *, args): with NodeStrategy(args) as node: response = list_controllers(node, args.controller_manager) + + # Structure data as table for nicer output + col_width_name = max(len(ctrl.name) for ctrl in response.controller) + col_width_type = max(len(ctrl.type) for ctrl in response.controller) + col_width_state = max(len(ctrl.state) for ctrl in response.controller) + for c in response.controller: - print_controller_state(c, args) + print_controller_state(c, args, col_width_name, col_width_state, col_width_type) return 0 diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index 8a5884f2cb..e2b427b233 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -15,6 +15,8 @@ from controller_manager import list_hardware_components from controller_manager.spawner import bcolors +from lifecycle_msgs.msg import State + from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy from ros2cli.verb import VerbExtension @@ -39,17 +41,26 @@ def main(self, *, args): hardware_components = list_hardware_components(node, args.controller_manager) for idx, component in enumerate(hardware_components.component): + # Set activity color for nicer visualization + activity_color = bcolors.FAIL + if component.state.id == State.PRIMARY_STATE_UNCONFIGURED: + activity_color = bcolors.WARNING + if component.state.id == State.PRIMARY_STATE_INACTIVE: + activity_color = bcolors.MAGENTA + if component.state.id == State.PRIMARY_STATE_ACTIVE: + activity_color = bcolors.OKGREEN + print( - f"Hardware Component {idx+1}\n\tname: {component.name}\n\ttype: {component.type}" + f"Hardware Component {idx+1}\n\tname: {activity_color}{component.name}{bcolors.ENDC}\n\ttype: {component.type}" ) if hasattr(component, "plugin_name"): - plugin_name = component.plugin_name + plugin_name = f"{component.plugin_name}" else: plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}" print( f"\tplugin name: {plugin_name}\n" - f"\tstate: id={component.state.id} label={component.state.label}\n" + f"\tstate: id={component.state.id} label={activity_color}{component.state.label}{bcolors.ENDC}\n" f"\tcommand interfaces" ) for cmd_interface in component.command_interfaces: From af4b48f75a0ff4c57f30df3477f9fa2786e07965 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 14 Aug 2024 16:24:23 +0100 Subject: [PATCH 2/7] Handle waiting in Spawner and align Hardware Spawner functionality (#1562) --- .../controller_manager_services.py | 37 +++++---- .../controller_manager/hardware_spawner.py | 77 ++++++------------- .../controller_manager/spawner.py | 76 ++++-------------- .../controller_manager/unspawner.py | 6 ++ controller_manager/doc/userdoc.rst | 20 +++++ .../test/test_spawner_unspawner.cpp | 22 +++--- 6 files changed, 98 insertions(+), 140 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 62b350f7d3..be11a4b782 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -28,15 +28,20 @@ import rclpy -def service_caller(node, service_name, service_type, request, service_timeout=10.0): +class ServiceNotFoundError(Exception): + pass + + +def service_caller(node, service_name, service_type, request, service_timeout=0.0): cli = node.create_client(service_type, service_name) - if not cli.service_is_ready(): - node.get_logger().debug( - f"waiting {service_timeout} seconds for service {service_name} to become available..." - ) - if not cli.wait_for_service(service_timeout): - raise RuntimeError(f"Could not contact service {service_name}") + while not cli.service_is_ready(): + node.get_logger().info(f"waiting for service {service_name} to become available...") + if service_timeout: + if not cli.wait_for_service(service_timeout): + raise ServiceNotFoundError(f"Could not contact service {service_name}") + elif not cli.wait_for_service(10.0): + node.get_logger().warn(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") future = cli.call_async(request) @@ -47,7 +52,7 @@ def service_caller(node, service_name, service_type, request, service_timeout=10 raise RuntimeError(f"Exception while calling service: {future.exception()}") -def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -59,7 +64,7 @@ def configure_controller(node, controller_manager_name, controller_name, service ) -def list_controllers(node, controller_manager_name, service_timeout=10.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0): request = ListControllers.Request() return service_caller( node, @@ -70,7 +75,7 @@ def list_controllers(node, controller_manager_name, service_timeout=10.0): ) -def list_controller_types(node, controller_manager_name, service_timeout=10.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0): request = ListControllerTypes.Request() return service_caller( node, @@ -81,7 +86,7 @@ def list_controller_types(node, controller_manager_name, service_timeout=10.0): ) -def list_hardware_components(node, controller_manager_name, service_timeout=10.0): +def list_hardware_components(node, controller_manager_name, service_timeout=0.0): request = ListHardwareComponents.Request() return service_caller( node, @@ -92,7 +97,7 @@ def list_hardware_components(node, controller_manager_name, service_timeout=10.0 ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -103,7 +108,7 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0 ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = LoadController.Request() request.name = controller_name return service_caller( @@ -115,7 +120,7 @@ def load_controller(node, controller_manager_name, controller_name, service_time ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -128,7 +133,7 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0 + node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 ): request = SetHardwareComponentState.Request() request.name = component_name @@ -165,7 +170,7 @@ def switch_controllers( ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): request = UnloadController.Request() request.name = controller_name return service_caller( diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index c95fb6181e..3e3a487c6a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -15,13 +15,15 @@ import argparse import sys -import time -from controller_manager import set_hardware_component_state +from controller_manager import ( + list_hardware_components, + set_hardware_component_state, +) +from controller_manager.controller_manager_services import ServiceNotFoundError from lifecycle_msgs.msg import State import rclpy -from rclpy.duration import Duration from rclpy.node import Node from rclpy.signals import SignalHandlerOptions @@ -43,17 +45,6 @@ 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 @@ -75,35 +66,11 @@ def has_service_names(node, node_name, node_namespace, service_names): 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 is_hardware_component_loaded( + node, controller_manager, hardware_component, service_timeout=0.0 +): + components = list_hardware_components(node, hardware_component, service_timeout).component + return any(c.name == hardware_component for c in components) def handle_set_component_state_service_call( @@ -167,10 +134,9 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) - # add arguments which are mutually exclusive activate_or_confiigure_grp.add_argument( "--activate", @@ -202,13 +168,15 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not wait_for_controller_manager( - node, controller_manager_name, controller_manager_timeout + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout ): - node.get_logger().error("Controller manager not available") - return 1 - - if activate: + node.get_logger().warn( + bcolors.WARNING + + "Hardware Component is not loaded - state can not be changed." + + bcolors.ENDC + ) + elif activate: activate_components(node, controller_manager_name, hardware_component) elif configure: configure_components(node, controller_manager_name, hardware_component) @@ -218,6 +186,11 @@ def main(args=None): ) parser.print_help() return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index a8b5d25eff..0e83eb9d07 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,10 +28,10 @@ switch_controllers, unload_controller, ) +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rcl_interfaces.msg import Parameter -from rclpy.duration import Duration from rclpy.node import Node # @note: The versions conditioning is added here to support the source-compatibility with Humble @@ -62,17 +62,6 @@ 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 @@ -93,46 +82,8 @@ def has_service_names(node, node_name, node_namespace, service_names): 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}/configure_controller", - f"{controller_manager}/list_controllers", - f"{controller_manager}/list_controller_types", - f"{controller_manager}/list_hardware_components", - f"{controller_manager}/list_hardware_interfaces", - f"{controller_manager}/load_controller", - f"{controller_manager}/reload_controller_libraries", - f"{controller_manager}/switch_controller", - f"{controller_manager}/unload_controller", - ) - - # 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 is_controller_loaded(node, controller_manager, controller_name): - controllers = list_controllers(node, controller_manager).controller +def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): + controllers = list_controllers(node, controller_manager, service_timeout).controller return any(c.name == controller_name for c in controllers) @@ -201,8 +152,8 @@ def main(args=None): "--controller-manager-timeout", help="Time to wait for the controller manager", required=False, - default=10, - type=int, + default=0, + type=float, ) parser.add_argument( "--activate-as-group", @@ -258,18 +209,12 @@ def main(args=None): 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( - bcolors.FAIL + "Controller manager not available" + bcolors.ENDC - ) - return 1 - for controller_name in controller_names: fallback_controllers = args.fallback_controllers - if is_controller_loaded(node, controller_manager_name, controller_name): + if is_controller_loaded( + node, controller_manager_name, controller_name, controller_manager_timeout + ): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" @@ -471,6 +416,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/unspawner.py b/controller_manager/controller_manager/unspawner.py index 5acf05e65d..e42d85aee9 100644 --- a/controller_manager/controller_manager/unspawner.py +++ b/controller_manager/controller_manager/unspawner.py @@ -19,6 +19,7 @@ import warnings from controller_manager import switch_controllers, unload_controller +from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy from rclpy.node import Node @@ -57,6 +58,11 @@ def main(args=None): node.get_logger().info("Unloaded controller") return 0 + except KeyboardInterrupt: + pass + except ServiceNotFoundError as err: + node.get_logger().fatal(str(err)) + return 1 finally: rclpy.shutdown() diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 6338211665..bc9f75384e 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -136,6 +136,7 @@ There are two scripts to interact with controller manager from launch files: 1. ``spawner`` - loads, configures and start a controller on startup. 2. ``unspawner`` - stops and unloads a controller. + 3. ``hardware_spawner`` - activates and configures a hardware component. ``spawner`` @@ -186,6 +187,25 @@ There are two scripts to interact with controller manager from launch files: -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node +``hardware_spawner`` +^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: console + + $ ros2 run controller_manager hardware_spawner -h + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + + positional arguments: + hardware_component_name + The name of the hardware component which should be activated. + + options: + -h, --help show this help message and exit + -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER + Name of the controller manager ROS node + --activate Activates the given components. Note: Components are by default configured before activated. + --configure Configures the given components. + rqt_controller_manager ---------------------- A GUI tool to interact with the controller manager services to be able to switch the lifecycle states of the controllers as well as the hardware components. diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index fa27ee2c97..3edf770436 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -88,9 +88,10 @@ TEST_F(TestLoadController, spawner_with_no_arguments_errors) EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments"; } -TEST_F(TestLoadController, spawner_without_manager_errors) +TEST_F(TestLoadController, spawner_without_manager_errors_with_given_timeout) { - EXPECT_NE(call_spawner("ctrl_1"), 0) << "Wrong controller manager name"; + EXPECT_NE(call_spawner("ctrl_1 --controller-manager-timeout 1.0"), 0) + << "Wrong controller manager name"; } TEST_F(TestLoadController, spawner_without_type_parameter_or_arg_errors) @@ -283,7 +284,9 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) EXPECT_EQ( call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager -p " + test_file_path), + "ctrl_with_parameters_and_no_type -c test_controller_manager --controller-manager-timeout " + "1.0 -p " + + test_file_path), 256); // Will still be same as the current call will fail ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); @@ -422,7 +425,7 @@ TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spaw cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256) + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --controller-manager-timeout 1.0"), 256) << "could not spawn controller because not robot description and not services for controller " "manager are active"; } @@ -504,7 +507,8 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param) cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 ctrl_2 -c test_controller_manager"), 256) + EXPECT_EQ( + call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --controller-manager-timeout 1.0"), 256) << "Should fail without defining the namespace"; EXPECT_EQ( call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0); @@ -603,7 +607,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager -p " + + "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; @@ -663,21 +667,21 @@ TEST_F( EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager -p " + + "test_controller_manager --controller-manager-timeout 1.0 -p " + test_file_path), 256) << "Should fail without the namespacing it"; EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p " + test_file_path + " --ros-args -r __ns:=/random_namespace"), 256) << "Should fail when parsed namespace through both way with different namespaces"; EXPECT_EQ( call_spawner( "ctrl_with_parameters_and_type chainable_ctrl_with_parameters_and_type --load-only -c " - "test_controller_manager --namespace foo_namespace -p " + + "test_controller_manager --namespace foo_namespace --controller-manager-timeout 1.0 -p" + test_file_path + " --ros-args -r __ns:=/foo_namespace"), 256) << "Should fail when parsed namespace through both ways even with same namespacing name"; From eb4316b1dc65d9a93a2463b4f2b22f3510f0ab1b Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Thu, 15 Aug 2024 14:34:02 +0200 Subject: [PATCH 3/7] Preparation of Handles for Variant Support (#1678) --- .../include/hardware_interface/handle.hpp | 80 ++++++++++--------- .../include/transmission_interface/handle.hpp | 8 +- 2 files changed, 45 insertions(+), 43 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index dc536e51be..bfe0ff1eb8 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -16,41 +16,45 @@ #define HARDWARE_INTERFACE__HANDLE_HPP_ #include +#include #include "hardware_interface/macros.hpp" namespace hardware_interface { + +using HANDLE_DATATYPE = std::variant; + /// A handle used to get and set a value on a given interface. -class ReadOnlyHandle +class Handle { public: - ReadOnlyHandle( + Handle( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } - explicit ReadOnlyHandle(const std::string & interface_name) + explicit Handle(const std::string & interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - explicit ReadOnlyHandle(const char * interface_name) + explicit Handle(const char * interface_name) : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + Handle(const Handle & other) = default; - ReadOnlyHandle(ReadOnlyHandle && other) = default; + Handle(Handle && other) = default; - ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; + Handle & operator=(const Handle & other) = default; - ReadOnlyHandle & operator=(ReadOnlyHandle && other) = default; + Handle & operator=(Handle && other) = default; - virtual ~ReadOnlyHandle() = default; + virtual ~Handle() = default; /// Returns true if handle references a value. inline operator bool() const { return value_ptr_ != nullptr; } @@ -70,60 +74,58 @@ class ReadOnlyHandle double get_value() const { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) return value_ if old functionality is removed THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; + // END + } + + void set_value(double value) + { + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) set value_ directly if old functionality is removed + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; + // END } protected: std::string prefix_name_; std::string interface_name_; + HANDLE_DATATYPE value_; + // BEGIN (Handle export change): for backward compatibility + // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed double * value_ptr_; + // END }; -class ReadWriteHandle : public ReadOnlyHandle +class StateInterface : public Handle { public: - ReadWriteHandle( + explicit StateInterface( const std::string & prefix_name, const std::string & interface_name, double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) - { - } - - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - - ReadWriteHandle(const ReadWriteHandle & other) = default; - - ReadWriteHandle(ReadWriteHandle && other) = default; - - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; - - ReadWriteHandle & operator=(ReadWriteHandle && other) = default; - - virtual ~ReadWriteHandle() = default; - - void set_value(double value) + : Handle(prefix_name, interface_name, value_ptr) { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; } -}; -class StateInterface : public ReadOnlyHandle -{ -public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using Handle::Handle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public Handle { public: + explicit CommandInterface( + const std::string & prefix_name, const std::string & interface_name, + double * value_ptr = nullptr) + : Handle(prefix_name, interface_name, value_ptr) + { + } /// CommandInterface copy constructor is actively deleted. /** * Command interfaces are having a unique ownership and thus @@ -134,7 +136,7 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using Handle::Handle; }; } // namespace hardware_interface diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index 024a019bfd..4c40189648 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -20,17 +20,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::Handle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::Handle::Handle; }; } // namespace transmission_interface From 80c264f024c06079707378e0bbaf0f361a1dff6c Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Fri, 16 Aug 2024 11:17:14 +0200 Subject: [PATCH 4/7] Robustify controller spawner and add integration test with many controllers (#1501) --------- Co-authored-by: Dr. Denis --- .../controller_manager_services.py | 54 ++++++++++++++++--- .../test/test_spawner_unspawner.cpp | 26 +++++++++ 2 files changed, 73 insertions(+), 7 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index be11a4b782..3f5a820fae 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -32,7 +32,39 @@ class ServiceNotFoundError(Exception): pass -def service_caller(node, service_name, service_type, request, service_timeout=0.0): +def service_caller( + node, + service_name, + service_type, + request, + service_timeout=0.0, + call_timeout=10.0, + max_attempts=3, +): + """ + Abstraction of a service call. + + Has an optional timeout to find the service, receive the answer to a call + and a mechanism to retry a call of no response is received. + + @param node Node object to be associated with + @type rclpy.node.Node + @param service_name Service URL + @type str + @param request The request to be sent + @type service request type + @param service_timeout Timeout (in seconds) to wait until the service is available. 0 means + waiting forever, retrying every 10 seconds. + @type float + @param call_timeout Timeout (in seconds) for getting a response + @type float + @param max_attempts Number of attempts until a valid response is received. With some + middlewares it can happen, that the service response doesn't reach the client leaving it in + a waiting state forever. + @type int + @return The service response + + """ cli = node.create_client(service_type, service_name) while not cli.service_is_ready(): @@ -44,12 +76,20 @@ def service_caller(node, service_name, service_type, request, service_timeout=0. node.get_logger().warn(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") - future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - return future.result() - else: - raise RuntimeError(f"Exception while calling service: {future.exception()}") + future = None + for attempt in range(max_attempts): + future = cli.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) + if future.result() is None: + node.get_logger().warning( + f"Failed getting a result from calling {service_name} in " + f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" + ) + else: + return future.result() + raise RuntimeError( + f"Could not successfully call service {service_name} after {max_attempts} attempts." + ) def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 3edf770436..db3940dbcc 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -374,6 +374,32 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) } } +TEST_F(TestLoadController, spawner_with_many_controllers) +{ + std::stringstream ss; + const size_t num_controllers = 50; + const std::string controller_base_name = "ctrl_"; + for (size_t i = 0; i < num_controllers; i++) + { + const std::string controller_name = controller_base_name + std::to_string(static_cast(i)); + cm_->set_parameter( + rclcpp::Parameter(controller_name + ".type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + ss << controller_name << " "; + } + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner(ss.str() + " -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), num_controllers); + + for (size_t i = 0; i < num_controllers; i++) + { + auto ctrl = cm_->get_loaded_controllers()[i]; + ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture { From 0631e3edecadd8b4a50ad2a3dc1374030167f71a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 12:21:41 +0200 Subject: [PATCH 5/7] Refactor spawner to be able to reuse code for ros2controlcli (#1661) --- .../controller_manager/__init__.py | 8 + .../controller_manager_services.py | 117 +++++++++++++ .../controller_manager/hardware_spawner.py | 14 +- .../controller_manager/spawner.py | 165 +----------------- .../test/test_spawner_unspawner.cpp | 13 +- .../ros2controlcli/verb/list_controllers.py | 3 +- .../verb/list_hardware_components.py | 3 +- .../verb/list_hardware_interfaces.py | 3 +- 8 files changed, 141 insertions(+), 185 deletions(-) diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py index f49bed4d34..4a8d7daee5 100644 --- a/controller_manager/controller_manager/__init__.py +++ b/controller_manager/controller_manager/__init__.py @@ -23,6 +23,10 @@ set_hardware_component_state, switch_controllers, unload_controller, + get_parameter_from_param_file, + set_controller_parameters, + set_controller_parameters_from_param_file, + bcolors, ) __all__ = [ @@ -36,4 +40,8 @@ "set_hardware_component_state", "switch_controllers", "unload_controller", + "get_parameter_from_param_file", + "set_controller_parameters", + "set_controller_parameters_from_param_file", + "bcolors", ] diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 3f5a820fae..7b0f726d68 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -26,6 +26,29 @@ ) import rclpy +import yaml +from rcl_interfaces.msg import Parameter + +# @note: The versions conditioning is added here to support the source-compatibility with Humble +# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0 +try: + from rclpy.parameter import get_parameter_value +except ImportError: + from ros2param.api import get_parameter_value +from ros2param.api import call_set_parameters + + +# from https://stackoverflow.com/a/287944 +class bcolors: + MAGENTA = "\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" class ServiceNotFoundError(Exception): @@ -220,3 +243,97 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti request, service_timeout, ) + + +def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): + with open(parameter_file) as f: + namespaced_controller = ( + controller_name if namespace == "/" else f"{namespace}/{controller_name}" + ) + parameters = yaml.safe_load(f) + if namespaced_controller in parameters: + value = parameters[namespaced_controller] + if not isinstance(value, dict) or "ros__parameters" not in value: + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + ) + if parameter_name in parameters[namespaced_controller]["ros__parameters"]: + return parameters[namespaced_controller]["ros__parameters"][parameter_name] + else: + return None + else: + return None + + +def set_controller_parameters( + node, controller_manager_name, controller_name, parameter_name, parameter_value +): + parameter = Parameter() + parameter.name = controller_name + "." + parameter_name + parameter_string = str(parameter_value) + parameter.value = get_parameter_value(string_value=parameter_string) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Setting controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) + return False + return True + + +def set_controller_parameters_from_param_file( + node, controller_manager_name, controller_name, parameter_file, namespace=None +): + if parameter_file: + spawner_namespace = namespace if namespace else node.get_namespace() + set_controller_parameters( + node, controller_manager_name, controller_name, "param_file", parameter_file + ) + + controller_type = get_parameter_from_param_file( + controller_name, spawner_namespace, parameter_file, "type" + ) + if controller_type: + if not set_controller_parameters( + node, controller_manager_name, controller_name, "type", controller_type + ): + return False + + fallback_controllers = get_parameter_from_param_file( + controller_name, spawner_namespace, parameter_file, "fallback_controllers" + ) + if fallback_controllers: + if not set_controller_parameters( + node, + controller_manager_name, + controller_name, + "fallback_controllers", + fallback_controllers, + ): + return False + return True diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 3e3a487c6a..29c0b5e97c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -19,6 +19,7 @@ from controller_manager import ( list_hardware_components, set_hardware_component_state, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -28,19 +29,6 @@ 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) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 0e83eb9d07..bd277d5464 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,7 +19,6 @@ import sys import time import warnings -import yaml from controller_manager import ( configure_controller, @@ -27,35 +26,14 @@ load_controller, switch_controllers, unload_controller, + set_controller_parameters_from_param_file, + bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError import rclpy -from rcl_interfaces.msg import Parameter from rclpy.node import Node - -# @note: The versions conditioning is added here to support the source-compatibility with Humble -# The `get_parameter_value` function is moved to `rclpy.parameter` module from `ros2param.api` module from version 3.6.0 -try: - from rclpy.parameter import get_parameter_value -except ImportError: - from ros2param.api import get_parameter_value from rclpy.signals import SignalHandlerOptions -from ros2param.api import call_set_parameters - -# from https://stackoverflow.com/a/287944 - - -class bcolors: - MAGENTA = "\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): @@ -87,24 +65,6 @@ def is_controller_loaded(node, controller_manager, controller_name, service_time return any(c.name == controller_name for c in controllers) -def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): - with open(parameter_file) as f: - namespaced_controller = ( - controller_name if namespace == "/" else f"{namespace}/{controller_name}" - ) - parameters = yaml.safe_load(f) - if namespaced_controller in parameters: - value = parameters[namespaced_controller] - if not isinstance(value, dict) or "ros__parameters" not in value: - raise RuntimeError( - f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" - ) - if parameter_name in parameters[namespaced_controller]["ros__parameters"]: - return parameters[namespaced_controller]["ros__parameters"][parameter_name] - else: - return None - - def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() @@ -162,14 +122,6 @@ def main(args=None): action="store_true", required=False, ) - parser.add_argument( - "--fallback_controllers", - help="Fallback controllers list are activated as a fallback strategy when the" - " spawned controllers fail. When the argument is provided, it takes precedence over" - " the fallback_controllers list in the param file", - default=None, - nargs="+", - ) command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) @@ -210,7 +162,6 @@ def main(args=None): try: for controller_name in controller_names: - fallback_controllers = args.fallback_controllers if is_controller_loaded( node, controller_manager_name, controller_name, controller_manager_timeout @@ -221,112 +172,14 @@ def main(args=None): + bcolors.ENDC ) else: - controller_type = ( - None - if param_file is None - else get_parameter_from_param_file( - controller_name, spawner_namespace, param_file, "type" - ) - ) - if controller_type: - parameter = Parameter() - parameter.name = controller_name + ".type" - parameter.value = get_parameter_value(string_value=controller_type) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller type to "' - + controller_type - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - return 1 - if param_file: - parameter = Parameter() - parameter.name = controller_name + ".params_file" - parameter.value = get_parameter_value(string_value=param_file) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller params file to "' - + param_file - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - return 1 - - if not fallback_controllers and param_file: - fallback_controllers = get_parameter_from_param_file( - controller_name, spawner_namespace, param_file, "fallback_controllers" - ) - - if fallback_controllers: - parameter = Parameter() - parameter.name = controller_name + ".fallback_controllers" - parameter.value = get_parameter_value(string_value=str(fallback_controllers)) - - response = call_set_parameters( - node=node, node_name=controller_manager_name, parameters=[parameter] - ) - assert len(response.results) == 1 - result = response.results[0] - if result.successful: - node.get_logger().info( - bcolors.OKCYAN - + 'Setting fallback_controllers to ["' - + ",".join(fallback_controllers) - + '"] for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) - else: - node.get_logger().fatal( - bcolors.FAIL - + 'Could not set fallback_controllers to ["' - + ",".join(fallback_controllers) - + '"] for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) + if not set_controller_parameters_from_param_file( + node, + controller_manager_name, + controller_name, + param_file, + spawner_namespace, + ): return 1 ret = load_controller(node, controller_manager_name, controller_name) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index db3940dbcc..852ea90bc7 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -329,20 +329,14 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ( - call_spawner( - "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " - "-p " + - test_file_path), - 0); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager --load-only -p " + test_file_path), 0); ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); { auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } @@ -355,8 +349,7 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers) auto ctrl_1 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); + ASSERT_TRUE(ctrl_1.info.fallback_controllers_names.empty()); ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); auto ctrl_2 = cm_->get_loaded_controllers()[1]; diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py index a26a1168a1..3daf752b8c 100644 --- a/ros2controlcli/ros2controlcli/verb/list_controllers.py +++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_controllers -from controller_manager.spawner import bcolors +from controller_manager import list_controllers, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py index e2b427b233..e3e2269920 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_components -from controller_manager.spawner import bcolors +from controller_manager import list_hardware_components, bcolors from lifecycle_msgs.msg import State diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py index 7aa850f3bc..4510998ad9 100644 --- a/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py +++ b/ros2controlcli/ros2controlcli/verb/list_hardware_interfaces.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from controller_manager import list_hardware_interfaces -from controller_manager.spawner import bcolors +from controller_manager import list_hardware_interfaces, bcolors from ros2cli.node.direct import add_arguments from ros2cli.node.strategy import NodeStrategy From 079392b94867c3372c0050492327001057118090 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 12:23:33 +0200 Subject: [PATCH 6/7] Fix spawner tests timeout (#1692) --- controller_manager/test/test_spawner_unspawner.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 852ea90bc7..49326f9e79 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -255,6 +255,7 @@ TEST_F(TestLoadController, spawner_test_type_in_params_file) const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( @@ -306,6 +307,7 @@ TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill // timeout command will kill it after the specified time with signal SIGINT + ControllerManagerRunner cm_runner(this); cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); std::stringstream ss; ss << "timeout --signal=INT 5 " @@ -622,6 +624,7 @@ TEST_F(TestLoadControllerWithNamespacedCM, spawner_test_type_in_params_file) const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( @@ -682,6 +685,7 @@ TEST_F( const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + "/test/test_controller_spawner_with_type.yaml"; + ControllerManagerRunner cm_runner(this); // Provide controller type via the parsed file EXPECT_EQ( call_spawner( From 4498d25ff7cfdc40780a34b36cf4d1fea40f7521 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 19 Aug 2024 13:40:45 +0200 Subject: [PATCH 7/7] Avoid using the global arguments for internal controller nodes (#1694) --- .../controller_interface_base.hpp | 5 +++-- .../test/test_controller_interface.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 9eaee9f15b..e881f08aae 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -174,11 +174,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy { // \note The versions conditioning is added here to support the source-compatibility with Humble #if RCLCPP_VERSION_MAJOR >= 21 - return rclcpp::NodeOptions().enable_logger_service(true); + return rclcpp::NodeOptions().enable_logger_service(true).use_global_arguments(false); #else return rclcpp::NodeOptions() .allow_undeclared_parameters(true) - .automatically_declare_parameters_from_overrides(true); + .automatically_declare_parameters_from_overrides(true) + .use_global_arguments(false); #endif } diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp index 03838b1a2e..f5c0a6b3de 100644 --- a/controller_interface/test/test_controller_interface.cpp +++ b/controller_interface/test/test_controller_interface.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include #include "rclcpp/executor_options.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -57,13 +59,16 @@ TEST(TestableControllerInterface, init) TEST(TestableControllerInterface, setting_update_rate_in_configure) { // mocks the declaration of overrides parameters in a yaml file - char const * const argv[] = {"", "--ros-args", "-p", "update_rate:=2812"}; - int argc = arrlen(argv); - rclcpp::init(argc, argv); + rclcpp::init(0, nullptr); TestableControllerInterface controller; // initialize, create node - const auto node_options = controller.define_custom_node_options(); + auto node_options = controller.define_custom_node_options(); + std::vector node_options_arguments = node_options.arguments(); + node_options_arguments.push_back("--ros-args"); + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("update_rate:=2812"); + node_options = node_options.arguments(node_options_arguments); ASSERT_EQ( controller.init(TEST_CONTROLLER_NAME, "", 1.0, "", node_options), controller_interface::return_type::OK);