diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 909e681ce6..7d958920f3 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -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 7e9fe3443b..5d5e34e2a4 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,7 +114,7 @@ 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.0, type=float, @@ -124,6 +128,13 @@ def main(args=None): default=5.0, type=float, ) + parser.add_argument( + "--service-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." @@ -138,6 +149,7 @@ def main(args=None): controller_manager_name = args.controller_manager param_file = args.param_file controller_manager_timeout = args.controller_manager_timeout + service_call_timeout = args.service_call_timeout switch_timeout = args.switch_timeout if param_file and not os.path.isfile(param_file): @@ -174,7 +186,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, + service_call_timeout, ): node.get_logger().warn( bcolors.WARNING @@ -207,7 +223,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, + service_call_timeout, + ) if not ret.ok: node.get_logger().error( bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC @@ -223,6 +245,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -247,6 +270,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -279,6 +303,7 @@ def main(args=None): True, True, switch_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 ca222d68c0..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] + [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] [--service-call-timeout SERVICE_CALL_TIMEOUT] controller_names [controller_names ...] 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 manager service to be available + --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 simulations at startup diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 09a9236d79..52fcf7cc20 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -81,6 +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 ``--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 ******************