From b1036b78049210af9686f91b12c78a59afac57f8 Mon Sep 17 00:00:00 2001
From: Bijou Abraham <Bijou.Abraham@technipfmc.com>
Date: Mon, 16 Dec 2024 12:59:38 -0800
Subject: [PATCH] Reuse service client for each service type

---
 .../controller_manager/__init__.py            |  2 ++
 .../controller_manager_services.py            | 21 ++++++++++++++++++-
 .../controller_manager/spawner.py             |  4 ++--
 3 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/controller_manager/controller_manager/__init__.py b/controller_manager/controller_manager/__init__.py
index 638a28ce86..e42ae50763 100644
--- a/controller_manager/controller_manager/__init__.py
+++ b/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",
diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py
index bfe36fe3b7..2dffaf03c1 100644
--- a/controller_manager/controller_manager/controller_manager_services.py
+++ b/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(
diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
index c5a23defe4..6d821866b4 100644
--- a/controller_manager/controller_manager/spawner.py
+++ b/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(