From 0e941193469e9b990d903e4bd3d453b586522411 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 9 Jan 2024 09:11:20 +0100 Subject: [PATCH] Fix rqt controller manager crash on ros2_control restart (#1273) (#1280) --- .../controller_manager_services.py | 90 +++++++++++++------ .../controller_manager.py | 27 +++--- 2 files changed, 80 insertions(+), 37 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 8fb2c1aca7..64e91a08ba 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -37,51 +37,84 @@ 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): +def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = ConfigureController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/configure_controller', - ConfigureController, request) + return service_caller( + node, + f"{controller_manager_name}/configure_controller", + ConfigureController, + request, + service_timeout, + ) -def list_controllers(node, controller_manager_name): +def list_controllers(node, controller_manager_name, service_timeout=10.0): request = ListControllers.Request() - return service_caller(node, f'{controller_manager_name}/list_controllers', - ListControllers, request) + return service_caller( + node, + f"{controller_manager_name}/list_controllers", + ListControllers, + request, + service_timeout, + ) -def list_controller_types(node, controller_manager_name): +def list_controller_types(node, controller_manager_name, service_timeout=10.0): request = ListControllerTypes.Request() - return service_caller(node, - f'{controller_manager_name}/list_controller_types', - ListControllerTypes, request) + return service_caller( + node, + f"{controller_manager_name}/list_controller_types", + ListControllerTypes, + request, + service_timeout, + ) -def list_hardware_components(node, controller_manager_name): +def list_hardware_components(node, controller_manager_name, service_timeout=10.0): request = ListHardwareComponents.Request() - return service_caller(node, f'{controller_manager_name}/list_hardware_components', - ListHardwareComponents, request) + return service_caller( + node, + f"{controller_manager_name}/list_hardware_components", + ListHardwareComponents, + request, + service_timeout, + ) -def list_hardware_interfaces(node, controller_manager_name): +def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0): request = ListHardwareInterfaces.Request() - return service_caller(node, f'{controller_manager_name}/list_hardware_interfaces', - ListHardwareInterfaces, request) + return service_caller( + node, + f"{controller_manager_name}/list_hardware_interfaces", + ListHardwareInterfaces, + request, + service_timeout, + ) -def load_controller(node, controller_manager_name, controller_name): +def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = LoadController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/load_controller', - LoadController, request) + return service_caller( + node, + f"{controller_manager_name}/load_controller", + LoadController, + request, + service_timeout, + ) -def reload_controller_libraries(node, controller_manager_name, force_kill): +def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0): request = ReloadControllerLibraries.Request() request.force_kill = force_kill - return service_caller(node, - f'{controller_manager_name}/reload_controller_libraries', - ReloadControllerLibraries, request) + return service_caller( + node, + f"{controller_manager_name}/reload_controller_libraries", + ReloadControllerLibraries, + request, + service_timeout, + ) def switch_controllers(node, controller_manager_name, deactivate_controllers, @@ -99,8 +132,13 @@ def switch_controllers(node, controller_manager_name, deactivate_controllers, SwitchController, request) -def unload_controller(node, controller_manager_name, controller_name): +def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0): request = UnloadController.Request() request.name = controller_name - return service_caller(node, f'{controller_manager_name}/unload_controller', - UnloadController, request) + return service_caller( + node, + f"{controller_manager_name}/unload_controller", + UnloadController, + request, + service_timeout, + ) diff --git a/rqt_controller_manager/rqt_controller_manager/controller_manager.py b/rqt_controller_manager/rqt_controller_manager/controller_manager.py index cbd6227409..54a3dc5c77 100644 --- a/rqt_controller_manager/rqt_controller_manager/controller_manager.py +++ b/rqt_controller_manager/rqt_controller_manager/controller_manager.py @@ -168,17 +168,22 @@ def _list_controllers(self): @rtype [str] """ # Add loaded controllers first - controllers = list_controllers(self._node, self._cm_name).controller - - # Append potential controller configs found in the node's parameters - for name in _get_parameter_controller_names(self._node, self._cm_name): - add_ctrl = all(name != ctrl.name for ctrl in controllers) - if add_ctrl: - type_str = _get_controller_type(self._node, self._cm_name, name) - uninit_ctrl = ControllerState(name=name, - type=type_str) - controllers.append(uninit_ctrl) - return controllers + try: + controllers = list_controllers( + self._node, self._cm_name, 2.0 / self._cm_update_freq + ).controller + + # Append potential controller configs found in the node's parameters + for name in _get_parameter_controller_names(self._node, self._cm_name): + add_ctrl = all(name != ctrl.name for ctrl in controllers) + if add_ctrl: + type_str = _get_controller_type(self._node, self._cm_name, name) + uninit_ctrl = ControllerState(name=name, type=type_str) + controllers.append(uninit_ctrl) + return controllers + except RuntimeError as e: + print(e) + return [] def _show_controllers(self): table_view = self._widget.table_view