refactor: move topic logic to topic api

src/rai_core/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "rai_core"
-version = "2.11.1"
+version = "2.11.2"
 description = "Core functionality for RAI framework"
 readme = "README.md"
 requires-python = ">=3.10,<3.13"

src/rai_core/rai/communication/ros2/api/topic.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 import time
+from functools import partial
 from typing import (
     Any,
     Callable,
@@ -243,6 +244,73 @@ def _verify_publisher_exists(self, topic: str) -> List[TopicEndpointInfo]:
             raise ValueError(f"No publisher found for topic: {topic}")
         return topic_endpoints
 
+    def receive_message(
+        self,
+        topic: str,
+        timeout_sec: float = 1.0,
+        msg_type: Optional[str] = None,
+        qos_profile: Optional[QoSProfile] = None,
+        auto_qos_matching: bool = True,
+    ) -> Any:
+        """Receive a single message from a ROS2 topic.
+
+        Creates an internal subscriber for the topic if one does not yet exist.
+        Checks the message cache first; if a sufficiently recent message is found
+        it is returned immediately.  Otherwise polls until a message arrives or the
+        timeout expires.
+
+        If ``destroy_subscribers`` was set to ``True`` at construction time the
+        subscriber is destroyed immediately after the first message is received.
+        This frees resources but may trigger the rclpy executor crash described in
+        https://github.com/ros2/rclpy/issues/1142, so the default is ``False``.
+
+        Args:
+            topic: Name of the topic to receive from.
+            timeout_sec: Maximum time to wait for a message, in seconds.
+            msg_type: ROS2 message type string.  Auto-detected when not provided.
+            qos_profile: QoS profile to use when creating the subscriber.
+            auto_qos_matching: Automatically match QoS with existing publishers.
+
+        Returns:
+            The first raw ROS2 message received from the topic.
+
+        Raises:
+            TimeoutError: If no message is received within ``timeout_sec``.
+        """
+        if topic not in self._subscriptions:
+            sub_qos = self._resolve_qos_profile(
+                topic, auto_qos_matching, qos_profile, for_publisher=False
+            )
+            if msg_type is None:
+                msg_type = self.get_topic_type(topic)
+            msg_cls = self.import_message_from_str(msg_type)
+            subscription = self.node.create_subscription(
+                topic=topic,
+                msg_type=msg_cls,
+                callback=partial(self._generic_callback, topic),
+                qos_profile=sub_qos,
+            )
+            self._subscriptions[topic] = subscription
+        else:
+            # Subscriber already exists; serve from cache if fresh enough.
+            if topic in self._last_msg:
+                timestamp, msg = self._last_msg[topic]
+                if timestamp > time.time() - timeout_sec:
+                    return msg
+
+        start_time = time.time()
+        while time.time() - start_time < timeout_sec:
+            if topic in self._last_msg:
+                _, msg = self._last_msg[topic]
+                if self._destroy_subscribers and topic in self._subscriptions:
+                    self.node.destroy_subscription(self._subscriptions.pop(topic))
+                return msg
+            time.sleep(0.1)
+
+        raise TimeoutError(
+            f"Message from {topic} not received in {timeout_sec} seconds"
+        )
+
     def shutdown(self) -> None:
         """Cleanup publishers when object is destroyed."""
         for publisher in self._publishers.values():

src/rai_core/rai/communication/ros2/connectors/base.py

@@ -16,7 +16,7 @@
 import time
 import uuid
 from functools import partial
-from typing import Any, Callable, Dict, Final, List, Literal, Optional, Tuple, TypeVar
+from typing import Any, Callable, Final, List, Literal, Optional, Tuple, TypeVar
 
 import rclpy
 import rclpy.executors
@@ -156,15 +156,14 @@ def __init__(
         elif self._executor_type == "single_threaded":
             self._executor = SingleThreadedExecutor()
         else:
+            self._tf_listener.unregister()
+            self._node.destroy_node()
             raise ValueError(f"Invalid executor type: {self._executor_type}")
 
         self._executor.add_node(self._node)
         self._thread = threading.Thread(target=self._executor.spin)
         self._thread.start()
-        self.last_executor_performance_time = time.time()
-
-        # cache for last received messages
-        self.last_msg: Dict[str, T] = {}
+        self._last_executor_performance_time = time.time()
 
     def _executor_performance_callback(self) -> None:
         """Monitor executor performance and log warnings if it falls behind schedule.
@@ -176,7 +175,7 @@ def _executor_performance_callback(self) -> None:
         current_time = time.time()
         time_behind = (
             current_time
-            - self.last_executor_performance_time
+            - self._last_executor_performance_time
             - self._executor_performance_time_delta
         )
         threshold = (
@@ -193,21 +192,7 @@ def _executor_performance_callback(self) -> None:
                 f"{self._executor.__class__.__name__} is {time_behind:.2f} seconds behind. "
                 f"If you see this message frequently, consider switching to {', '.join(alternative_executors)}."
             )
-            self.last_executor_performance_time = current_time
-        else:
-            self.last_executor_performance_time = current_time
-
-    def _last_message_callback(self, source: str, msg: T):
-        """Store the last received message for a given source.
-
-        Parameters
-        ----------
-        source : str
-            The topic source identifier.
-        msg : T
-            The received message.
-        """
-        self.last_msg[source] = msg
+        self._last_executor_performance_time = current_time
 
     def get_topics_names_and_types(self) -> List[Tuple[str, List[str]]]:
         """Get list of available topics and their message types.
@@ -372,31 +357,14 @@ def receive_message(
         TimeoutError
             If no message is received within the timeout period.
         """
-        if self._topic_api.subscriber_exists(source):
-            # trying to hit cache first
-            if source in self.last_msg:
-                if self.last_msg[source].timestamp > time.time() - timeout_sec:
-                    return self.last_msg[source]
-        else:
-            self._topic_api.create_subscriber(
-                topic=source,
-                callback=partial(self.general_callback, source),
-                msg_type=msg_type,
-                qos_profile=qos_profile,
-                auto_qos_matching=auto_qos_matching,
-            )
-            self.register_callback(source, partial(self._last_message_callback, source))
-
-        start_time = time.time()
-        # wait for the message to be received
-        while time.time() - start_time < timeout_sec:
-            if source in self.last_msg:
-                return self.last_msg[source]
-            time.sleep(0.1)
-        else:
-            raise TimeoutError(
-                f"Message from {source} not received in {timeout_sec} seconds"
-            )
+        raw_msg = self._topic_api.receive_message(
+            topic=source,
+            timeout_sec=timeout_sec,
+            msg_type=msg_type,
+            qos_profile=qos_profile,
+            auto_qos_matching=auto_qos_matching,
+        )
+        return self.general_callback_preprocessor(raw_msg)
 
     @staticmethod
     def wait_for_transform(
@@ -563,6 +531,7 @@ def shutdown(self):
         4. Shuts down the topic API
         5. Shuts down the executor
         6. Joins the executor thread
+
         """
         self._tf_listener.unregister()
         self._node.destroy_node()