Skip to content

Commit

Permalink
Add call timeout
Browse files Browse the repository at this point in the history 
Signed-off-by: Angsa Deployment Team <team@angsa-robotics.com>
  • Loading branch information
Angsa Deployment Team committed Oct 22, 2024
1 parent d55def1 commit 63de32d
Show file tree
Hide file tree
Showing 2 changed files with 94 additions and 20 deletions.
55 changes: 43 additions & 12 deletions controller_manager/controller_manager/controller_manager_services.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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(
Expand All @@ -132,54 +134,65 @@ 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,
f"{controller_manager_name}/list_controllers",
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,
f"{controller_manager_name}/list_controller_types",
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,
f"{controller_manager_name}/list_hardware_components",
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,
f"{controller_manager_name}/list_hardware_interfaces",
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(
Expand All @@ -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(
Expand All @@ -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
Expand All @@ -215,6 +237,7 @@ def set_hardware_component_state(
SetHardwareComponentState,
request,
service_timeout,
call_timeout,
)


Expand All @@ -226,6 +249,7 @@ def switch_controllers(
strict,
activate_asap,
timeout,
call_timeout=10.0,
):
request = SwitchController.Request()
request.activate_controllers = activate_controllers
Expand All @@ -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(
Expand All @@ -250,6 +280,7 @@ def unload_controller(node, controller_manager_name, controller_name, service_ti
UnloadController,
request,
service_timeout,
call_timeout,
)


Expand Down
59 changes: 51 additions & 8 deletions controller_manager/controller_manager/spawner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)


Expand Down Expand Up @@ -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."
Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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(
Expand All @@ -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(
Expand All @@ -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(
Expand Down

0 comments on commit 63de32d

Please sign in to comment.