diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index cc0b6f4645..330e320052 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -115,7 +115,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( @@ -124,10 +126,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, @@ -135,10 +138,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, @@ -146,10 +150,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, @@ -157,10 +164,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, @@ -168,10 +178,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( @@ -180,10 +193,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( @@ -192,11 +208,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 @@ -206,6 +228,8 @@ def set_hardware_component_state( f"{controller_manager_name}/set_hardware_component_state", SetHardwareComponentState, request, + service_timeout, + call_timeout, ) @@ -217,6 +241,7 @@ def switch_controllers( strict, activate_asap, timeout, + call_timeout=10.0, ): request = SwitchController.Request() request.activate_controllers = activate_controllers @@ -228,11 +253,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( @@ -241,6 +272,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 83b1e2f89d..47911296fa 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -61,8 +61,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) @@ -120,7 +124,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, @@ -134,6 +138,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." @@ -148,6 +159,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): @@ -175,7 +187,11 @@ def main(args=None): try: 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 @@ -217,7 +233,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 @@ -233,6 +255,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -257,6 +280,7 @@ def main(args=None): True, True, switch_timeout, + service_call_timeout, ) if not ret.ok: node.get_logger().error( @@ -291,6 +315,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 8f1c57161c..a2f0c550da 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -91,7 +91,8 @@ 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] [--stopped] [--inactive] [-t CONTROLLER_TYPE] [-u] - [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] [--activate-as-group] + [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] [--switch-timeout SWITCH_TIMEOUT] + [--service-call-timeout SERVICE_CALL_TIMEOUT] [--activate-as-group] controller_names [controller_names ...] positional arguments: @@ -112,7 +113,9 @@ There are two scripts to interact with controller manager from launch files: If not provided it should exist in the controller manager namespace -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 when switching cannot be performed immediately, e.g., paused simulations at startup diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 400e234d23..38a59adba8 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,3 +22,4 @@ controller_manager * 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 `_). * ``--switch-timeout`` was added as parameter to the helper scripts ``spawner.py`` and ``unspawner.py``. Useful if controllers cannot be switched immediately, e.g., paused simulations at startup (`#1790 `_). +* 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 `_).