From 111900679d3f594dfb8f57d660b1199f0f381963 Mon Sep 17 00:00:00 2001
From: Bartok9 <danielrpike9@gmail.com>
Date: Fri, 10 Jul 2026 20:02:01 -0400
Subject: [PATCH] fix(ros2): precise TF timeout and API cleanup on connector
 shutdown
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Salvage of RobotecAI/rai#775 by @maciejmajek — rebased to main.

- Duration uses seconds+nanoseconds for fractional timeouts
- ServiceAPI.shutdown destroys services/clients
- TopicAPI.shutdown clears publishers and subscriptions
- Connector shuts down APIs before destroying the node and only
  rclpy.shutdown() when it initialized the context
---
 .../rai/communication/ros2/api/service.py     |  9 ++++++++
 .../rai/communication/ros2/api/topic.py       | 13 ++++++++++-
 .../rai/communication/ros2/connectors/base.py | 22 ++++++++++++-------
 3 files changed, 35 insertions(+), 9 deletions(-)

diff --git a/src/rai_core/rai/communication/ros2/api/service.py b/src/rai_core/rai/communication/ros2/api/service.py
index 06dac1bc1..44b04cc0a 100644
--- a/src/rai_core/rai/communication/ros2/api/service.py
+++ b/src/rai_core/rai/communication/ros2/api/service.py
@@ -127,3 +127,12 @@ def create_service(
         handle = str(uuid.uuid4())
         self._services[handle] = service
         return handle
+
+    def shutdown(self) -> None:
+        for service in self._services.values():
+            service.destroy()
+        self._services.clear()
+        with self._persistent_clients_lock:
+            for client in self._persistent_clients.values():
+                client.destroy()
+            self._persistent_clients.clear()
diff --git a/src/rai_core/rai/communication/ros2/api/topic.py b/src/rai_core/rai/communication/ros2/api/topic.py
index cb55002f6..eeba9ee0b 100644
--- a/src/rai_core/rai/communication/ros2/api/topic.py
+++ b/src/rai_core/rai/communication/ros2/api/topic.py
@@ -244,6 +244,17 @@ def _verify_publisher_exists(self, topic: str) -> List[TopicEndpointInfo]:
         return topic_endpoints
 
     def shutdown(self) -> None:
-        """Cleanup publishers when object is destroyed."""
+        """Cleanup publishers and subscribers when object is destroyed."""
         for publisher in self._publishers.values():
             publisher.destroy()
+        self._publishers.clear()
+        for publisher in self.publishers.values():
+            publisher.destroy()
+        self.publishers.clear()
+
+        for subscription in self._subscriptions.values():
+            subscription.destroy()
+        self._subscriptions.clear()
+        for subscription in self.subscriptions.values():
+            subscription.destroy()
+        self.subscriptions.clear()
diff --git a/src/rai_core/rai/communication/ros2/connectors/base.py b/src/rai_core/rai/communication/ros2/connectors/base.py
index 4f0accec1..349391ce9 100644
--- a/src/rai_core/rai/communication/ros2/connectors/base.py
+++ b/src/rai_core/rai/communication/ros2/connectors/base.py
@@ -122,10 +122,12 @@ def __init__(
             If an invalid executor type is provided.
         """
         super().__init__()
+        self._owns_rclpy_context: bool = False
         if node_name is None:
             node_name = f"rai_ros2_connector_{str(uuid.uuid4())[-12:]}"
         if not rclpy.ok():
             rclpy.init()
+            self._owns_rclpy_context = True
             self.logger.warning(
                 "Auto-initializing ROS2, but manual initialization is recommended. "
                 "For better control and predictability, call rclpy.init() or ROS2Context before creating this connector."
@@ -464,11 +466,12 @@ def get_transform(
             raise LookupException(
                 f"Could not find transform from {source_frame} to {target_frame} in {timeout_sec} seconds"
             )
+        seconds, nanoseconds = divmod(timeout_sec * 1e9, 1e9)
         transform: TransformStamped = self._tf_buffer.lookup_transform(
             target_frame,
             source_frame,
             rclpy.time.Time(),
-            timeout=Duration(seconds=int(timeout_sec)),
+            timeout=Duration(seconds=int(seconds), nanoseconds=int(nanoseconds)),
         )
 
         return transform
@@ -558,15 +561,18 @@ def shutdown(self):
 
         This method:
         1. Unregisters the TF listener
-        2. Destroys the ROS2 node
-        3. Shuts down the action API
-        4. Shuts down the topic API
-        5. Shuts down the executor
-        6. Joins the executor thread
+        2. Shuts down topic / service / action APIs
+        3. Destroys the ROS2 node
+        4. Shuts down the executor
+        5. Joins the executor thread
+        6. Shuts down ROS2 context only if this connector initialized it
         """
         self._tf_listener.unregister()
-        self._node.destroy_node()
-        self._actions_api.shutdown()
         self._topic_api.shutdown()
+        self._service_api.shutdown()
+        self._actions_api.shutdown()
+        self._node.destroy_node()
         self._executor.shutdown()
         self._thread.join()
+        if self._owns_rclpy_context:
+            rclpy.shutdown()