diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index 0219805..b385778 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 1450feb..45529e3 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 5f1a411..75bfcf5 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