Fix rqt controller manager crash on ros2_control restart (#1273) (#1276)

ros-controls · Jan 8, 2024 · fc4d31e · fc4d31e
1 parent 7556a50
commit fc4d31e
Show file tree

Hide file tree

Showing 2 changed files with 56 additions and 24 deletions.
diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py
@@ -47,68 +47,89 @@ 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
+        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
+        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
+        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,
+        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,
+        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
+        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,
+        service_timeout,
     )
 
 
-def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state):
+def set_hardware_component_state(
+    node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0
+):
     request = SetHardwareComponentState.Request()
     request.name = component_name
     request.target_state = lifecyle_state
@@ -117,6 +138,7 @@ def set_hardware_component_state(node, controller_manager_name, component_name,
         f"{controller_manager_name}/set_hardware_component_state",
         SetHardwareComponentState,
         request,
+        service_timeout,
     )
 
 
@@ -143,9 +165,13 @@ def switch_controllers(
     )
 
 
-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
+        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
@@ -172,16 +172,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