Skip to content

Commit

Permalink
Update spawner to have a timeout on service call
Browse files Browse the repository at this point in the history
In some cases, the service call might never return. As a consequence,
the call to spin_until_future_complete will loop infinitely.
  • Loading branch information
agennart committed Jun 7, 2024
1 parent d61abcd commit 049a5a8
Showing 1 changed file with 19 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
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=10.0, call_timeout=10.0):
cli = node.create_client(service_type, service_name)

if not cli.service_is_ready():
Expand All @@ -40,14 +40,14 @@ def service_caller(node, service_name, service_type, request, service_timeout=10

node.get_logger().debug(f"requester: making request: {request}\n")
future = cli.call_async(request)
rclpy.spin_until_future_complete(node, future)
rclpy.spin_until_future_complete(node, future, timeout_sec=10.0)
if future.result() is not None:
return future.result()
else:
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=10.0, call_timeout=10.0):
request = ConfigureController.Request()
request.name = controller_name
return service_caller(
Expand All @@ -59,51 +59,55 @@ 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=10.0, call_timeout=10.0):
request = ListControllers.Request()
return service_caller(
node,
f"{controller_manager_name}/list_controllers",
ListControllers,
request,
service_timeout,
call_timeout,
)


def list_controller_types(node, controller_manager_name, service_timeout=10.0):
def list_controller_types(node, controller_manager_name, service_timeout=10.0, call_timeout=10.0):
request = ListControllerTypes.Request()
return service_caller(
node,
f"{controller_manager_name}/list_controller_types",
ListControllerTypes,
request,
service_timeout,
call_timeout,
)


def list_hardware_components(node, controller_manager_name, service_timeout=10.0):
def list_hardware_components(node, controller_manager_name, service_timeout=10.0, call_timeout=10.0):
request = ListHardwareComponents.Request()
return service_caller(
node,
f"{controller_manager_name}/list_hardware_components",
ListHardwareComponents,
request,
service_timeout,
call_timeout,
)


def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0):
def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0, call_timeout=10.0):
request = ListHardwareInterfaces.Request()
return service_caller(
node,
f"{controller_manager_name}/list_hardware_interfaces",
ListHardwareInterfaces,
request,
service_timeout,
call_timeout,
)


def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0, call_timeout=10.0):
request = LoadController.Request()
request.name = controller_name
return service_caller(
Expand All @@ -112,10 +116,11 @@ 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=10.0):
def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0, call_timeout=10.0):
request = ReloadControllerLibraries.Request()
request.force_kill = force_kill
return service_caller(
Expand All @@ -124,11 +129,12 @@ 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=10.0
node, controller_manager_name, component_name, lifecyle_state, service_timeout=10, call_timeout=10.0
):
request = SetHardwareComponentState.Request()
request.name = component_name
Expand All @@ -139,6 +145,7 @@ def set_hardware_component_state(
SetHardwareComponentState,
request,
service_timeout,
call_timeout,
)


Expand All @@ -165,7 +172,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=10.0, call_timeout=10.0):
request = UnloadController.Request()
request.name = controller_name
return service_caller(
Expand All @@ -174,4 +181,5 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
UnloadController,
request,
service_timeout,
call_timeout,
)

0 comments on commit 049a5a8

Please sign in to comment.