diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 5baec140c4..d9d55058fd 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -28,14 +28,16 @@ import rclpy -def service_caller(node, service_name, service_type, request, service_timeout=10.0): +def service_caller(node, service_name, service_type, request, service_timeout=0.0): cli = node.create_client(service_type, service_name) while 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): + node.get_logger().debug(f"waiting for service {service_name} to become available...") + if service_timeout: + if not cli.wait_for_service(service_timeout): + node.get_logger().fatal(f"Could not contact service {service_name}") + raise RuntimeError(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") @@ -47,7 +49,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 +61,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 +72,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 +83,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 +94,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 +105,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 +117,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( @@ -127,9 +129,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 -): +def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state, service_timeout=0.0): request = SetHardwareComponentState.Request() request.name = component_name request.target_state = lifecyle_state @@ -165,7 +165,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/spawner.py b/controller_manager/controller_manager/spawner.py index 95404266f9..f0fee848cb 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -80,8 +80,8 @@ 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): - 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) @@ -130,6 +130,13 @@ def main(args=None): help="Wait until this application is interrupted and unload controller", action="store_true", ) + parser.add_argument( + "--controller-manager-timeout", + help="Time to wait for the controller manager", + required=False, + default=0, + type=float, + ) parser.add_argument( "--activate-as-group", help="Activates all the parsed controllers list together instead of one by one." @@ -145,6 +152,7 @@ def main(args=None): controller_namespace = args.namespace param_file = args.param_file controller_type = args.controller_type + timeout = args.controller_manager_timeout if param_file and not os.path.isfile(param_file): raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file) @@ -164,7 +172,7 @@ def main(args=None): if controller_namespace: prefixed_controller_name = controller_namespace + "/" + controller_name - if is_controller_loaded(node, controller_manager_name, prefixed_controller_name): + if is_controller_loaded(node, controller_manager_name, prefixed_controller_name, timeout): node.get_logger().warn( bcolors.WARNING + "Controller already loaded, skipping load_controller" diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 193e2095e2..5037c8fdcb 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -146,6 +146,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_name positional arguments: @@ -164,6 +165,8 @@ There are two scripts to interact with controller manager from launch files: -t CONTROLLER_TYPE, --controller-type CONTROLLER_TYPE 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 ``unspawner``