Reuse service client for each service type

controller_manager/controller_manager/__init__.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 from .controller_manager_services import (
+    ServiceCallerNode,
     configure_controller,
     list_controller_types,
     list_controllers,
@@ -30,6 +31,7 @@
 )
 
 __all__ = [
+    "ServiceCallerNode",
     "configure_controller",
     "list_controller_types",
     "list_controllers",

controller_manager/controller_manager/controller_manager_services.py

@@ -26,6 +26,7 @@
 )
 
 import rclpy
+from rclpy.node import Node
 import yaml
 from rcl_interfaces.msg import Parameter
 
@@ -55,6 +56,18 @@ class ServiceNotFoundError(Exception):
     pass
 
 
+class ServiceCallerNode(Node):
+    def __init__(self, node_name: str):
+        super().__init__(node_name)
+        # initialize dictionary to store service clients
+        self.service_clients = {}
+
+    def __del__(self):
+        # destroy all service clients
+        for key in self.service_clients:
+            self.destroy_client(self.service_clients[key])
+
+
 def service_caller(
     node,
     service_name,
@@ -92,7 +105,13 @@ def service_caller(
     fully_qualified_service_name = (
         f"{namespace}/{service_name}" if not service_name.startswith("/") else service_name
     )
-    cli = node.create_client(service_type, fully_qualified_service_name)
+
+    if (service_type, fully_qualified_service_name) not in node.service_clients:
+        cli = node.create_client(service_type, fully_qualified_service_name)
+        # save client for next time so we're not creating a new one every time
+        node.service_clients[(service_type, fully_qualified_service_name)] = cli
+    else:
+        cli = node.service_clients[(service_type, fully_qualified_service_name)]
 
     while not cli.service_is_ready():
         node.get_logger().info(

controller_manager/controller_manager/spawner.py

@@ -21,6 +21,7 @@
 import warnings
 
 from controller_manager import (
+    ServiceCallerNode,
     configure_controller,
     list_controllers,
     load_controller,
@@ -32,7 +33,6 @@
 from controller_manager.controller_manager_services import ServiceNotFoundError
 
 import rclpy
-from rclpy.node import Node
 from rclpy.signals import SignalHandlerOptions
 
 
@@ -160,7 +160,7 @@ def main(args=None):
             if not os.path.isfile(param_file):
                 raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
 
-    node = Node("spawner_" + controller_names[0])
+    node = ServiceCallerNode("spawner_" + controller_names[0])
 
     if node.get_namespace() != "/" and args.namespace:
         raise RuntimeError(