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

ros-controls · Jan 9, 2024 · 0e94119 · 0e94119
1 parent f4f875a
commit 0e94119
Show file tree

Hide file tree

Showing 2 changed files with 80 additions and 37 deletions.
diff --git 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
@@ -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