From 910cd19e57cac302ca7c84fe5cf35a7300eda8d9 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 9 Oct 2023 11:59:27 +1000 Subject: [PATCH 1/8] Adding the QoS profile for a clock topic. --- rcldotnet/QosProfile.cs | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/rcldotnet/QosProfile.cs b/rcldotnet/QosProfile.cs index d05d9f8d..9e61ded6 100644 --- a/rcldotnet/QosProfile.cs +++ b/rcldotnet/QosProfile.cs @@ -210,6 +210,11 @@ private QosProfile( /// public static QosProfile DefaultProfile { get; } = CreateDefaultProfile(); + /// + /// Profile for a clock topic. + /// + public static QosProfile ClockProfile { get; } = CreateClockProfile(); + /// /// The history policy. /// @@ -505,6 +510,23 @@ private static QosProfile CreateDefaultProfile() return result; } + private static QosProfile CreateClockProfile() + { + // Values from https://docs.ros.org/en/rolling/p/rclcpp/generated/classrclcpp_1_1ClockQoS.html + // Only available in versions of rclcpp >= Galactic + return new QosProfile( + history: QosHistoryPolicy.KeepLast, + depth: 1, + reliability: QosReliabilityPolicy.BestEffort, + durability: QosDurabilityPolicy.Volatile, + deadline: TimeSpan.Zero, + lifespan: TimeSpan.Zero, + liveliness: QosLivelinessPolicy.SystemDefault, + livelinessLeaseDuration: TimeSpan.Zero, + avoidRosNamespaceConventions: false + ); + } + internal static SafeQosProfileHandle CreateQosProfileHandle() { var qosProfileHandle = new SafeQosProfileHandle(); From 14822faddeb4208dfcc847d8f5ee556d1592e3d3 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 9 Oct 2023 16:29:39 +1000 Subject: [PATCH 2/8] Using native RMW QoS profiles --- rcldotnet/QosProfile.cs | 96 ++++++++++++++++++++++++++++++++--------- rcldotnet/RCLdotnet.cs | 16 +++++++ rcldotnet/rcldotnet.c | 24 +++++++++++ rcldotnet/rcldotnet.h | 18 ++++++++ 4 files changed, 133 insertions(+), 21 deletions(-) diff --git a/rcldotnet/QosProfile.cs b/rcldotnet/QosProfile.cs index 9e61ded6..72c857f4 100644 --- a/rcldotnet/QosProfile.cs +++ b/rcldotnet/QosProfile.cs @@ -14,10 +14,33 @@ */ using System; +using System.Runtime.InteropServices; using System.Threading; namespace ROS2 { + [StructLayout(LayoutKind.Sequential)] + internal struct RmwTime + { + public ulong Sec; + public ulong NSec; + } + + [StructLayout(LayoutKind.Sequential)] + internal struct RmwQosProfile + { + public QosHistoryPolicy History; + public UIntPtr Depth; // size_t is properly represented by UIntPtr + public QosReliabilityPolicy Reliability; + public QosDurabilityPolicy Durability; + public RmwTime Deadline; + public RmwTime Lifespan; + public QosLivelinessPolicy Liveliness; + public RmwTime LivelinessLeaseDuration; + [MarshalAs(UnmanagedType.U1)] + public bool AvoidRosNamespaceConventions; + } + /// /// The `HISTORY` DDS QoS policy. /// @@ -208,13 +231,38 @@ private QosProfile( /// /// The default QoS profile. /// - public static QosProfile DefaultProfile { get; } = CreateDefaultProfile(); + public static QosProfile DefaultProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_default()); /// - /// Profile for a clock topic. + /// Profile for clock messages. /// public static QosProfile ClockProfile { get; } = CreateClockProfile(); + /// + /// Profile for parameter event messages. + /// + public static QosProfile ParameterEventsProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameter_events()); + + /// + /// Profile for parameter messages. + /// + public static QosProfile ParametersProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameters()); + + /// + /// Profile for sensor messages. + /// + public static QosProfile SensorDataProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_sensor_data()); + + /// + /// Profile for services. + /// + public static QosProfile ServicesProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_services_default()); + + /// + /// The system default (null) profile. + /// + public static QosProfile SystemDefaultProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_system_default()); + /// /// The history policy. /// @@ -491,25 +539,6 @@ public QosProfile WithAvoidRosNamespaceConventions(bool avoidRosNamespaceConvent return result; } - private static QosProfile CreateDefaultProfile() - { - // taken from rmw_qos_profile_default - // TODO: (sh) read values from rmw layer instead of hardcoding them here. - - var result = new QosProfile( - history: QosHistoryPolicy.KeepLast, - depth: 10, - reliability: QosReliabilityPolicy.Reliable, - durability: QosDurabilityPolicy.Volatile, - deadline: TimeSpan.Zero, - lifespan: TimeSpan.Zero, - liveliness: QosLivelinessPolicy.SystemDefault, - livelinessLeaseDuration: TimeSpan.Zero, - avoidRosNamespaceConventions: false); - - return result; - } - private static QosProfile CreateClockProfile() { // Values from https://docs.ros.org/en/rolling/p/rclcpp/generated/classrclcpp_1_1ClockQoS.html @@ -584,5 +613,30 @@ private static void ToRmwTime(TimeSpan timeSpan, out ulong sec, out ulong nsec) sec = (ulong)seconds; nsec = (ulong)(ticksInsideSecond * NanosecondsPerTick); } + + private static TimeSpan FromRmwTime(RmwTime time) + { + if (time.Sec == 9223372036 && time.NSec == 854775807) + { + // see RMW_DURATION_INFINITE and comment on QosProfile.InfiniteDuration above. + return InfiniteDuration; + } + + const long NanosecondsPerTick = 1000000 / TimeSpan.TicksPerMillisecond; // ~= 100. + ulong ticks = time.Sec * TimeSpan.TicksPerSecond; + ticks += time.NSec / NanosecondsPerTick; + return new TimeSpan((long)ticks); + } + + private static QosProfile ToQosProfile(RmwQosProfile rmwProfile) + { + return new QosProfile( + rmwProfile.History, (int)rmwProfile.Depth, rmwProfile.Reliability, rmwProfile.Durability, + FromRmwTime(rmwProfile.Deadline), + FromRmwTime(rmwProfile.Lifespan), + rmwProfile.Liveliness, + FromRmwTime(rmwProfile.LivelinessLeaseDuration), + rmwProfile.AvoidRosNamespaceConventions); + } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index eeefea8a..729df169 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -353,6 +353,15 @@ internal delegate RCLRet NativeRCLWriteToQosProfileHandleType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr NativeRCLGetStringType(SafeHandle handle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RmwQosProfile NativeRCLGetQosProfileType(); + internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_default = null; + internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_parameter_events = null; + internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_parameters = null; + internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_sensor_data = null; + internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_services_default = null; + internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_system_default = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreateClockHandleType(ref SafeClockHandle clockHandle, int clockType); @@ -729,6 +738,13 @@ static RCLdotnetDelegates() (NativeRCLWriteToQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_write_to_qos_profile_handle_ptr, typeof(NativeRCLWriteToQosProfileHandleType)); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_default), out native_rcl_qos_get_profile_default); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_parameter_events), out native_rcl_qos_get_profile_parameter_events); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_parameters), out native_rcl_qos_get_profile_parameters); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_sensor_data), out native_rcl_qos_get_profile_sensor_data); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_services_default), out native_rcl_qos_get_profile_services_default); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_system_default), out native_rcl_qos_get_profile_system_default); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_clock_handle), out native_rcl_create_clock_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_clock_handle), out native_rcl_destroy_clock_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_timer_handle), out native_rcl_create_timer_handle); diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 40be6421..f1063428 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -762,6 +762,30 @@ int32_t native_rcl_write_to_qos_profile_handle( return RCL_RET_OK; } +const rmw_qos_profile_t native_rcl_qos_get_profile_default() { + return rmw_qos_profile_default; +} + +const rmw_qos_profile_t native_rcl_qos_get_profile_parameter_events() { + return rmw_qos_profile_parameter_events; +} + +const rmw_qos_profile_t native_rcl_qos_get_profile_parameters() { + return rmw_qos_profile_parameters; +} + +const rmw_qos_profile_t native_rcl_qos_get_profile_sensor_data() { + return rmw_qos_profile_sensor_data; +} + +const rmw_qos_profile_t native_rcl_qos_get_profile_services_default() { + return rmw_qos_profile_services_default; +} + +const rmw_qos_profile_t native_rcl_qos_get_profile_system_default() { + return rmw_qos_profile_system_default; +} + int32_t native_rcl_create_timer_handle(void **timer_handle, void *clock_handle, int64_t period, rcl_timer_callback_t callback) { rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_timer_t *timer = malloc(sizeof(rcl_timer_t)); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index c1021a76..a849ea6a 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -248,6 +248,24 @@ int32_t RCLDOTNET_CDECL native_rcl_write_to_qos_profile_handle( uint64_t liveliness_lease_duration_nsec, int32_t /* bool */ avoid_ros_namespace_conventions); +RCLDOTNET_EXPORT +const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_default(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_parameter_events(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_parameters(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_sensor_data(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_services_default(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_system_default(); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_timer_handle(void **timer_handle, void *clock_handle, int64_t period, rcl_timer_callback_t callback); From 1fc4e9d37c1b4776e316381b95ba177e7c565249 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 12 Oct 2023 16:36:30 +1000 Subject: [PATCH 3/8] Using safer typing and improving style --- rcldotnet/QosProfile.cs | 30 ++++++++++++++++-------------- rcldotnet/RCLdotnet.cs | 2 +- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/rcldotnet/QosProfile.cs b/rcldotnet/QosProfile.cs index 72c857f4..b8ad4572 100644 --- a/rcldotnet/QosProfile.cs +++ b/rcldotnet/QosProfile.cs @@ -37,6 +37,7 @@ internal struct RmwQosProfile public RmwTime Lifespan; public QosLivelinessPolicy Liveliness; public RmwTime LivelinessLeaseDuration; + // TODO: Windows uses a 32-bit bool representation. Confirm this works in Windows... [MarshalAs(UnmanagedType.U1)] public bool AvoidRosNamespaceConventions; } @@ -231,37 +232,37 @@ private QosProfile( /// /// The default QoS profile. /// - public static QosProfile DefaultProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_default()); + public static QosProfile DefaultProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_default()); /// /// Profile for clock messages. /// - public static QosProfile ClockProfile { get; } = CreateClockProfile(); + public static QosProfile ClockProfile => CreateClockProfile(); /// /// Profile for parameter event messages. /// - public static QosProfile ParameterEventsProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameter_events()); + public static QosProfile ParameterEventsProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameter_events()); /// /// Profile for parameter messages. /// - public static QosProfile ParametersProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameters()); + public static QosProfile ParametersProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameters()); /// /// Profile for sensor messages. /// - public static QosProfile SensorDataProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_sensor_data()); + public static QosProfile SensorDataProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_sensor_data()); /// /// Profile for services. /// - public static QosProfile ServicesProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_services_default()); + public static QosProfile ServicesProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_services_default()); /// /// The system default (null) profile. /// - public static QosProfile SystemDefaultProfile { get; } = ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_system_default()); + public static QosProfile SystemDefaultProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_system_default()); /// /// The history policy. @@ -628,15 +629,16 @@ private static TimeSpan FromRmwTime(RmwTime time) return new TimeSpan((long)ticks); } - private static QosProfile ToQosProfile(RmwQosProfile rmwProfile) + private static QosProfile ToQosProfile(IntPtr rmwProfile) { + RmwQosProfile native = Marshal.PtrToStructure(rmwProfile); return new QosProfile( - rmwProfile.History, (int)rmwProfile.Depth, rmwProfile.Reliability, rmwProfile.Durability, - FromRmwTime(rmwProfile.Deadline), - FromRmwTime(rmwProfile.Lifespan), - rmwProfile.Liveliness, - FromRmwTime(rmwProfile.LivelinessLeaseDuration), - rmwProfile.AvoidRosNamespaceConventions); + native.History, (int)native.Depth, native.Reliability, native.Durability, + FromRmwTime(native.Deadline), + FromRmwTime(native.Lifespan), + native.Liveliness, + FromRmwTime(native.LivelinessLeaseDuration), + native.AvoidRosNamespaceConventions); } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 729df169..6a788dc7 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -354,7 +354,7 @@ internal delegate RCLRet NativeRCLWriteToQosProfileHandleType( internal delegate IntPtr NativeRCLGetStringType(SafeHandle handle); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate RmwQosProfile NativeRCLGetQosProfileType(); + internal delegate IntPtr NativeRCLGetQosProfileType(); internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_default = null; internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_parameter_events = null; internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_parameters = null; From a5105db638d7a128939ff7efa8ee57142da46bc3 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Fri, 13 Oct 2023 14:18:22 +1000 Subject: [PATCH 4/8] Improving management of default QoS profiles. --- rcldotnet/CMakeLists.txt | 4 +- rcldotnet/QoS/QoSProfileDelegates.cs | 116 +++++++++++++++++++++++++++ rcldotnet/{ => QoS}/QosProfile.cs | 72 +++++------------ rcldotnet/RCLdotnet.cs | 7 -- rcldotnet/rcldotnet.c | 24 ------ rcldotnet/rcldotnet.h | 18 ----- rcldotnet/rcldotnet_qos_profile.c | 94 ++++++++++++++++++++++ rcldotnet/rcldotnet_qos_profile.h | 67 ++++++++++++++++ 8 files changed, 300 insertions(+), 102 deletions(-) create mode 100644 rcldotnet/QoS/QoSProfileDelegates.cs rename rcldotnet/{ => QoS}/QosProfile.cs (89%) create mode 100644 rcldotnet/rcldotnet_qos_profile.c create mode 100644 rcldotnet/rcldotnet_qos_profile.h diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 766db758..f80380fc 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -45,7 +45,6 @@ set(CS_SOURCES MessageStaticMemberCache.cs Node.cs Publisher.cs - QosProfile.cs RCLdotnet.cs RCLExceptionHelper.cs RCLRet.cs @@ -67,6 +66,8 @@ set(CS_SOURCES ServiceDefinitionStaticMemberCache.cs Subscription.cs Timer.cs + QoS/QosProfile.cs + QoS/QoSProfileDelegates.cs ) find_package(rcldotnet_common REQUIRED) @@ -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 ) diff --git a/rcldotnet/QoS/QoSProfileDelegates.cs b/rcldotnet/QoS/QoSProfileDelegates.cs new file mode 100644 index 00000000..0530677b --- /dev/null +++ b/rcldotnet/QoS/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.Qos +{ + 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); + } + } +} diff --git a/rcldotnet/QosProfile.cs b/rcldotnet/QoS/QosProfile.cs similarity index 89% rename from rcldotnet/QosProfile.cs rename to rcldotnet/QoS/QosProfile.cs index b8ad4572..0d1fd2ed 100644 --- a/rcldotnet/QosProfile.cs +++ b/rcldotnet/QoS/QosProfile.cs @@ -14,34 +14,11 @@ */ using System; -using System.Runtime.InteropServices; using System.Threading; +using ROS2.Qos; namespace ROS2 { - [StructLayout(LayoutKind.Sequential)] - internal struct RmwTime - { - public ulong Sec; - public ulong NSec; - } - - [StructLayout(LayoutKind.Sequential)] - internal struct RmwQosProfile - { - public QosHistoryPolicy History; - public UIntPtr Depth; // size_t is properly represented by UIntPtr - public QosReliabilityPolicy Reliability; - public QosDurabilityPolicy Durability; - public RmwTime Deadline; - public RmwTime Lifespan; - public QosLivelinessPolicy Liveliness; - public RmwTime LivelinessLeaseDuration; - // TODO: Windows uses a 32-bit bool representation. Confirm this works in Windows... - [MarshalAs(UnmanagedType.U1)] - public bool AvoidRosNamespaceConventions; - } - /// /// The `HISTORY` DDS QoS policy. /// @@ -232,7 +209,7 @@ private QosProfile( /// /// The default QoS profile. /// - public static QosProfile DefaultProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_default()); + public static QosProfile DefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_default); /// /// Profile for clock messages. @@ -242,27 +219,27 @@ private QosProfile( /// /// Profile for parameter event messages. /// - public static QosProfile ParameterEventsProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameter_events()); + public static QosProfile ParameterEventsProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_parameter_events); /// /// Profile for parameter messages. /// - public static QosProfile ParametersProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_parameters()); + public static QosProfile ParametersProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_parameters); /// /// Profile for sensor messages. /// - public static QosProfile SensorDataProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_sensor_data()); + public static QosProfile SensorDataProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_sensor_data); /// - /// Profile for services. + /// Default profile for services. /// - public static QosProfile ServicesProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_services_default()); + public static QosProfile ServicesDefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_services_default); /// /// The system default (null) profile. /// - public static QosProfile SystemDefaultProfile => ToQosProfile(RCLdotnetDelegates.native_rcl_qos_get_profile_system_default()); + public static QosProfile SystemDefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_system_default); /// /// The history policy. @@ -615,30 +592,21 @@ private static void ToRmwTime(TimeSpan timeSpan, out ulong sec, out ulong nsec) nsec = (ulong)(ticksInsideSecond * NanosecondsPerTick); } - private static TimeSpan FromRmwTime(RmwTime time) + // 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) { - if (time.Sec == 9223372036 && time.NSec == 854775807) - { - // see RMW_DURATION_INFINITE and comment on QosProfile.InfiniteDuration above. - return InfiniteDuration; - } + IntPtr nativeProfileConst = nativeDelegate(); - const long NanosecondsPerTick = 1000000 / TimeSpan.TicksPerMillisecond; // ~= 100. - ulong ticks = time.Sec * TimeSpan.TicksPerSecond; - ticks += time.NSec / NanosecondsPerTick; - return new TimeSpan((long)ticks); - } - - private static QosProfile ToQosProfile(IntPtr rmwProfile) - { - RmwQosProfile native = Marshal.PtrToStructure(rmwProfile); return new QosProfile( - native.History, (int)native.Depth, native.Reliability, native.Durability, - FromRmwTime(native.Deadline), - FromRmwTime(native.Lifespan), - native.Liveliness, - FromRmwTime(native.LivelinessLeaseDuration), - native.AvoidRosNamespaceConventions); + 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); } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 6a788dc7..1f931179 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -738,13 +738,6 @@ static RCLdotnetDelegates() (NativeRCLWriteToQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_write_to_qos_profile_handle_ptr, typeof(NativeRCLWriteToQosProfileHandleType)); - _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_default), out native_rcl_qos_get_profile_default); - _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_parameter_events), out native_rcl_qos_get_profile_parameter_events); - _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_parameters), out native_rcl_qos_get_profile_parameters); - _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_sensor_data), out native_rcl_qos_get_profile_sensor_data); - _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_services_default), out native_rcl_qos_get_profile_services_default); - _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_qos_get_profile_system_default), out native_rcl_qos_get_profile_system_default); - _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_clock_handle), out native_rcl_create_clock_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_clock_handle), out native_rcl_destroy_clock_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_timer_handle), out native_rcl_create_timer_handle); diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index f1063428..40be6421 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -762,30 +762,6 @@ int32_t native_rcl_write_to_qos_profile_handle( return RCL_RET_OK; } -const rmw_qos_profile_t native_rcl_qos_get_profile_default() { - return rmw_qos_profile_default; -} - -const rmw_qos_profile_t native_rcl_qos_get_profile_parameter_events() { - return rmw_qos_profile_parameter_events; -} - -const rmw_qos_profile_t native_rcl_qos_get_profile_parameters() { - return rmw_qos_profile_parameters; -} - -const rmw_qos_profile_t native_rcl_qos_get_profile_sensor_data() { - return rmw_qos_profile_sensor_data; -} - -const rmw_qos_profile_t native_rcl_qos_get_profile_services_default() { - return rmw_qos_profile_services_default; -} - -const rmw_qos_profile_t native_rcl_qos_get_profile_system_default() { - return rmw_qos_profile_system_default; -} - int32_t native_rcl_create_timer_handle(void **timer_handle, void *clock_handle, int64_t period, rcl_timer_callback_t callback) { rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_timer_t *timer = malloc(sizeof(rcl_timer_t)); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index a849ea6a..c1021a76 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -248,24 +248,6 @@ int32_t RCLDOTNET_CDECL native_rcl_write_to_qos_profile_handle( uint64_t liveliness_lease_duration_nsec, int32_t /* bool */ avoid_ros_namespace_conventions); -RCLDOTNET_EXPORT -const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_default(); - -RCLDOTNET_EXPORT -const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_parameter_events(); - -RCLDOTNET_EXPORT -const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_parameters(); - -RCLDOTNET_EXPORT -const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_sensor_data(); - -RCLDOTNET_EXPORT -const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_services_default(); - -RCLDOTNET_EXPORT -const rmw_qos_profile_t RCLDOTNET_CDECL native_rcl_qos_get_profile_system_default(); - RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_timer_handle(void **timer_handle, void *clock_handle, int64_t period, rcl_timer_callback_t callback); diff --git a/rcldotnet/rcldotnet_qos_profile.c b/rcldotnet/rcldotnet_qos_profile.c new file mode 100644 index 00000000..f140b665 --- /dev/null +++ b/rcldotnet/rcldotnet_qos_profile.c @@ -0,0 +1,94 @@ +// 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. + +#include + +#include "rcldotnet_qos_profile.h" + +const rmw_qos_profile_t * native_rcl_qos_get_const_profile_default() { + return &rmw_qos_profile_default; +} + +const rmw_qos_profile_t * native_rcl_qos_get_const_profile_parameter_events() { + return &rmw_qos_profile_parameter_events; +} + +const rmw_qos_profile_t * native_rcl_qos_get_const_profile_parameters() { + return &rmw_qos_profile_parameters; +} + +const rmw_qos_profile_t * native_rcl_qos_get_const_profile_sensor_data() { + return &rmw_qos_profile_sensor_data; +} + +const rmw_qos_profile_t * native_rcl_qos_get_const_profile_services_default() { + return &rmw_qos_profile_services_default; +} + +const rmw_qos_profile_t * native_rcl_qos_get_const_profile_system_default() { + return &rmw_qos_profile_system_default; +} + +int32_t native_rcl_qos_profile_read_history(void *qos_profile_handle) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + return (int32_t)qos_profile->history; +} + +int32_t native_rcl_qos_profile_read_depth(void *qos_profile_handle) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + return (int32_t)qos_profile->depth; +} + +int32_t native_rcl_qos_profile_read_reliability(void *qos_profile_handle) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + return (int32_t)qos_profile->reliability; +} + +int32_t native_rcl_qos_profile_read_durability(void *qos_profile_handle) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + return (int32_t)qos_profile->durability; +} + +void rcldotnet_qos_profile_read_rmw_time(rmw_time_t *time, void *sec, void *nsec) { + int64_t *sec_ptr = (int64_t *)sec; + *sec_ptr = time->sec; + + int64_t *nsec_ptr = (int64_t *)nsec; + *nsec_ptr = time->nsec; +} + +void native_rcl_qos_profile_read_deadline(void *qos_profile_handle, void *sec, void *nsec) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + rcldotnet_qos_profile_read_rmw_time(&qos_profile->deadline, sec, nsec); +} + +void native_rcl_qos_profile_read_lifespan(void *qos_profile_handle, void *sec, void *nsec) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + rcldotnet_qos_profile_read_rmw_time(&qos_profile->lifespan, sec, nsec); +} + +int32_t native_rcl_qos_profile_read_liveliness(void *qos_profile_handle) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + return (int32_t)qos_profile->liveliness; +} + +void native_rcl_qos_profile_read_liveliness_lease_duration(void *qos_profile_handle, void *sec, void *nsec) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + rcldotnet_qos_profile_read_rmw_time(&qos_profile->liveliness_lease_duration, sec, nsec); +} + +int32_t /* bool */ native_rcl_qos_profile_read_avoid_ros_namespace_conventions(void *qos_profile_handle) { + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + return qos_profile->avoid_ros_namespace_conventions; +} diff --git a/rcldotnet/rcldotnet_qos_profile.h b/rcldotnet/rcldotnet_qos_profile.h new file mode 100644 index 00000000..557f73d5 --- /dev/null +++ b/rcldotnet/rcldotnet_qos_profile.h @@ -0,0 +1,67 @@ +// 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. + +#ifndef RCLDOTNET_QOS_PROFILE_H +#define RCLDOTNET_QOS_PROFILE_H + +#include "rcldotnet_macros.h" + +// The below profile getters are intentionally provided as const pointers to avoid construction of copies. + +RCLDOTNET_EXPORT +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_default(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_parameter_events(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_parameters(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_sensor_data(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_services_default(); + +RCLDOTNET_EXPORT +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_system_default(); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_qos_profile_read_history(void *qos_profile_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_qos_profile_read_depth(void *qos_profile_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_qos_profile_read_reliability(void *qos_profile_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_qos_profile_read_durability(void *qos_profile_handle); + +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_qos_profile_read_deadline(void *qos_profile_handle, void *sec, void *nsec); + +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_qos_profile_read_lifespan(void *qos_profile_handle, void *sec, void *nsec); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_qos_profile_read_liveliness(void *qos_profile_handle); + +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_qos_profile_read_liveliness_lease_duration(void *qos_profile_handle, void *sec, void *nsec); + +RCLDOTNET_EXPORT +int32_t /* bool */ RCLDOTNET_CDECL native_rcl_qos_profile_read_avoid_ros_namespace_conventions(void *qos_profile_handle); + +#endif // RCLDOTNET_QOS_PROFILE_H From 4182a4e9d875dbd3803b831ebd1cf454851a741c Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Tue, 31 Oct 2023 10:20:11 +1000 Subject: [PATCH 5/8] Adding description of QoS Profile defaults for ease of use --- rcldotnet/QoS/QosProfile.cs | 97 +++++++++++++++++++++++++++++++++++++ 1 file changed, 97 insertions(+) diff --git a/rcldotnet/QoS/QosProfile.cs b/rcldotnet/QoS/QosProfile.cs index 0d1fd2ed..e55862df 100644 --- a/rcldotnet/QoS/QosProfile.cs +++ b/rcldotnet/QoS/QosProfile.cs @@ -208,36 +208,132 @@ private QosProfile( /// /// 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 | /// public static QosProfile DefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_default); /// /// 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 | /// public static QosProfile ClockProfile => CreateClockProfile(); /// /// 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 | /// public static QosProfile ParameterEventsProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_parameter_events); /// /// 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 | /// public static QosProfile ParametersProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_parameters); /// /// 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 | /// public static QosProfile SensorDataProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_sensor_data); /// /// 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 | /// public static QosProfile ServicesDefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_services_default); /// /// 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 | /// public static QosProfile SystemDefaultProfile => ReadConstQoSProfile(QosProfileDelegates.native_rcl_qos_get_const_profile_system_default); @@ -521,6 +617,7 @@ private static QosProfile CreateClockProfile() { // 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: 1, From 90b7ec717252329c339792ad003818625ac94128 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Tue, 16 Jan 2024 17:13:54 +1000 Subject: [PATCH 6/8] Removing unused QOS profile delegates --- rcldotnet/RCLdotnet.cs | 9 --------- 1 file changed, 9 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 1f931179..eeefea8a 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -353,15 +353,6 @@ internal delegate RCLRet NativeRCLWriteToQosProfileHandleType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr NativeRCLGetStringType(SafeHandle handle); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate IntPtr NativeRCLGetQosProfileType(); - internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_default = null; - internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_parameter_events = null; - internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_parameters = null; - internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_sensor_data = null; - internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_services_default = null; - internal static NativeRCLGetQosProfileType native_rcl_qos_get_profile_system_default = null; - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreateClockHandleType(ref SafeClockHandle clockHandle, int clockType); From b3dba613b188ca032c25b7db4ab9669880f6d898 Mon Sep 17 00:00:00 2001 From: Rene Moll Date: Fri, 9 Feb 2024 16:37:21 +0100 Subject: [PATCH 7/8] rcldotnet_qos_profile: fix naming for DLL usage --- rcldotnet/rcldotnet_qos_profile.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rcldotnet/rcldotnet_qos_profile.h b/rcldotnet/rcldotnet_qos_profile.h index 557f73d5..c3cc4fd5 100644 --- a/rcldotnet/rcldotnet_qos_profile.h +++ b/rcldotnet/rcldotnet_qos_profile.h @@ -20,22 +20,22 @@ // The below profile getters are intentionally provided as const pointers to avoid construction of copies. RCLDOTNET_EXPORT -const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_default(); +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_const_profile_default(); RCLDOTNET_EXPORT -const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_parameter_events(); +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_const_profile_parameter_events(); RCLDOTNET_EXPORT -const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_parameters(); +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_const_profile_parameters(); RCLDOTNET_EXPORT -const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_sensor_data(); +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_const_profile_sensor_data(); RCLDOTNET_EXPORT -const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_services_default(); +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_const_profile_services_default(); RCLDOTNET_EXPORT -const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_profile_system_default(); +const rmw_qos_profile_t * RCLDOTNET_CDECL native_rcl_qos_get_const_profile_system_default(); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_qos_profile_read_history(void *qos_profile_handle); From 125e979df17f4f5b6582271d692fbc05e793962b Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 4 Apr 2024 21:53:09 +0200 Subject: [PATCH 8/8] move QoS files back --- rcldotnet/CMakeLists.txt | 4 ++-- rcldotnet/{QoS => }/QoSProfileDelegates.cs | 2 +- rcldotnet/{QoS => }/QosProfile.cs | 27 +++++++++++----------- 3 files changed, 16 insertions(+), 17 deletions(-) rename rcldotnet/{QoS => }/QoSProfileDelegates.cs (99%) rename rcldotnet/{QoS => }/QosProfile.cs (99%) diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index f80380fc..257b2751 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -45,6 +45,8 @@ set(CS_SOURCES MessageStaticMemberCache.cs Node.cs Publisher.cs + QosProfile.cs + QoSProfileDelegates.cs RCLdotnet.cs RCLExceptionHelper.cs RCLRet.cs @@ -66,8 +68,6 @@ set(CS_SOURCES ServiceDefinitionStaticMemberCache.cs Subscription.cs Timer.cs - QoS/QosProfile.cs - QoS/QoSProfileDelegates.cs ) find_package(rcldotnet_common REQUIRED) diff --git a/rcldotnet/QoS/QoSProfileDelegates.cs b/rcldotnet/QoSProfileDelegates.cs similarity index 99% rename from rcldotnet/QoS/QoSProfileDelegates.cs rename to rcldotnet/QoSProfileDelegates.cs index 0530677b..c9666def 100644 --- a/rcldotnet/QoS/QoSProfileDelegates.cs +++ b/rcldotnet/QoSProfileDelegates.cs @@ -17,7 +17,7 @@ using System.Runtime.InteropServices; using ROS2.Utils; -namespace ROS2.Qos +namespace ROS2 { internal struct RmwTime { diff --git a/rcldotnet/QoS/QosProfile.cs b/rcldotnet/QosProfile.cs similarity index 99% rename from rcldotnet/QoS/QosProfile.cs rename to rcldotnet/QosProfile.cs index e55862df..37cfa9ae 100644 --- a/rcldotnet/QoS/QosProfile.cs +++ b/rcldotnet/QosProfile.cs @@ -15,7 +15,6 @@ using System; using System.Threading; -using ROS2.Qos; namespace ROS2 { @@ -208,10 +207,10 @@ private QosProfile( /// /// 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 | @@ -228,7 +227,7 @@ private QosProfile( /// /// Profile for clock messages. /// See CreateClockProfile for more details. - /// + /// /// | --------------- | --------------- | /// | History | KEEP_LAST | /// | Depth | 1 | @@ -244,10 +243,10 @@ private QosProfile( /// /// 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 | @@ -263,10 +262,10 @@ private QosProfile( /// /// 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 | @@ -282,10 +281,10 @@ private QosProfile( /// /// 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 | @@ -301,10 +300,10 @@ private QosProfile( /// /// 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 | @@ -320,10 +319,10 @@ private QosProfile( /// /// 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 |