From eb9d68f4c5173be44a35a2e40492408f8e1432d5 Mon Sep 17 00:00:00 2001
From: DieuwertDemcon <dieuwert.wolters@demcon.com>
Date: Mon, 7 Apr 2025 09:27:23 +0200
Subject: [PATCH] Add subscription count to publisher

---
 rcldotnet/Publisher.cs          | 17 +++++++++++++++++
 rcldotnet/rcldotnet_publisher.c | 13 +++++++++++++
 rcldotnet/rcldotnet_publisher.h |  3 +++
 3 files changed, 33 insertions(+)

diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs
index 02198057..b385778d 100644
--- a/rcldotnet/Publisher.cs
+++ b/rcldotnet/Publisher.cs
@@ -29,6 +29,11 @@ internal delegate RCLRet NativeRCLPublishType(
 
         internal static NativeRCLPublishType native_rcl_publish = null;
 
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate int NativeRCLGetSubscriptionCountType(SafePublisherHandle publisherHandle, out int count);
+
+        internal static NativeRCLGetSubscriptionCountType native_rcl_get_subscription_count = null;
+
         static PublisherDelegates()
         {
             _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils();
@@ -37,6 +42,10 @@ static PublisherDelegates()
             IntPtr native_rcl_publish_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_publish");
             PublisherDelegates.native_rcl_publish = (NativeRCLPublishType)Marshal.GetDelegateForFunctionPointer(
                 native_rcl_publish_ptr, typeof(NativeRCLPublishType));
+
+            IntPtr native_rcl_get_subscription_count_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_get_subscription_count");
+            PublisherDelegates.native_rcl_get_subscription_count = (NativeRCLGetSubscriptionCountType)Marshal.GetDelegateForFunctionPointer(
+                native_rcl_get_subscription_count_ptr, typeof(NativeRCLGetSubscriptionCountType));
         }
     }
 
@@ -79,5 +88,13 @@ public void Publish(T message)
                 RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(PublisherDelegates.native_rcl_publish)}() failed.");
             }
         }
+        
+        public int GetSubscriptionCount()
+        {
+            int count;
+            int ret = PublisherDelegates.native_rcl_get_subscription_count(Handle, out count);
+            RCLExceptionHelper.CheckReturnValue((RCLRet)ret, $"{nameof(PublisherDelegates.native_rcl_get_subscription_count)}() failed.");
+            return count;
+        }
     }
 }
diff --git a/rcldotnet/rcldotnet_publisher.c b/rcldotnet/rcldotnet_publisher.c
index 1450feb8..45529e3d 100644
--- a/rcldotnet/rcldotnet_publisher.c
+++ b/rcldotnet/rcldotnet_publisher.c
@@ -33,3 +33,16 @@ int32_t native_rcl_publish(void * publisher_handle, void * raw_ros_message)
   
   return ret;
 }
+
+int32_t native_rcl_get_subscription_count(void * publisher_handle, int32_t * count_out)
+{
+  rcl_publisher_t * publisher = (rcl_publisher_t *)publisher_handle;
+
+  int32_t count = 0;
+  rcl_ret_t ret = rcl_publisher_get_subscription_count(publisher, &count);
+  if (ret != RCL_RET_OK) {
+    return ret;
+  }
+  *count_out = count;
+  return ret;
+}
diff --git a/rcldotnet/rcldotnet_publisher.h b/rcldotnet/rcldotnet_publisher.h
index 5f1a4112..75bfcf50 100644
--- a/rcldotnet/rcldotnet_publisher.h
+++ b/rcldotnet/rcldotnet_publisher.h
@@ -20,4 +20,7 @@
 RCLDOTNET_EXPORT
 int32_t RCLDOTNET_CDECL native_rcl_publish(void *, void *);
 
+RCLDOTNET_EXPORT
+int32_t RCLDOTNET_CDECL native_rcl_get_subscription_count(void* publisher_handle, int32_t* count_out);
+
 #endif // RCLDOTNET_PUBLISHER_H