From 63de32d96092e7952d25c0f49e5c9734817f7ee9 Mon Sep 17 00:00:00 2001 From: Angsa Deployment Team Date: Tue, 22 Oct 2024 11:41:12 +0200 Subject: [PATCH 1/7] Add call timeout Signed-off-by: Angsa Deployment Team --- .../controller_manager_services.py | 55 +++++++++++++---- .../controller_manager/spawner.py | 59 ++++++++++++++++--- 2 files changed, 94 insertions(+), 20 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 102531f8e8..9045be12ae 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -113,8 +113,8 @@ def service_caller( 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 {fully_qualified_service_name} in " - f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" + f"Failed getting a result from calling {service_name} in " + f"{call_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: return future.result() @@ -123,7 +123,9 @@ def service_caller( ) -def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def configure_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = ConfigureController.Request() request.name = controller_name return service_caller( @@ -132,10 +134,11 @@ def configure_controller(node, controller_manager_name, controller_name, service ConfigureController, request, service_timeout, + call_timeout, ) -def list_controllers(node, controller_manager_name, service_timeout=0.0): +def list_controllers(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): request = ListControllers.Request() return service_caller( node, @@ -143,10 +146,11 @@ def list_controllers(node, controller_manager_name, service_timeout=0.0): ListControllers, request, service_timeout, + call_timeout, ) -def list_controller_types(node, controller_manager_name, service_timeout=0.0): +def list_controller_types(node, controller_manager_name, service_timeout=0.0, call_timeout=10.0): request = ListControllerTypes.Request() return service_caller( node, @@ -154,10 +158,13 @@ def list_controller_types(node, controller_manager_name, service_timeout=0.0): ListControllerTypes, request, service_timeout, + call_timeout, ) -def list_hardware_components(node, controller_manager_name, service_timeout=0.0): +def list_hardware_components( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): request = ListHardwareComponents.Request() return service_caller( node, @@ -165,10 +172,13 @@ def list_hardware_components(node, controller_manager_name, service_timeout=0.0) ListHardwareComponents, request, service_timeout, + call_timeout, ) -def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0): +def list_hardware_interfaces( + node, controller_manager_name, service_timeout=0.0, call_timeout=10.0 +): request = ListHardwareInterfaces.Request() return service_caller( node, @@ -176,10 +186,13 @@ def list_hardware_interfaces(node, controller_manager_name, service_timeout=0.0) ListHardwareInterfaces, request, service_timeout, + call_timeout, ) -def load_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def load_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = LoadController.Request() request.name = controller_name return service_caller( @@ -188,10 +201,13 @@ def load_controller(node, controller_manager_name, controller_name, service_time LoadController, request, service_timeout, + call_timeout, ) -def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=0.0): +def reload_controller_libraries( + node, controller_manager_name, force_kill, service_timeout=0.0, call_timeout=10.0 +): request = ReloadControllerLibraries.Request() request.force_kill = force_kill return service_caller( @@ -200,11 +216,17 @@ def reload_controller_libraries(node, controller_manager_name, force_kill, servi ReloadControllerLibraries, request, service_timeout, + call_timeout, ) def set_hardware_component_state( - node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0 + node, + controller_manager_name, + component_name, + lifecyle_state, + service_timeout=0.0, + call_timeout=10.0, ): request = SetHardwareComponentState.Request() request.name = component_name @@ -215,6 +237,7 @@ def set_hardware_component_state( SetHardwareComponentState, request, service_timeout, + call_timeout, ) @@ -226,6 +249,7 @@ def switch_controllers( strict, activate_asap, timeout, + call_timeout=10.0, ): request = SwitchController.Request() request.activate_controllers = activate_controllers @@ -237,11 +261,17 @@ def switch_controllers( request.activate_asap = activate_asap request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg() return service_caller( - node, f"{controller_manager_name}/switch_controller", SwitchController, request + node, + f"{controller_manager_name}/switch_controller", + SwitchController, + request, + call_timeout=call_timeout, ) -def unload_controller(node, controller_manager_name, controller_name, service_timeout=0.0): +def unload_controller( + node, controller_manager_name, controller_name, service_timeout=0.0, call_timeout=10.0 +): request = UnloadController.Request() request.name = controller_name return service_caller( @@ -250,6 +280,7 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti UnloadController, request, service_timeout, + call_timeout, ) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index d6df3be01f..3fcf7c11c2 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -60,8 +60,12 @@ def has_service_names(node, node_name, node_namespace, service_names): return all(service in client_names for service in service_names) -def is_controller_loaded(node, controller_manager, controller_name, service_timeout=0.0): - controllers = list_controllers(node, controller_manager, service_timeout).controller +def is_controller_loaded( + node, controller_manager, controller_name, service_timeout=0.0, call_timeout=10.0 +): + controllers = list_controllers( + node, controller_manager, service_timeout, call_timeout + ).controller return any(c.name == controller_name for c in controllers) @@ -110,11 +114,18 @@ def main(args=None): ) parser.add_argument( "--controller-manager-timeout", - help="Time to wait for the controller manager", + help="Time to wait for the controller manager service to be available", required=False, default=0, type=float, ) + parser.add_argument( + "--controller-manager-call-timeout", + help="Time to wait for the service response from the controller manager", + required=False, + default=10.0, + type=float, + ) parser.add_argument( "--activate-as-group", help="Activates all the parsed controllers list together instead of one by one." @@ -129,6 +140,7 @@ def main(args=None): controller_manager_name = args.controller_manager param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout + controller_manager_call_timeout = args.controller_manager_call_timeout if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -164,7 +176,11 @@ def main(args=None): for controller_name in controller_names: if is_controller_loaded( - node, controller_manager_name, controller_name, controller_manager_timeout + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + controller_manager_call_timeout, ): node.get_logger().warn( bcolors.WARNING @@ -197,7 +213,13 @@ def main(args=None): ) if not args.load_only: - ret = configure_controller(node, controller_manager_name, controller_name) + ret = configure_controller( + node, + controller_manager_name, + controller_name, + controller_manager_timeout, + controller_manager_call_timeout, + ) if not ret.ok: node.get_logger().error( bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC @@ -206,7 +228,14 @@ def main(args=None): if not args.inactive and not args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], [controller_name], True, True, 5.0 + node, + controller_manager_name, + [], + [controller_name], + True, + True, + controller_manager_timeout, + controller_manager_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -224,7 +253,14 @@ def main(args=None): if not args.inactive and args.activate_as_group: ret = switch_controllers( - node, controller_manager_name, [], controller_names, True, True, 5.0 + node, + controller_manager_name, + [], + controller_names, + True, + True, + controller_manager_timeout, + controller_manager_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -250,7 +286,14 @@ def main(args=None): node.get_logger().info("Interrupt captured, deactivating and unloading controller") # TODO(saikishor) we might have an issue in future, if any of these controllers is in chained mode ret = switch_controllers( - node, controller_manager_name, controller_names, [], True, True, 5.0 + node, + controller_manager_name, + controller_names, + [], + True, + True, + controller_manager_timeout, + controller_manager_call_timeout, ) if not ret.ok: node.get_logger().error( From 26c79d33b37848a7a47c15800d8eaf238f2f9b8e Mon Sep 17 00:00:00 2001 From: Angsa Deployment Team Date: Tue, 22 Oct 2024 12:04:18 +0200 Subject: [PATCH 2/7] fix cherrypick Signed-off-by: Angsa Deployment Team --- .../controller_manager/controller_manager_services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 9045be12ae..7d958920f3 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -113,7 +113,7 @@ def service_caller( 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"Failed getting a result from calling {fully_qualified_service_name} in " f"{call_timeout}. (Attempt {attempt+1} of {max_attempts}.)" ) else: From 5735f0da76f41cbf45547350f77a8c053cda0f96 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 6 Nov 2024 12:05:47 +0100 Subject: [PATCH 3/7] Add docs Signed-off-by: Tony Najjar --- controller_manager/doc/userdoc.rst | 6 ++++-- doc/release_notes.rst | 1 + 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index fa673cff0a..4967ef2bd8 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -158,7 +158,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-t CONTROLLER_TYPE] [-u] - [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] + [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT] controller_name positional arguments: @@ -176,7 +176,9 @@ There are two scripts to interact with controller manager from launch files: --inactive Load and configure the controller, however do not activate them -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT - Time to wait for the controller manager + Time to wait for the controller + --controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT + Time to wait for the service response from the controller manager --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether --fallback_controllers FALLBACK_CONTROLLERS [FALLBACK_CONTROLLERS ...] 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 diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 12ded009dc..7b47dbcf5b 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -79,6 +79,7 @@ controller_manager * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). +* The ``--controller-manager-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). hardware_interface ****************** From 620e2acc1d6fc653019d2ccb451e0391ec1dbec0 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 6 Nov 2024 13:31:01 +0100 Subject: [PATCH 4/7] Update controller_manager/doc/userdoc.rst MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- controller_manager/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 4967ef2bd8..c72f0b2fb3 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -176,7 +176,7 @@ There are two scripts to interact with controller manager from launch files: --inactive Load and configure the controller, however do not activate them -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT - Time to wait for the controller + Time to wait for the controller manager service to be available --controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT Time to wait for the service response from the controller manager --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether From a495547d05fd56deb5a2eb7430533bb19f0dac62 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 22 Nov 2024 12:55:23 +0100 Subject: [PATCH 5/7] fix order Signed-off-by: Tony Najjar --- controller_manager/doc/userdoc.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 9b538338ce..bb35a45999 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -176,12 +176,12 @@ There are two scripts to interact with controller manager from launch files: --inactive Load and configure the controller, however do not activate them -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT - Time to wait for the controller manager + Time to wait for the controller manager service to be available + --controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT + Time to wait for the service response from the controller manager --switch-timeout SWITCH_TIMEOUT Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup - --controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT - Time to wait for the service response from the controller manager --activate-as-group Activates all the parsed controllers list together instead of one by one. Useful for activating all chainable controllers altogether From b5372605397b59117bb55a1219f707d53b0c2fa3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 19:20:44 +0100 Subject: [PATCH 6/7] rename Signed-off-by: Tony Najjar --- controller_manager/controller_manager/spawner.py | 14 +++++++------- controller_manager/doc/userdoc.rst | 4 ++-- doc/release_notes.rst | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 7afc2dc906..5d5e34e2a4 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -129,7 +129,7 @@ def main(args=None): type=float, ) parser.add_argument( - "--controller-manager-call-timeout", + "--service-call-timeout", help="Time to wait for the service response from the controller manager", required=False, default=10.0, @@ -149,7 +149,7 @@ def main(args=None): controller_manager_name = args.controller_manager param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout - controller_manager_call_timeout = args.controller_manager_call_timeout + service_call_timeout = args.service_call_timeout switch_timeout = args.switch_timeout if param_file and not os.path.isfile(param_file): @@ -190,7 +190,7 @@ def main(args=None): controller_manager_name, controller_name, controller_manager_timeout, - controller_manager_call_timeout, + service_call_timeout, ): node.get_logger().warn( bcolors.WARNING @@ -228,7 +228,7 @@ def main(args=None): controller_manager_name, controller_name, controller_manager_timeout, - controller_manager_call_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -245,7 +245,7 @@ def main(args=None): True, True, switch_timeout, - controller_manager_call_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -270,7 +270,7 @@ def main(args=None): True, True, switch_timeout, - controller_manager_call_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -303,7 +303,7 @@ def main(args=None): True, True, switch_timeout, - controller_manager_call_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index bb35a45999..ade33be47f 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -158,7 +158,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout service_call_timeout] controller_names [controller_names ...] positional arguments: @@ -177,7 +177,7 @@ There are two scripts to interact with controller manager from launch files: -u, --unload-on-kill Wait until this application is interrupted and unload controller --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT Time to wait for the controller manager service to be available - --controller-manager-call-timeout CONTROLLER_MANAGER_CALL_TIMEOUT + --service-call-timeout SERVICE_CALL_TIMEOUT Time to wait for the service response from the controller manager --switch-timeout SWITCH_TIMEOUT Time to wait for a successful state switch of controllers. Useful if controllers cannot be switched immediately, e.g., paused diff --git a/doc/release_notes.rst b/doc/release_notes.rst index fbc969e890..52fcf7cc20 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -81,7 +81,7 @@ controller_manager * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_). * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 `_). -* The ``--controller-manager-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). +* The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 `_). hardware_interface ****************** From 22d383c6081389ffe603cc5d7d5ae805620d82a4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 19:28:13 +0100 Subject: [PATCH 7/7] Update controller_manager/doc/userdoc.rst Co-authored-by: Sai Kishor Kothakota --- controller_manager/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index ade33be47f..1ac867fa9b 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -158,7 +158,7 @@ There are two scripts to interact with controller manager from launch files: $ ros2 run controller_manager spawner -h usage: spawner [-h] [-c CONTROLLER_MANAGER] [-p PARAM_FILE] [-n NAMESPACE] [--load-only] [--inactive] [-u] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] - [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout service_call_timeout] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] controller_names [controller_names ...] positional arguments: