Add Publisher::get_subscription_count (#457)

Co-authored-by: Romain Reignier <[email protected]>

Author: romainreignier

rclrs/src/publisher.rs

@@ -146,6 +146,20 @@ where
         }
     }
 
+    /// Returns the number of subscriptions of the publisher.
+    pub fn get_subscription_count(&self) -> Result<usize, RclrsError> {
+        let mut subscription_count = 0;
+        // SAFETY: No preconditions for the function called.
+        unsafe {
+            rcl_publisher_get_subscription_count(
+                &*self.handle.rcl_publisher.lock().unwrap(),
+                &mut subscription_count,
+            )
+            .ok()?
+        };
+        Ok(subscription_count)
+    }
+
     /// Publishes a message.
     ///
     /// The [`MessageCow`] trait is implemented by any
@@ -327,6 +341,29 @@ mod tests {
             expected_publishers_info
         );
 
+        // Test get_subscription_count()
+        assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(0));
+        assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(0));
+        assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(0));
+        let _node_1_empty_subscriber = graph.node1.create_subscription(
+            "graph_test_topic_1",
+            QOS_PROFILE_SYSTEM_DEFAULT,
+            |_msg: msg::Empty| {},
+        );
+        let _node_1_basic_types_subscriber = graph.node1.create_subscription(
+            "graph_test_topic_2",
+            QOS_PROFILE_SYSTEM_DEFAULT,
+            |_msg: msg::BasicTypes| {},
+        );
+        let _node_2_default_subscriber = graph.node2.create_subscription(
+            "graph_test_topic_3",
+            QOS_PROFILE_SYSTEM_DEFAULT,
+            |_msg: msg::Defaults| {},
+        );
+        assert_eq!(node_1_empty_publisher.get_subscription_count(), Ok(1));
+        assert_eq!(node_1_basic_types_publisher.get_subscription_count(), Ok(1));
+        assert_eq!(node_2_default_publisher.get_subscription_count(), Ok(1));
+
         Ok(())
     }
 }