Merge pull request #131 from hoffmann-stefan/upstream_pr2_qos

Use RMW Quality of Service profile definitions

rcldotnet/CMakeLists.txt

@@ -46,6 +46,7 @@ set(CS_SOURCES
   Node.cs
   Publisher.cs
   QosProfile.cs
+  QoSProfileDelegates.cs
   RCLdotnet.cs
   RCLExceptionHelper.cs
   RCLRet.cs
@@ -96,6 +97,7 @@ add_library(${PROJECT_NAME}_native SHARED
   rcldotnet_node.c
   rcldotnet_publisher.c
   rcldotnet_timer.c
+  rcldotnet_qos_profile.c
   rcldotnet.c
 )
 

rcldotnet/QoSProfileDelegates.cs

@@ -0,0 +1,116 @@
+/* Copyright 2023 Queensland University of Technology.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using ROS2.Utils;
+
+namespace ROS2
+{
+    internal struct RmwTime
+    {
+        public ulong sec;
+        public ulong nsec;
+
+        public RmwTime(QosProfileDelegates.NativeRCLQosProfileReadRMWTimeType nativeDelegate, IntPtr profile)
+        {
+            sec = 0UL;
+            nsec = 0UL;
+
+            nativeDelegate(profile, ref sec, ref nsec);
+        }
+
+        public TimeSpan AsTimespan()
+        {
+            if (sec == 9223372036UL && nsec == 854775807UL)
+            {
+                // see RMW_DURATION_INFINITE and comment on QosProfile.InfiniteDuration above.
+                return QosProfile.InfiniteDuration;
+            }
+
+            const ulong NanosecondsPerTick = 1000000 / TimeSpan.TicksPerMillisecond; // ~= 100.
+            ulong ticks = sec * TimeSpan.TicksPerSecond;
+            ticks += nsec / NanosecondsPerTick;
+            return new TimeSpan((long)ticks);
+        }
+    }
+
+    internal static class QosProfileDelegates
+    {
+        private static readonly DllLoadUtils _dllLoadUtils;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate IntPtr NativeRCLGetConstQosProfileHandleType();
+        internal static NativeRCLGetConstQosProfileHandleType native_rcl_qos_get_const_profile_default = null;
+        internal static NativeRCLGetConstQosProfileHandleType native_rcl_qos_get_const_profile_parameter_events = null;
+        internal static NativeRCLGetConstQosProfileHandleType native_rcl_qos_get_const_profile_parameters = null;
+        internal static NativeRCLGetConstQosProfileHandleType native_rcl_qos_get_const_profile_sensor_data = null;
+        internal static NativeRCLGetConstQosProfileHandleType native_rcl_qos_get_const_profile_services_default = null;
+        internal static NativeRCLGetConstQosProfileHandleType native_rcl_qos_get_const_profile_system_default = null;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate QosHistoryPolicy NativeRCLQosProfileReadHistoryType(IntPtr qosProfileHandle);
+        internal static NativeRCLQosProfileReadHistoryType native_rcl_qos_profile_read_history;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate int NativeRCLQosProfileReadDepthType(IntPtr qosProfileHandle);
+        internal static NativeRCLQosProfileReadDepthType native_rcl_qos_profile_read_depth;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate QosReliabilityPolicy NativeRCLQosProfileReadReliabilityType(IntPtr qosProfileHandle);
+        internal static NativeRCLQosProfileReadReliabilityType native_rcl_qos_profile_read_reliability;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate QosDurabilityPolicy NativeRCLQosProfileReadDurabilityType(IntPtr qosProfileHandle);
+        internal static NativeRCLQosProfileReadDurabilityType native_rcl_qos_profile_read_durability;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate void NativeRCLQosProfileReadRMWTimeType(IntPtr qosProfileHandle, ref ulong sec, ref ulong nsec);
+        internal static NativeRCLQosProfileReadRMWTimeType native_rcl_qos_profile_read_deadline;
+        internal static NativeRCLQosProfileReadRMWTimeType native_rcl_qos_profile_read_lifespan;
+        internal static NativeRCLQosProfileReadRMWTimeType native_rcl_qos_profile_read_liveliness_lease_duration;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate QosLivelinessPolicy NativeRCLQosProfileReadLivelinessType(IntPtr qosProfileHandle);
+        internal static NativeRCLQosProfileReadLivelinessType native_rcl_qos_profile_read_liveliness;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate int NativeRCLQosProfileReadAvoidRosNamespaceConventionsType(IntPtr qosProfileHandle);
+        internal static NativeRCLQosProfileReadAvoidRosNamespaceConventionsType native_rcl_qos_profile_read_avoid_ros_namespace_conventions;
+
+        static QosProfileDelegates()
+        {
+            _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils();
+            IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet");
+
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_const_profile_default), out native_rcl_qos_get_const_profile_default);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_const_profile_parameter_events), out native_rcl_qos_get_const_profile_parameter_events);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_const_profile_parameters), out native_rcl_qos_get_const_profile_parameters);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_const_profile_sensor_data), out native_rcl_qos_get_const_profile_sensor_data);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_const_profile_services_default), out native_rcl_qos_get_const_profile_services_default);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_const_profile_system_default), out native_rcl_qos_get_const_profile_system_default);
+
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_history), out native_rcl_qos_profile_read_history);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_depth), out native_rcl_qos_profile_read_depth);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_reliability), out native_rcl_qos_profile_read_reliability);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_durability), out native_rcl_qos_profile_read_durability);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_deadline), out native_rcl_qos_profile_read_deadline);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_lifespan), out native_rcl_qos_profile_read_lifespan);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_liveliness), out native_rcl_qos_profile_read_liveliness);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_liveliness_lease_duration), out native_rcl_qos_profile_read_liveliness_lease_duration);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_profile_read_avoid_ros_namespace_conventions), out native_rcl_qos_profile_read_avoid_ros_namespace_conventions);
+        }
+    }
+}

rcldotnet/QosProfile.cs

@@ -207,8 +207,134 @@ private QosProfile(
 
         /// <summary>
         /// The default QoS profile.
+        ///
+        /// Unlikely to change but valid as at 2023-10-31.
+        /// https://github.com/ros2/rmw/blob/rolling/rmw/include/rmw/qos_profiles.h
+        ///
+        /// | --------------- | --------------- |
+        /// | History         | KEEP_LAST       |
+        /// | Depth           | 10              |
+        /// | Reliability     | RELIABLE        |
+        /// | Durability      | VOLATILE        |
+        /// | Deadline        | DEFAULT         |
+        /// | Lifespan        | DEFAULT         |
+        /// | Liveliness      | SYSTEM_DEFAULT  |
+        /// | Lease Duration  | DEFAULT         |
+        /// | Avoid Namespace | false           |
+        /// </summary>
+        public static QosProfile DefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_default);
+
+        /// <summary>
+        /// Profile for clock messages.
+        /// See CreateClockProfile for more details.
+        ///
+        /// | --------------- | --------------- |
+        /// | History         | KEEP_LAST       |
+        /// | Depth           | 1               |
+        /// | Reliability     | BEST_EFFORT     |
+        /// | Durability      | VOLATILE        |
+        /// | Deadline        | DEFAULT         |
+        /// | Lifespan        | DEFAULT         |
+        /// | Liveliness      | SYSTEM_DEFAULT  |
+        /// | Lease Duration  | DEFAULT         |
+        /// | Avoid Namespace | false           |
+        /// </summary>
+        public static QosProfile ClockProfile => CreateClockProfile();
+
+        /// <summary>
+        /// Profile for parameter event messages.
+        ///
+        /// Unlikely to change but valid as at 2023-10-31.
+        /// https://github.com/ros2/rmw/blob/rolling/rmw/include/rmw/qos_profiles.h
+        ///
+        /// | --------------- | --------------- |
+        /// | History         | KEEP_LAST       |
+        /// | Depth           | 1000            |
+        /// | Reliability     | RELIABLE        |
+        /// | Durability      | VOLATILE        |
+        /// | Deadline        | DEFAULT         |
+        /// | Lifespan        | DEFAULT         |
+        /// | Liveliness      | SYSTEM_DEFAULT  |
+        /// | Lease Duration  | DEFAULT         |
+        /// | Avoid Namespace | false           |
+        /// </summary>
+        public static QosProfile ParameterEventsProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_parameter_events);
+
+        /// <summary>
+        /// Profile for parameter messages.
+        ///
+        /// Unlikely to change but valid as at 2023-10-31.
+        /// https://github.com/ros2/rmw/blob/rolling/rmw/include/rmw/qos_profiles.h
+        ///
+        /// | --------------- | --------------- |
+        /// | History         | KEEP_LAST       |
+        /// | Depth           | 1000            |
+        /// | Reliability     | RELIABLE        |
+        /// | Durability      | VOLATILE        |
+        /// | Deadline        | DEFAULT         |
+        /// | Lifespan        | DEFAULT         |
+        /// | Liveliness      | SYSTEM_DEFAULT  |
+        /// | Lease Duration  | DEFAULT         |
+        /// | Avoid Namespace | false           |
+        /// </summary>
+        public static QosProfile ParametersProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_parameters);
+
+        /// <summary>
+        /// Profile for sensor messages.
+        ///
+        /// Unlikely to change but valid as at 2023-10-31.
+        /// https://github.com/ros2/rmw/blob/rolling/rmw/include/rmw/qos_profiles.h
+        ///
+        /// | --------------- | --------------- |
+        /// | History         | KEEP_LAST       |
+        /// | Depth           | 5               |
+        /// | Reliability     | BEST_EFFORT     |
+        /// | Durability      | VOLATILE        |
+        /// | Deadline        | DEFAULT         |
+        /// | Lifespan        | DEFAULT         |
+        /// | Liveliness      | SYSTEM_DEFAULT  |
+        /// | Lease Duration  | DEFAULT         |
+        /// | Avoid Namespace | false           |
         /// </summary>
-        public static QosProfile DefaultProfile { get; } = CreateDefaultProfile();
+        public static QosProfile SensorDataProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_sensor_data);
+
+        /// <summary>
+        /// Default profile for services.
+        ///
+        /// Unlikely to change but valid as at 2023-10-31.
+        /// https://github.com/ros2/rmw/blob/rolling/rmw/include/rmw/qos_profiles.h
+        ///
+        /// | --------------- | --------------- |
+        /// | History         | KEEP_LAST       |
+        /// | Depth           | 10              |
+        /// | Reliability     | RELIABLE        |
+        /// | Durability      | VOLATILE        |
+        /// | Deadline        | DEFAULT         |
+        /// | Lifespan        | DEFAULT         |
+        /// | Liveliness      | SYSTEM_DEFAULT  |
+        /// | Lease Duration  | DEFAULT         |
+        /// | Avoid Namespace | false           |
+        /// </summary>
+        public static QosProfile ServicesDefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_services_default);
+
+        /// <summary>
+        /// The system default (null) profile.
+        ///
+        /// Unlikely to change but valid as at 2023-10-31.
+        /// https://github.com/ros2/rmw/blob/rolling/rmw/include/rmw/qos_profiles.h
+        ///
+        /// | --------------- | --------------- |
+        /// | History         | SYSTEM_DEFAULT  |
+        /// | Depth           | SYSTEM_DEFAULT  |
+        /// | Reliability     | SYSTEM_DEFAULT  |
+        /// | Durability      | SYSTEM_DEFAULT  |
+        /// | Deadline        | DEFAULT         |
+        /// | Lifespan        | DEFAULT         |
+        /// | Liveliness      | SYSTEM_DEFAULT  |
+        /// | Lease Duration  | DEFAULT         |
+        /// | Avoid Namespace | false           |
+        /// </summary>
+        public static QosProfile SystemDefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_system_default);
 
         /// <summary>
         /// The history policy.
@@ -486,23 +612,22 @@ public QosProfile WithAvoidRosNamespaceConventions(bool avoidRosNamespaceConvent
             return result;
         }
 
-        private static QosProfile CreateDefaultProfile()
+        private static QosProfile CreateClockProfile()
         {
-            // taken from rmw_qos_profile_default
-            // TODO: (sh) read values from rmw layer instead of hardcoding them here.
-
-            var result = new QosProfile(
+            // Values from https://docs.ros.org/en/rolling/p/rclcpp/generated/classrclcpp_1_1ClockQoS.html
+            // Only available in versions of rclcpp >= Galactic
+            // If changed, update comment at top of file also.
+            return new QosProfile(
                 history: QosHistoryPolicy.KeepLast,
-                depth: 10,
-                reliability: QosReliabilityPolicy.Reliable,
+                depth: 1,
+                reliability: QosReliabilityPolicy.BestEffort,
                 durability: QosDurabilityPolicy.Volatile,
                 deadline: TimeSpan.Zero,
                 lifespan: TimeSpan.Zero,
                 liveliness: QosLivelinessPolicy.SystemDefault,
                 livelinessLeaseDuration: TimeSpan.Zero,
-                avoidRosNamespaceConventions: false);
-
-            return result;
+                avoidRosNamespaceConventions: false
+            );
         }
 
         internal static SafeQosProfileHandle CreateQosProfileHandle()
@@ -562,5 +687,22 @@ private static void ToRmwTime(TimeSpan timeSpan, out ulong sec, out ulong nsec)
             sec = (ulong)seconds;
             nsec = (ulong)(ticksInsideSecond * NanosecondsPerTick);
         }
+
+        // This method is intended only for reading from a const rmw_qos_profile_t * - it will perform no memory management on the pointer!
+        private static QosProfile ReadConstQoSProfile(QosProfileDelegates.NativeRCLGetConstQosProfileHandleType nativeDelegate)
+        {
+            IntPtr nativeProfileConst = nativeDelegate();
+
+            return new QosProfile(
+                QosProfileDelegates.native_rcl_qos_profile_read_history(nativeProfileConst),
+                QosProfileDelegates.native_rcl_qos_profile_read_depth(nativeProfileConst),
+                QosProfileDelegates.native_rcl_qos_profile_read_reliability(nativeProfileConst),
+                QosProfileDelegates.native_rcl_qos_profile_read_durability(nativeProfileConst),
+                new RmwTime(QosProfileDelegates.native_rcl_qos_profile_read_deadline, nativeProfileConst).AsTimespan(),
+                new RmwTime(QosProfileDelegates.native_rcl_qos_profile_read_lifespan, nativeProfileConst).AsTimespan(),
+                QosProfileDelegates.native_rcl_qos_profile_read_liveliness(nativeProfileConst),
+                new RmwTime(QosProfileDelegates.native_rcl_qos_profile_read_liveliness_lease_duration, nativeProfileConst).AsTimespan(),
+                QosProfileDelegates.native_rcl_qos_profile_read_avoid_ros_namespace_conventions(nativeProfileConst) != 0);
+        }
     }
 }