From 1f8e7398037320b3997735bde11bce42087c1919 Mon Sep 17 00:00:00 2001
From: Martin Dahl <martin.dahl@gmail.com>
Date: Wed, 30 Oct 2024 20:52:59 +0100
Subject: [PATCH] add destroy publisher functions

---
 r2r/examples/publishers.rs |  3 +++
 r2r/src/nodes.rs           | 24 ++++++++++++++++++++++++
 r2r/src/publishers.rs      |  4 ++--
 3 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/r2r/examples/publishers.rs b/r2r/examples/publishers.rs
index 3a9aeece7..535007d67 100644
--- a/r2r/examples/publishers.rs
+++ b/r2r/examples/publishers.rs
@@ -31,6 +31,9 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         count += 1;
     }
 
+    // destroy publisher before the node
+    node.destroy_publisher(publisher);
+
     println!("All done!");
 
     Ok(())
diff --git a/r2r/src/nodes.rs b/r2r/src/nodes.rs
index b9bdb395b..c797f1df6 100644
--- a/r2r/src/nodes.rs
+++ b/r2r/src/nodes.rs
@@ -905,6 +905,30 @@ impl Node {
         Ok(p)
     }
 
+    /// Destroy a ROS publisher.
+    pub fn destroy_publisher<T: WrappedTypesupport>(&mut self, p: Publisher<T>) {
+        if let Some(handle) = p.handle.upgrade() {
+            // Remove handle from list of publishers.
+            self.pubs.iter().position(|p| Arc::ptr_eq(p, &handle))
+                .map(|i| self.pubs.swap_remove(i));
+
+            let handle = wait_until_unwrapped(handle);
+            handle.destroy(self.node_handle.as_mut());
+        }
+    }
+
+    /// Destroy a ROS publisher.
+    pub fn destroy_publisher_untyped(&mut self, p: PublisherUntyped) {
+        if let Some(handle) = p.handle.upgrade() {
+            // Remove handle from list of publishers.
+            self.pubs.iter().position(|p| Arc::ptr_eq(p, &handle))
+                .map(|i| self.pubs.swap_remove(i));
+
+            let handle = wait_until_unwrapped(handle);
+            handle.destroy(self.node_handle.as_mut());
+        }
+    }
+
     /// Spin the ROS node.
     ///
     /// This handles wakeups of all subscribes, services, etc on the
diff --git a/r2r/src/publishers.rs b/r2r/src/publishers.rs
index afe71be6c..cb3bd3bd7 100644
--- a/r2r/src/publishers.rs
+++ b/r2r/src/publishers.rs
@@ -104,7 +104,7 @@ pub struct Publisher<T>
 where
     T: WrappedTypesupport,
 {
-    handle: Weak<Publisher_>,
+    pub(crate) handle: Weak<Publisher_>,
     type_: PhantomData<T>,
 }
 
@@ -116,7 +116,7 @@ unsafe impl Send for PublisherUntyped {}
 /// move between threads.
 #[derive(Debug, Clone)]
 pub struct PublisherUntyped {
-    handle: Weak<Publisher_>,
+    pub(crate) handle: Weak<Publisher_>,
     type_: String,
 }