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()