Add quality of service for publishers and subscribers.

Most of the documentation was taken from ros2_rust. https://github.com/ros2-rust/ros2_rust/blob/main/rclrs/src/qos.rs
ros2-dotnet · Apr 22, 2023 · 0a16152 · 0a16152
1 parent b3cbf6b
commit 0a16152
Show file tree

Hide file tree

Showing 9 changed files with 785 additions and 8 deletions.
diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt
@@ -44,6 +44,7 @@ set(CS_SOURCES
   MessageStaticMemberCache.cs
   Node.cs
   Publisher.cs
+  QosProfile.cs
   RCLdotnet.cs
   RCLExceptionHelper.cs
   RCLRet.cs
@@ -54,6 +55,7 @@ set(CS_SOURCES
   SafeGuardConditionHandle.cs
   SafeNodeHandle.cs
   SafePublisherHandle.cs
+  SafeQosProfileHandle.cs
   SafeRequestIdHandle.cs
   SafeServiceHandle.cs
   SafeSubscriptionHandle.cs

diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs
@@ -26,7 +26,7 @@ internal static class NodeDelegates
 
         [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
         internal delegate RCLRet NativeRCLCreatePublisherHandleType(
-            ref SafePublisherHandle publisherHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle);
+            ref SafePublisherHandle publisherHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle, SafeQosProfileHandle qosProfileHandle);
 
         internal static NativeRCLCreatePublisherHandleType native_rcl_create_publisher_handle = null;
 
@@ -38,7 +38,7 @@ internal delegate RCLRet NativeRCLDestroyPublisherHandleType(
 
         [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
         internal delegate RCLRet NativeRCLCreateSubscriptionHandleType(
-            ref SafeSubscriptionHandle subscriptionHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle);
+            ref SafeSubscriptionHandle subscriptionHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle, SafeQosProfileHandle qosProfileHandle);
 
         internal static NativeRCLCreateSubscriptionHandleType native_rcl_create_subscription_handle = null;
 
@@ -232,12 +232,28 @@ internal Node(SafeNodeHandle handle)
         // Disposed if the node is not live anymore.
         internal SafeNodeHandle Handle { get; }
 
-        public Publisher<T> CreatePublisher<T>(string topic) where T : IRosMessage
+        public Publisher<T> CreatePublisher<T>(string topic, QosProfile qosProfile = null) where T : IRosMessage
+        {
+            if (qosProfile != null)
+            {
+                using (SafeQosProfileHandle qosProfileHandle = QosProfile.CreateQosProfileHandle())
+                {
+                    QosProfile.WriteToQosProfileHandle(qosProfile, qosProfileHandle);
+                    return CreatePublisherInner<T>(topic, qosProfileHandle);
+                }
+            }
+            else
+            {
+                return CreatePublisherInner<T>(topic, SafeQosProfileHandle.Null);
+            }
+        }
+
+        private Publisher<T> CreatePublisherInner<T>(string topic, SafeQosProfileHandle qosProfileHandle) where T : IRosMessage
         {
             IntPtr typeSupport = MessageStaticMemberCache<T>.GetTypeSupport();
 
             var publisherHandle = new SafePublisherHandle();
-            RCLRet ret = NodeDelegates.native_rcl_create_publisher_handle(ref publisherHandle, Handle, topic, typeSupport);
+            RCLRet ret = NodeDelegates.native_rcl_create_publisher_handle(ref publisherHandle, Handle, topic, typeSupport, qosProfileHandle);
             publisherHandle.SetParent(Handle);
             if (ret != RCLRet.Ok)
             {
@@ -250,12 +266,28 @@ public Publisher<T> CreatePublisher<T>(string topic) where T : IRosMessage
             return publisher;
         }
 
-        public Subscription<T> CreateSubscription<T>(string topic, Action<T> callback) where T : IRosMessage, new()
+        public Subscription<T> CreateSubscription<T>(string topic, Action<T> callback, QosProfile qosProfile = null) where T : IRosMessage, new()
+        {
+            if (qosProfile != null)
+            {
+                using (SafeQosProfileHandle qosProfileHandle = QosProfile.CreateQosProfileHandle())
+                {
+                    QosProfile.WriteToQosProfileHandle(qosProfile, qosProfileHandle);
+                    return CreateSubscriptionInner(topic, callback, qosProfileHandle);
+                }
+            }
+            else
+            {
+                return CreateSubscriptionInner(topic, callback, SafeQosProfileHandle.Null);
+            }
+        }
+
+        private Subscription<T> CreateSubscriptionInner<T>(string topic, Action<T> callback, SafeQosProfileHandle qosProfileHandle) where T : IRosMessage, new()
         {
             IntPtr typeSupport = MessageStaticMemberCache<T>.GetTypeSupport();
 
             var subscriptionHandle = new SafeSubscriptionHandle();
-            RCLRet ret = NodeDelegates.native_rcl_create_subscription_handle(ref subscriptionHandle, Handle, topic, typeSupport);
+            RCLRet ret = NodeDelegates.native_rcl_create_subscription_handle(ref subscriptionHandle, Handle, topic, typeSupport, qosProfileHandle);
             subscriptionHandle.SetParent(Handle);
             if (ret != RCLRet.Ok)
             {