From 94019296295d7529a7d30e021f14c75ff4c988a1 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 19 Aug 2021 10:00:44 +0200 Subject: [PATCH 01/73] Remove unnecessary if --- rosidl_generator_dotnet/resource/msg.c.em | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rosidl_generator_dotnet/resource/msg.c.em b/rosidl_generator_dotnet/resource/msg.c.em index 7c4c64db..900395e5 100644 --- a/rosidl_generator_dotnet/resource/msg.c.em +++ b/rosidl_generator_dotnet/resource/msg.c.em @@ -52,11 +52,7 @@ void * @(msg_typename)__get_field_@(member.name)_message(void *message_handle, i } int @(msg_typename)__getsize_array_field_@(member.name)_message() { -@[ if isinstance(member.type, Array)]@ return @(member.type.size); -@[ else]@ - return 0; -@[ end if]@ } @[ if isinstance(member.type.value_type, BasicType)]@ From 52363ebd8585ab8c27add103dcc04419e1e0d7ac Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 19 Aug 2021 10:04:17 +0200 Subject: [PATCH 02/73] Formating and improve formating of generated code --- rosidl_generator_dotnet/resource/msg.cs.em | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index f0edc7b3..203b5c31 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -212,16 +212,16 @@ public class @(type_name) : IMessage { @(get_field_name(type_name, member.name)).Clear(); for (int i=0; i Date: Thu, 19 Aug 2021 10:06:44 +0200 Subject: [PATCH 03/73] Add support for arrays of strings --- rosidl_generator_dotnet/resource/msg.c.em | 7 ++++--- rosidl_generator_dotnet/resource/msg.cs.em | 7 +++---- rosidl_generator_dotnet/resource/msg.h.em | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rosidl_generator_dotnet/resource/msg.c.em b/rosidl_generator_dotnet/resource/msg.c.em index 900395e5..230bcf4a 100644 --- a/rosidl_generator_dotnet/resource/msg.c.em +++ b/rosidl_generator_dotnet/resource/msg.c.em @@ -70,13 +70,14 @@ void @(msg_typename)__write_field_@(member.name)(void *message_handle, @(msg_typ @[ elif isinstance(member.type.value_type, AbstractString)]@ void @(msg_typename)__write_field_@(member.name)(void *message_handle, @(msg_type_to_c(member.type.value_type)) value) { - + rosidl_runtime_c__String * ros_message = (rosidl_runtime_c__String *)message_handle; + rosidl_runtime_c__String__assign(ros_message, value); } @(msg_type_to_c(member.type.value_type)) @(msg_typename)__read_field_@(member.name)(void *message_handle) { - @(msg_typename) * ros_message = (@(msg_typename)*)message_handle; - return ros_message->@(member.name)->data; + rosidl_runtime_c__String * ros_message = (rosidl_runtime_c__String *)message_handle; + return ros_message->data; } @[ end if]@ diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 203b5c31..470448d8 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -215,14 +215,15 @@ public class @(type_name) : IMessage { @[ if isinstance(member.type.value_type, BasicType)] @(get_field_name(type_name, member.name)).Add(native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i))); @[ elif isinstance(member.type.value_type, AbstractString)] - // TODO: String types are not supported + IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i)); + @(get_field_name(type_name, member.name)).Add(Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name)))); @[ elif isinstance(member.type.value_type, AbstractWString)] // TODO: Unicode types are not supported @[ else] @(get_field_name(type_name, member.name)).Add(new @(get_dotnet_type(member.type.value_type))()); @(get_field_name(type_name, member.name))[@(get_field_name(type_name, member.name)).Count-1]._READ_HANDLE(native_get_field_@(member.name)_message(messageHandle, i)); @[ end if] - } + } } @[ elif isinstance(member.type, AbstractSequence)]@ @@ -251,8 +252,6 @@ public class @(type_name) : IMessage { { @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, count++), value); -@[ elif isinstance(member.type.value_type, AbstractString)] -// TODO: String types are not supported @[ elif isinstance(member.type.value_type, AbstractWString)] // TODO: Unicode types are not supported @[ else]@ diff --git a/rosidl_generator_dotnet/resource/msg.h.em b/rosidl_generator_dotnet/resource/msg.h.em index 2e2fb805..b158113d 100644 --- a/rosidl_generator_dotnet/resource/msg.h.em +++ b/rosidl_generator_dotnet/resource/msg.h.em @@ -51,7 +51,7 @@ void * @(msg_prefix)_CDECL @(msg_typename)__get_field_@(member.name)_message(voi @(msg_prefix)_EXPORT int @(msg_prefix)_CDECL @(msg_typename)__getsize_array_field_@(member.name)_message(); -@[ if isinstance(member.type.value_type, BasicType)]@ +@[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ @(msg_prefix)_EXPORT void @(msg_typename)__write_field_@(member.name)(void *, @(msg_type_to_c(member.type.value_type))); @(msg_prefix)_EXPORT From cb0f23f7bab250fd0a01795c454f44e9c30e44f7 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 19 Aug 2021 10:07:10 +0200 Subject: [PATCH 04/73] Uncomment test for arrays of strings --- rcldotnet/test/test_messages.cs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index 3cb7e172..e8b842c6 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -159,9 +159,9 @@ public void TestPublishArrays() msg.Uint64_values.Add(35); // string_values - // msg.String_values.Add("one"); - // msg.String_values.Add("two"); - // msg.String_values.Add("three"); + msg.String_values.Add("one"); + msg.String_values.Add("two"); + msg.String_values.Add("three"); test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); basic_type_1.Bool_value = true; @@ -295,10 +295,10 @@ public void TestPublishArrays() Assert.Equal((ulong)35, msg2.Uint64_values[2]); // string_values - // Not supported yet - // Assert.Equal("one", msg2.String_values[0]); - // Assert.Equal("two", msg2.String_values[1]); - // Assert.Equal("three", msg2.String_values[2]); + Assert.Equal("one", msg2.String_values[0]); + Assert.Equal("two", msg2.String_values[1]); + Assert.Equal("three", msg2.String_values[2]); + Assert.Equal(3, msg2.Basic_types_values.Count); Assert.True(msg2.Basic_types_values[0].Bool_value); Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); From 2b198f483df6222587c92e42e4ebb27a608f0a70 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 19 Aug 2021 14:09:40 +0200 Subject: [PATCH 05/73] Generate array size constants Named {member}_Count as this is the normal .NET lingo. removed getsize_array_field c function as well as it is constant, no need for interop --- rcldotnet/test/test_messages.cs | 29 ++++++++++++++++++++++ rosidl_generator_dotnet/resource/msg.c.em | 4 --- rosidl_generator_dotnet/resource/msg.cs.em | 13 +++++----- rosidl_generator_dotnet/resource/msg.h.em | 2 -- 4 files changed, 36 insertions(+), 12 deletions(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index e8b842c6..3a28cbf4 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -230,75 +230,104 @@ public void TestPublishArrays() } // bool_values + Assert.Equal(3, test_msgs.msg.Arrays.Bool_values_Count); + Assert.Equal(3, msg2.Bool_values.Count); Assert.True(msg2.Bool_values[0]); Assert.False(msg2.Bool_values[1]); Assert.True(msg2.Bool_values[2]); // byte_values + Assert.Equal(3, test_msgs.msg.Arrays.Byte_values_Count); + Assert.Equal(3, msg2.Byte_values.Count); Assert.Equal(0, msg2.Byte_values[0]); Assert.Equal(1, msg2.Byte_values[1]); Assert.Equal(2, msg2.Byte_values[2]); // char_values + Assert.Equal(3, test_msgs.msg.Arrays.Char_values_Count); + Assert.Equal(3, msg2.Char_values.Count); Assert.Equal(3, msg2.Char_values[0]); Assert.Equal(4, msg2.Char_values[1]); Assert.Equal(5, msg2.Char_values[2]); // float32_values + Assert.Equal(3, test_msgs.msg.Arrays.Float32_values_Count); + Assert.Equal(3, msg2.Float32_values.Count); Assert.Equal(6.1f, msg2.Float32_values[0]); Assert.Equal(7.1f, msg2.Float32_values[1]); Assert.Equal(8.1f, msg2.Float32_values[2]); // float64_values + Assert.Equal(3, test_msgs.msg.Arrays.Float64_values_Count); + Assert.Equal(3, msg2.Float64_values.Count); Assert.Equal(9.1, msg2.Float64_values[0]); Assert.Equal(10.1, msg2.Float64_values[1]); Assert.Equal(11.1, msg2.Float64_values[2]); // int8_values + Assert.Equal(3, test_msgs.msg.Arrays.Int8_values_Count); + Assert.Equal(3, msg2.Int8_values.Count); Assert.Equal(12, msg2.Int8_values[0]); Assert.Equal(13, msg2.Int8_values[1]); Assert.Equal(14, msg2.Int8_values[2]); // uint8_values + Assert.Equal(3, test_msgs.msg.Arrays.Uint8_values_Count); + Assert.Equal(3, msg2.Uint8_values.Count); Assert.Equal(15, msg2.Uint8_values[0]); Assert.Equal(16, msg2.Uint8_values[1]); Assert.Equal(17, msg2.Uint8_values[2]); // int16_values + Assert.Equal(3, test_msgs.msg.Arrays.Int16_values_Count); + Assert.Equal(3, msg2.Int16_values.Count); Assert.Equal(18, msg2.Int16_values[0]); Assert.Equal(19, msg2.Int16_values[1]); Assert.Equal(20, msg2.Int16_values[2]); // uint16_values + Assert.Equal(3, test_msgs.msg.Arrays.Uint16_values_Count); + Assert.Equal(3, msg2.Uint16_values.Count); Assert.Equal(21, msg2.Uint16_values[0]); Assert.Equal(22, msg2.Uint16_values[1]); Assert.Equal(23, msg2.Uint16_values[2]); // int32_values + Assert.Equal(3, test_msgs.msg.Arrays.Int32_values_Count); + Assert.Equal(3, msg2.Int32_values.Count); Assert.Equal(24, msg2.Int32_values[0]); Assert.Equal(25, msg2.Int32_values[1]); Assert.Equal(26, msg2.Int32_values[2]); // uint32_values + Assert.Equal(3, test_msgs.msg.Arrays.Uint32_values_Count); + Assert.Equal(3, msg2.Uint32_values.Count); Assert.Equal((uint)27, msg2.Uint32_values[0]); Assert.Equal((uint)28, msg2.Uint32_values[1]); Assert.Equal((uint)29, msg2.Uint32_values[2]); // int64_values + Assert.Equal(3, test_msgs.msg.Arrays.Int64_values_Count); + Assert.Equal(3, msg2.Int64_values.Count); Assert.Equal(30, msg2.Int64_values[0]); Assert.Equal(31, msg2.Int64_values[1]); Assert.Equal(32, msg2.Int64_values[2]); // uint64_values + Assert.Equal(3, test_msgs.msg.Arrays.Uint64_values_Count); + Assert.Equal(3, msg2.Uint64_values.Count); Assert.Equal((ulong)33, msg2.Uint64_values[0]); Assert.Equal((ulong)34, msg2.Uint64_values[1]); Assert.Equal((ulong)35, msg2.Uint64_values[2]); // string_values + Assert.Equal(3, test_msgs.msg.Arrays.String_values_Count); + Assert.Equal(3, msg2.String_values.Count); Assert.Equal("one", msg2.String_values[0]); Assert.Equal("two", msg2.String_values[1]); Assert.Equal("three", msg2.String_values[2]); + Assert.Equal(3, test_msgs.msg.Arrays.Basic_types_values_Count); Assert.Equal(3, msg2.Basic_types_values.Count); Assert.True(msg2.Basic_types_values[0].Bool_value); Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); diff --git a/rosidl_generator_dotnet/resource/msg.c.em b/rosidl_generator_dotnet/resource/msg.c.em index 230bcf4a..6df3db00 100644 --- a/rosidl_generator_dotnet/resource/msg.c.em +++ b/rosidl_generator_dotnet/resource/msg.c.em @@ -51,10 +51,6 @@ void * @(msg_typename)__get_field_@(member.name)_message(void *message_handle, i return &(ros_message->@(member.name)[index]); } -int @(msg_typename)__getsize_array_field_@(member.name)_message() { - return @(member.type.size); -} - @[ if isinstance(member.type.value_type, BasicType)]@ void @(msg_typename)__write_field_@(member.name)(void *message_handle, @(msg_type_to_c(member.type.value_type)) value) { diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 470448d8..c68d57b6 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -31,6 +31,12 @@ namespace @(ns) public class @(type_name) : IMessage { private static readonly DllLoadUtils dllLoadUtils; +@[for member in message.structure.members]@ +@[ if isinstance(member.type, Array)]@ + public const int @(get_field_name(type_name, member.name))_Count = @(member.type.size); +@[ end if]@ +@[end for]@ + public @(type_name)() { @[for member in message.structure.members]@ @@ -76,7 +82,6 @@ public class @(type_name) : IMessage { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ IntPtr native_get_field_@(member.name)_message_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__get_field_@(member.name)_message"); - IntPtr native_getsize_array_field_@(member.name)_message_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__getsize_array_field_@(member.name)_message"); @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ IntPtr native_write_field_@(member.name)_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__write_field_@(member.name)"); IntPtr native_read_field_@(member.name)_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__read_field_@(member.name)"); @@ -84,8 +89,6 @@ public class @(type_name) : IMessage { @(type_name).native_get_field_@(member.name)_message = (NativeGetField@(get_field_name(type_name, member.name))Type)Marshal.GetDelegateForFunctionPointer( native_get_field_@(member.name)_message_ptr, typeof(NativeGetField@(get_field_name(type_name, member.name))Type)); - @(type_name).native_getsize_array_field_@(member.name)_message = (NativeGetSizeArrayField@(get_field_name(type_name, member.name))Type)Marshal.GetDelegateForFunctionPointer( - native_getsize_array_field_@(member.name)_message_ptr, typeof(NativeGetSizeArrayField@(get_field_name(type_name, member.name))Type)); @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ @(type_name).native_write_field_@(member.name) = (NativeWriteField@(get_field_name(type_name, member.name))Type)Marshal.GetDelegateForFunctionPointer( @@ -139,7 +142,6 @@ public class @(type_name) : IMessage { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ private static NativeGetField@(get_field_name(type_name, member.name))Type native_get_field_@(member.name)_message = null; - private static NativeGetSizeArrayField@(get_field_name(type_name, member.name))Type native_getsize_array_field_@(member.name)_message = null; @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ private static NativeWriteField@(get_field_name(type_name, member.name))Type native_write_field_@(member.name) = null; private static NativeReadField@(get_field_name(type_name, member.name))Type native_read_field_@(member.name) = null; @@ -208,9 +210,8 @@ public class @(type_name) : IMessage { @[ if isinstance(member.type, Array)]@ { - int size = native_getsize_array_field_@(member.name)_message(messageHandle); @(get_field_name(type_name, member.name)).Clear(); - for (int i=0; i Date: Fri, 20 Aug 2021 13:32:45 +0200 Subject: [PATCH 06/73] Add initial support for sequences --- rosidl_generator_dotnet/resource/msg.c.em | 47 +++++++++++++-- rosidl_generator_dotnet/resource/msg.cs.em | 69 ++++++++++++++-------- rosidl_generator_dotnet/resource/msg.h.em | 12 +++- 3 files changed, 96 insertions(+), 32 deletions(-) diff --git a/rosidl_generator_dotnet/resource/msg.c.em b/rosidl_generator_dotnet/resource/msg.c.em index 6df3db00..34ff7439 100644 --- a/rosidl_generator_dotnet/resource/msg.c.em +++ b/rosidl_generator_dotnet/resource/msg.c.em @@ -1,4 +1,6 @@ @{ +from rosidl_generator_c import idl_type_to_c + from rosidl_generator_dotnet import msg_type_to_c from rosidl_parser.definition import AbstractNestedType @@ -22,6 +24,8 @@ header_filename = "{0}/rcldotnet_{1}.h".format('/'.join(message.structure.namesp #include #include +#include + #include <@('/'.join(message.structure.namespaced_type.namespaces))/@(convert_camel_case_to_lower_case_underscore(type_name)).h> #include "rosidl_runtime_c/message_type_support_struct.h" @@ -45,12 +49,50 @@ const void * @(msg_typename)__get_typesupport() { } @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array)]@ +@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ void * @(msg_typename)__get_field_@(member.name)_message(void *message_handle, int index) { @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; +@[ if isinstance(member.type, Array)]@ return &(ros_message->@(member.name)[index]); +@[ elif isinstance(member.type, AbstractSequence)]@ + return &(ros_message->@(member.name).data[index]); +@[ end if]@ +} + +@[ if isinstance(member.type, AbstractSequence)]@ +int @(msg_typename)__getsize_field_@(member.name)_message(void *message_handle) { + @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; + return ros_message->@(member.name).size; } +bool @(msg_typename)__init_sequence_field_@(member.name)_message(void *message_handle, int size) { + @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + if (ros_message->@(member.name).data) { + allocator.deallocate(ros_message->@(member.name).data, allocator.state); + ros_message->@(member.name).data = NULL; + ros_message->@(member.name).size = 0; + ros_message->@(member.name).capacity = 0; + } + + @(idl_type_to_c(member.type.value_type)) * data = NULL; + if (size) { + data = (@(idl_type_to_c(member.type.value_type)) *)allocator.zero_allocate( + size, sizeof(@(idl_type_to_c(member.type.value_type))), allocator.state); + if (!data) { + return false; + } + } + + ros_message->@(member.name).data = data; + ros_message->@(member.name).size = size; + ros_message->@(member.name).capacity = size; + return true; +} +@[ end if]@ + @[ if isinstance(member.type.value_type, BasicType)]@ void @(msg_typename)__write_field_@(member.name)(void *message_handle, @(msg_type_to_c(member.type.value_type)) value) { @@ -77,9 +119,6 @@ void @(msg_typename)__write_field_@(member.name)(void *message_handle, @(msg_typ } @[ end if]@ -//////////////////////////////////////////////////////// -@[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index c68d57b6..206fda07 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -34,6 +34,8 @@ public class @(type_name) : IMessage { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ public const int @(get_field_name(type_name, member.name))_Count = @(member.type.size); +@[ elif isinstance(member.type, AbstractSequence) and member.type.has_maximum_size()]@ + public const int @(get_field_name(type_name, member.name))_MaxCount = @(member.type.maximum_size); @[ end if]@ @[end for]@ @@ -41,13 +43,9 @@ public class @(type_name) : IMessage { { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ -@[ if isinstance(member.type.value_type, BasicType)]@ - // @(member.type.value_type) @(member.type.value_type.typename) -@[ end if]@ @(get_field_name(type_name, member.name)) = new List<@(get_dotnet_type(member.type.value_type))>(@(member.type.size)); - @[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported + @(get_field_name(type_name, member.name)) = new List<@(get_dotnet_type(member.type.value_type))>(); @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType)]@ @@ -80,8 +78,12 @@ public class @(type_name) : IMessage { native_destroy_native_message_ptr, typeof(NativeDestroyNativeType)); @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array)]@ +@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ IntPtr native_get_field_@(member.name)_message_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__get_field_@(member.name)_message"); +@[ if isinstance(member.type, AbstractSequence)]@ + IntPtr native_getsize_field_@(member.name)_message_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__getsize_field_@(member.name)_message"); + IntPtr native_init_sequence_field_@(member.name)_message_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__init_sequence_field_@(member.name)_message"); +@[ end if]@ @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ IntPtr native_write_field_@(member.name)_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__write_field_@(member.name)"); IntPtr native_read_field_@(member.name)_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(msg_typename)__read_field_@(member.name)"); @@ -90,6 +92,12 @@ public class @(type_name) : IMessage { @(type_name).native_get_field_@(member.name)_message = (NativeGetField@(get_field_name(type_name, member.name))Type)Marshal.GetDelegateForFunctionPointer( native_get_field_@(member.name)_message_ptr, typeof(NativeGetField@(get_field_name(type_name, member.name))Type)); +@[ if isinstance(member.type, AbstractSequence)]@ + @(type_name).native_getsize_field_@(member.name)_message = (NativeGetSizeField@(get_field_name(type_name, member.name))Type)Marshal.GetDelegateForFunctionPointer( + native_getsize_field_@(member.name)_message_ptr, typeof(NativeGetSizeField@(get_field_name(type_name, member.name))Type)); + @(type_name).native_init_seqence_field_@(member.name)_message = (NativeInitSequenceField@(get_field_name(type_name, member.name))Type)Marshal.GetDelegateForFunctionPointer( + native_init_sequence_field_@(member.name)_message_ptr, typeof(NativeInitSequenceField@(get_field_name(type_name, member.name))Type)); +@[ end if]@ @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ @(type_name).native_write_field_@(member.name) = (NativeWriteField@(get_field_name(type_name, member.name))Type)Marshal.GetDelegateForFunctionPointer( native_write_field_@(member.name)_ptr, typeof(NativeWriteField@(get_field_name(type_name, member.name))Type)); @@ -97,8 +105,6 @@ public class @(type_name) : IMessage { native_read_field_@(member.name)_ptr, typeof(NativeReadField@(get_field_name(type_name, member.name))Type)); @[ end if]@ -@[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @@ -140,8 +146,12 @@ public class @(type_name) : IMessage { private static NativeDestroyNativeType native_destroy_native_message = null; @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array)]@ +@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ private static NativeGetField@(get_field_name(type_name, member.name))Type native_get_field_@(member.name)_message = null; +@[ if isinstance(member.type, AbstractSequence)]@ + private static NativeGetSizeField@(get_field_name(type_name, member.name))Type native_getsize_field_@(member.name)_message = null; + private static NativeInitSequenceField@(get_field_name(type_name, member.name))Type native_init_seqence_field_@(member.name)_message = null; +@[ end if]@ @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ private static NativeWriteField@(get_field_name(type_name, member.name))Type native_write_field_@(member.name) = null; private static NativeReadField@(get_field_name(type_name, member.name))Type native_read_field_@(member.name) = null; @@ -149,8 +159,14 @@ public class @(type_name) : IMessage { [UnmanagedFunctionPointer(CallingConvention.Cdecl)] private delegate IntPtr NativeGetField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, int index); +@[ if isinstance(member.type, AbstractSequence)]@ + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + private delegate int NativeGetSizeField@(get_field_name(type_name, member.name))Type( + IntPtr messageHandle); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate int NativeGetSizeArrayField@(get_field_name(type_name, member.name))Type(IntPtr messageHandle); + private delegate bool NativeInitSequenceField@(get_field_name(type_name, member.name))Type( + IntPtr messageHandle, int size); +@[ end if]@ @[ if isinstance(member.type.value_type, AbstractString)]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] private delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( @@ -165,8 +181,6 @@ public class @(type_name) : IMessage { private delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, @(get_dotnet_type(member.type.value_type)) value); @[ end if]@ -@[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @@ -207,11 +221,16 @@ public class @(type_name) : IMessage { public void _READ_HANDLE(IntPtr messageHandle) { @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array)]@ +@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ { @(get_field_name(type_name, member.name)).Clear(); +@[ if isinstance(member.type, AbstractSequence)]@ + int size__local_variable = native_getsize_field_@(member.name)_message(messageHandle); + for (int i = 0; i < size__local_variable; i++) +@[ else]@ for (int i = 0; i < @(member.type.size); i++) +@[ end if]@ { @[ if isinstance(member.type.value_type, BasicType)] @(get_field_name(type_name, member.name)).Add(native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i))); @@ -227,8 +246,6 @@ public class @(type_name) : IMessage { } } -@[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @@ -246,23 +263,27 @@ public class @(type_name) : IMessage { public void _WRITE_HANDLE(IntPtr messageHandle) { @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array)]@ +@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ { - int count = 0; + int count__local_variable = 0; +@[ if isinstance(member.type, AbstractSequence)]@ + if (!native_init_seqence_field_@(member.name)_message(messageHandle, @(get_field_name(type_name, member.name)).Count)) + { + throw new Exception("The method 'native_init_seqence_field_@(member.name)_message()' failed."); + } +@[ end if]@ foreach(@(get_dotnet_type(member.type.value_type)) value in @(get_field_name(type_name, member.name))) { @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ - native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, count++), value); + native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, count__local_variable++), value); @[ elif isinstance(member.type.value_type, AbstractWString)] // TODO: Unicode types are not supported @[ else]@ - value._WRITE_HANDLE(native_get_field_@(member.name)_message(messageHandle, count++)); + value._WRITE_HANDLE(native_get_field_@(member.name)_message(messageHandle, count__local_variable++)); @[ end if]@ } } -@[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @@ -283,10 +304,8 @@ public class @(type_name) : IMessage { @[end for]@ @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array)]@ - public List<@(get_dotnet_type(member.type.value_type))> @(get_field_name(type_name, member.name)); -@[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported +@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ + public List<@(get_dotnet_type(member.type.value_type))> @(get_field_name(type_name, member.name)) { get; set; } @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ else]@ diff --git a/rosidl_generator_dotnet/resource/msg.h.em b/rosidl_generator_dotnet/resource/msg.h.em index 0514a41f..39df84bd 100644 --- a/rosidl_generator_dotnet/resource/msg.h.em +++ b/rosidl_generator_dotnet/resource/msg.h.em @@ -45,10 +45,18 @@ void * @(msg_prefix)_CDECL @(msg_typename)__create_native_message(); void @(msg_prefix)_CDECL @(msg_typename)__destroy_native_message(void *); @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array)]@ +@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ @(msg_prefix)_EXPORT void * @(msg_prefix)_CDECL @(msg_typename)__get_field_@(member.name)_message(void *, int); +@[ if isinstance(member.type, AbstractSequence)]@ +@(msg_prefix)_EXPORT +int @(msg_prefix)_CDECL @(msg_typename)__getsize_field_@(member.name)_message(void *); + +@(msg_prefix)_EXPORT +bool @(msg_prefix)_CDECL @(msg_typename)__init_sequence_field_@(member.name)_message(void *, int); + +@[ end if]@ @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ @(msg_prefix)_EXPORT void @(msg_typename)__write_field_@(member.name)(void *, @(msg_type_to_c(member.type.value_type))); @@ -56,8 +64,6 @@ void @(msg_typename)__write_field_@(member.name)(void *, @(msg_type_to_c(member. @(msg_type_to_c(member.type.value_type)) @(msg_prefix)_CDECL @(msg_typename)__read_field_@(member.name)(void *); @[ end if]@ -@[ elif isinstance(member.type, AbstractSequence)]@ -// TODO: Sequence types are not supported @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ From 0820c7828afb60ca923caba2f383129f7cc6bdb6 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 20 Aug 2021 13:33:13 +0200 Subject: [PATCH 07/73] Add tests for sequences --- rcldotnet/test/test_messages.cs | 570 +++++++++++++++++++++++++++++++- 1 file changed, 567 insertions(+), 3 deletions(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index 3a28cbf4..71cbbcf4 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -86,8 +86,8 @@ public void TestPublishBuiltins() public void TestPublishArrays() { RCLdotnet.Init (); - INode node_array_1 = RCLdotnet.CreateNode ("test_string_1"); - INode node_array_2 = RCLdotnet.CreateNode ("test_string_2"); + INode node_array_1 = RCLdotnet.CreateNode ("test_arrays_1"); + INode node_array_2 = RCLdotnet.CreateNode ("test_arrays_2"); IPublisher chatter_pub = node_array_1.CreatePublisher ("topic_array"); test_msgs.msg.Arrays msg = new test_msgs.msg.Arrays (); @@ -372,5 +372,569 @@ public void TestPublishArrays() Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); } + [Fact] + public void TestPublishUnboundedSequences() + { + RCLdotnet.Init(); + INode node_array_1 = RCLdotnet.CreateNode("test_unbounded_sequences_1"); + INode node_array_2 = RCLdotnet.CreateNode("test_unbounded_sequences_2"); + IPublisher chatter_pub = node_array_1.CreatePublisher("topic_unbounded_sequences"); + + test_msgs.msg.UnboundedSequences msg = new test_msgs.msg.UnboundedSequences(); + test_msgs.msg.UnboundedSequences msg2 = new test_msgs.msg.UnboundedSequences(); + + // bool_values + msg.Bool_values.Add(true); + msg.Bool_values.Add(false); + msg.Bool_values.Add(true); + + // byte_values + msg.Byte_values.Add(0); + msg.Byte_values.Add(1); + msg.Byte_values.Add(2); + + // char_values + msg.Char_values.Add(3); + msg.Char_values.Add(4); + msg.Char_values.Add(5); + + // float32_values + msg.Float32_values.Add(6.1f); + msg.Float32_values.Add(7.1f); + msg.Float32_values.Add(8.1f); + + // float64_values + msg.Float64_values.Add(9.1); + msg.Float64_values.Add(10.1); + msg.Float64_values.Add(11.1); + + // int8_values + msg.Int8_values.Add(12); + msg.Int8_values.Add(13); + msg.Int8_values.Add(14); + + // uint8_values + msg.Uint8_values.Add(15); + msg.Uint8_values.Add(16); + msg.Uint8_values.Add(17); + + // int16_values + msg.Int16_values.Add(18); + msg.Int16_values.Add(19); + msg.Int16_values.Add(20); + + // uint16_values + msg.Uint16_values.Add(21); + msg.Uint16_values.Add(22); + msg.Uint16_values.Add(23); + + // int32_values + msg.Int32_values.Add(24); + msg.Int32_values.Add(25); + msg.Int32_values.Add(26); + + // uint32_values + msg.Uint32_values.Add(27); + msg.Uint32_values.Add(28); + msg.Uint32_values.Add(29); + + // int64_values + msg.Int64_values.Add(30); + msg.Int64_values.Add(31); + msg.Int64_values.Add(32); + + // uint64_values + msg.Uint64_values.Add(33); + msg.Uint64_values.Add(34); + msg.Uint64_values.Add(35); + + // string_values + msg.String_values.Add("one"); + msg.String_values.Add("two"); + msg.String_values.Add("three"); + + test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); + basic_type_1.Bool_value = true; + basic_type_1.Byte_value = 36; + basic_type_1.Char_value = 37; + basic_type_1.Float32_value = 38.1f; + basic_type_1.Float64_value = 39.1; + basic_type_1.Int8_value = 40; + basic_type_1.Uint8_value = 41; + basic_type_1.Int16_value = 42; + basic_type_1.Uint16_value = 43; + basic_type_1.Int32_value = 44; + basic_type_1.Uint32_value = 45; + basic_type_1.Int64_value = 46; + basic_type_1.Uint64_value = 47; + + test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); + basic_type_2.Bool_value = false; + basic_type_2.Byte_value = 48; + basic_type_2.Char_value = 49; + basic_type_2.Float32_value = 50.1f; + basic_type_2.Float64_value = 51.1; + basic_type_2.Int8_value = 52; + basic_type_2.Uint8_value = 53; + basic_type_2.Int16_value = 54; + basic_type_2.Uint16_value = 55; + basic_type_2.Int32_value = 56; + basic_type_2.Uint32_value = 57; + basic_type_2.Int64_value = 58; + basic_type_2.Uint64_value = 59; + + test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); + basic_type_3.Bool_value = true; + basic_type_3.Byte_value = 60; + basic_type_3.Char_value = 61; + basic_type_3.Float32_value = 62.1f; + basic_type_3.Float64_value = 63.1; + basic_type_3.Int8_value = 64; + basic_type_3.Uint8_value = 65; + basic_type_3.Int16_value = 66; + basic_type_3.Uint16_value = 67; + basic_type_3.Int32_value = 68; + basic_type_3.Uint32_value = 69; + basic_type_3.Int64_value = 70; + basic_type_3.Uint64_value = 71; + + msg.Basic_types_values.Add(basic_type_1); + msg.Basic_types_values.Add(basic_type_2); + msg.Basic_types_values.Add(basic_type_3); + + bool received=false; + ISubscription chatter_sub = node_array_2.CreateSubscription( + "topic_unbounded_sequences", rcv_msg => + { + received=true; + msg2 = rcv_msg; + } + ); + + while(!received) + { + chatter_pub.Publish(msg); + + RCLdotnet.SpinOnce(node_array_1, 500); + RCLdotnet.SpinOnce(node_array_2, 500); + } + + // bool_values + Assert.Equal(3, msg2.Bool_values.Count); + Assert.True(msg2.Bool_values[0]); + Assert.False(msg2.Bool_values[1]); + Assert.True(msg2.Bool_values[2]); + + // byte_values + Assert.Equal(3, msg2.Byte_values.Count); + Assert.Equal(0, msg2.Byte_values[0]); + Assert.Equal(1, msg2.Byte_values[1]); + Assert.Equal(2, msg2.Byte_values[2]); + + // char_values + Assert.Equal(3, msg2.Char_values.Count); + Assert.Equal(3, msg2.Char_values[0]); + Assert.Equal(4, msg2.Char_values[1]); + Assert.Equal(5, msg2.Char_values[2]); + + // float32_values + Assert.Equal(3, msg2.Float32_values.Count); + Assert.Equal(6.1f, msg2.Float32_values[0]); + Assert.Equal(7.1f, msg2.Float32_values[1]); + Assert.Equal(8.1f, msg2.Float32_values[2]); + + // float64_values + Assert.Equal(3, msg2.Float64_values.Count); + Assert.Equal(9.1, msg2.Float64_values[0]); + Assert.Equal(10.1, msg2.Float64_values[1]); + Assert.Equal(11.1, msg2.Float64_values[2]); + + // int8_values + Assert.Equal(3, msg2.Int8_values.Count); + Assert.Equal(12, msg2.Int8_values[0]); + Assert.Equal(13, msg2.Int8_values[1]); + Assert.Equal(14, msg2.Int8_values[2]); + + // uint8_values + Assert.Equal(3, msg2.Uint8_values.Count); + Assert.Equal(15, msg2.Uint8_values[0]); + Assert.Equal(16, msg2.Uint8_values[1]); + Assert.Equal(17, msg2.Uint8_values[2]); + + // int16_values + Assert.Equal(3, msg2.Int16_values.Count); + Assert.Equal(18, msg2.Int16_values[0]); + Assert.Equal(19, msg2.Int16_values[1]); + Assert.Equal(20, msg2.Int16_values[2]); + + // uint16_values + Assert.Equal(3, msg2.Uint16_values.Count); + Assert.Equal(21, msg2.Uint16_values[0]); + Assert.Equal(22, msg2.Uint16_values[1]); + Assert.Equal(23, msg2.Uint16_values[2]); + + // int32_values + Assert.Equal(3, msg2.Int32_values.Count); + Assert.Equal(24, msg2.Int32_values[0]); + Assert.Equal(25, msg2.Int32_values[1]); + Assert.Equal(26, msg2.Int32_values[2]); + + // uint32_values + Assert.Equal(3, msg2.Uint32_values.Count); + Assert.Equal((uint)27, msg2.Uint32_values[0]); + Assert.Equal((uint)28, msg2.Uint32_values[1]); + Assert.Equal((uint)29, msg2.Uint32_values[2]); + + // int64_values + Assert.Equal(3, msg2.Int64_values.Count); + Assert.Equal(30, msg2.Int64_values[0]); + Assert.Equal(31, msg2.Int64_values[1]); + Assert.Equal(32, msg2.Int64_values[2]); + + // uint64_values + Assert.Equal(3, msg2.Uint64_values.Count); + Assert.Equal((ulong)33, msg2.Uint64_values[0]); + Assert.Equal((ulong)34, msg2.Uint64_values[1]); + Assert.Equal((ulong)35, msg2.Uint64_values[2]); + + // string_values + Assert.Equal(3, msg2.String_values.Count); + Assert.Equal("one", msg2.String_values[0]); + Assert.Equal("two", msg2.String_values[1]); + Assert.Equal("three", msg2.String_values[2]); + + Assert.Equal(3, msg2.Basic_types_values.Count); + Assert.True(msg2.Basic_types_values[0].Bool_value); + Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); + Assert.Equal(37, msg2.Basic_types_values[0].Char_value); + Assert.Equal(38.1f, msg2.Basic_types_values[0].Float32_value); + Assert.Equal(39.1, msg2.Basic_types_values[0].Float64_value); + Assert.Equal(40, msg2.Basic_types_values[0].Int8_value); + Assert.Equal(41, msg2.Basic_types_values[0].Uint8_value); + Assert.Equal(42, msg2.Basic_types_values[0].Int16_value); + Assert.Equal(43, msg2.Basic_types_values[0].Uint16_value); + Assert.Equal(44, msg2.Basic_types_values[0].Int32_value); + Assert.Equal((uint)45, msg2.Basic_types_values[0].Uint32_value); + Assert.Equal(46, msg2.Basic_types_values[0].Int64_value); + Assert.Equal((ulong)47, msg2.Basic_types_values[0].Uint64_value); + + Assert.False(msg2.Basic_types_values[1].Bool_value); + Assert.Equal(48, msg2.Basic_types_values[1].Byte_value); + Assert.Equal(49, msg2.Basic_types_values[1].Char_value); + Assert.Equal(50.1f, msg2.Basic_types_values[1].Float32_value); + Assert.Equal(51.1, msg2.Basic_types_values[1].Float64_value); + Assert.Equal(52, msg2.Basic_types_values[1].Int8_value); + Assert.Equal(53, msg2.Basic_types_values[1].Uint8_value); + Assert.Equal(54, msg2.Basic_types_values[1].Int16_value); + Assert.Equal(55, msg2.Basic_types_values[1].Uint16_value); + Assert.Equal(56, msg2.Basic_types_values[1].Int32_value); + Assert.Equal((uint)57, msg2.Basic_types_values[1].Uint32_value); + Assert.Equal(58, msg2.Basic_types_values[1].Int64_value); + Assert.Equal((ulong)59, msg2.Basic_types_values[1].Uint64_value); + + Assert.True(msg2.Basic_types_values[2].Bool_value); + Assert.Equal(60, msg2.Basic_types_values[2].Byte_value); + Assert.Equal(61, msg2.Basic_types_values[2].Char_value); + Assert.Equal(62.1f, msg2.Basic_types_values[2].Float32_value); + Assert.Equal(63.1, msg2.Basic_types_values[2].Float64_value); + Assert.Equal(64, msg2.Basic_types_values[2].Int8_value); + Assert.Equal(65, msg2.Basic_types_values[2].Uint8_value); + Assert.Equal(66, msg2.Basic_types_values[2].Int16_value); + Assert.Equal(67, msg2.Basic_types_values[2].Uint16_value); + Assert.Equal(68, msg2.Basic_types_values[2].Int32_value); + Assert.Equal((uint)69, msg2.Basic_types_values[2].Uint32_value); + Assert.Equal(70, msg2.Basic_types_values[2].Int64_value); + Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); + } + + [Fact] + public void TestPublishBoundedSequences() + { + RCLdotnet.Init(); + INode node_array_1 = RCLdotnet.CreateNode("test_bounded_sequences_1"); + INode node_array_2 = RCLdotnet.CreateNode("test_bounded_sequences_2"); + IPublisher chatter_pub = node_array_1.CreatePublisher("topic_bounded_sequences"); + + test_msgs.msg.BoundedSequences msg = new test_msgs.msg.BoundedSequences(); + test_msgs.msg.BoundedSequences msg2 = new test_msgs.msg.BoundedSequences(); + + // bool_values + msg.Bool_values.Add(true); + msg.Bool_values.Add(false); + msg.Bool_values.Add(true); + + // byte_values + msg.Byte_values.Add(0); + msg.Byte_values.Add(1); + msg.Byte_values.Add(2); + + // char_values + msg.Char_values.Add(3); + msg.Char_values.Add(4); + msg.Char_values.Add(5); + + // float32_values + msg.Float32_values.Add(6.1f); + msg.Float32_values.Add(7.1f); + msg.Float32_values.Add(8.1f); + + // float64_values + msg.Float64_values.Add(9.1); + msg.Float64_values.Add(10.1); + msg.Float64_values.Add(11.1); + + // int8_values + msg.Int8_values.Add(12); + msg.Int8_values.Add(13); + msg.Int8_values.Add(14); + + // uint8_values + msg.Uint8_values.Add(15); + msg.Uint8_values.Add(16); + msg.Uint8_values.Add(17); + + // int16_values + msg.Int16_values.Add(18); + msg.Int16_values.Add(19); + msg.Int16_values.Add(20); + + // uint16_values + msg.Uint16_values.Add(21); + msg.Uint16_values.Add(22); + msg.Uint16_values.Add(23); + + // int32_values + msg.Int32_values.Add(24); + msg.Int32_values.Add(25); + msg.Int32_values.Add(26); + + // uint32_values + msg.Uint32_values.Add(27); + msg.Uint32_values.Add(28); + msg.Uint32_values.Add(29); + + // int64_values + msg.Int64_values.Add(30); + msg.Int64_values.Add(31); + msg.Int64_values.Add(32); + + // uint64_values + msg.Uint64_values.Add(33); + msg.Uint64_values.Add(34); + msg.Uint64_values.Add(35); + + // string_values + msg.String_values.Add("one"); + msg.String_values.Add("two"); + msg.String_values.Add("three"); + + test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); + basic_type_1.Bool_value = true; + basic_type_1.Byte_value = 36; + basic_type_1.Char_value = 37; + basic_type_1.Float32_value = 38.1f; + basic_type_1.Float64_value = 39.1; + basic_type_1.Int8_value = 40; + basic_type_1.Uint8_value = 41; + basic_type_1.Int16_value = 42; + basic_type_1.Uint16_value = 43; + basic_type_1.Int32_value = 44; + basic_type_1.Uint32_value = 45; + basic_type_1.Int64_value = 46; + basic_type_1.Uint64_value = 47; + + test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); + basic_type_2.Bool_value = false; + basic_type_2.Byte_value = 48; + basic_type_2.Char_value = 49; + basic_type_2.Float32_value = 50.1f; + basic_type_2.Float64_value = 51.1; + basic_type_2.Int8_value = 52; + basic_type_2.Uint8_value = 53; + basic_type_2.Int16_value = 54; + basic_type_2.Uint16_value = 55; + basic_type_2.Int32_value = 56; + basic_type_2.Uint32_value = 57; + basic_type_2.Int64_value = 58; + basic_type_2.Uint64_value = 59; + + test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); + basic_type_3.Bool_value = true; + basic_type_3.Byte_value = 60; + basic_type_3.Char_value = 61; + basic_type_3.Float32_value = 62.1f; + basic_type_3.Float64_value = 63.1; + basic_type_3.Int8_value = 64; + basic_type_3.Uint8_value = 65; + basic_type_3.Int16_value = 66; + basic_type_3.Uint16_value = 67; + basic_type_3.Int32_value = 68; + basic_type_3.Uint32_value = 69; + basic_type_3.Int64_value = 70; + basic_type_3.Uint64_value = 71; + + msg.Basic_types_values.Add(basic_type_1); + msg.Basic_types_values.Add(basic_type_2); + msg.Basic_types_values.Add(basic_type_3); + + bool received=false; + ISubscription chatter_sub = node_array_2.CreateSubscription( + "topic_bounded_sequences", rcv_msg => + { + received=true; + msg2 = rcv_msg; + } + ); + + while(!received) + { + chatter_pub.Publish(msg); + + RCLdotnet.SpinOnce(node_array_1, 500); + RCLdotnet.SpinOnce(node_array_2, 500); + } + + // bool_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Bool_values_MaxCount); + Assert.Equal(3, msg2.Bool_values.Count); + Assert.True(msg2.Bool_values[0]); + Assert.False(msg2.Bool_values[1]); + Assert.True(msg2.Bool_values[2]); + + // byte_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Byte_values_MaxCount); + Assert.Equal(3, msg2.Byte_values.Count); + Assert.Equal(0, msg2.Byte_values[0]); + Assert.Equal(1, msg2.Byte_values[1]); + Assert.Equal(2, msg2.Byte_values[2]); + + // char_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Char_values_MaxCount); + Assert.Equal(3, msg2.Char_values.Count); + Assert.Equal(3, msg2.Char_values[0]); + Assert.Equal(4, msg2.Char_values[1]); + Assert.Equal(5, msg2.Char_values[2]); + + // float32_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Float32_values_MaxCount); + Assert.Equal(3, msg2.Float32_values.Count); + Assert.Equal(6.1f, msg2.Float32_values[0]); + Assert.Equal(7.1f, msg2.Float32_values[1]); + Assert.Equal(8.1f, msg2.Float32_values[2]); + + // float64_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Float64_values_MaxCount); + Assert.Equal(3, msg2.Float64_values.Count); + Assert.Equal(9.1, msg2.Float64_values[0]); + Assert.Equal(10.1, msg2.Float64_values[1]); + Assert.Equal(11.1, msg2.Float64_values[2]); + + // int8_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int8_values_MaxCount); + Assert.Equal(3, msg2.Int8_values.Count); + Assert.Equal(12, msg2.Int8_values[0]); + Assert.Equal(13, msg2.Int8_values[1]); + Assert.Equal(14, msg2.Int8_values[2]); + + // uint8_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint8_values_MaxCount); + Assert.Equal(3, msg2.Uint8_values.Count); + Assert.Equal(15, msg2.Uint8_values[0]); + Assert.Equal(16, msg2.Uint8_values[1]); + Assert.Equal(17, msg2.Uint8_values[2]); + + // int16_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int16_values_MaxCount); + Assert.Equal(3, msg2.Int16_values.Count); + Assert.Equal(18, msg2.Int16_values[0]); + Assert.Equal(19, msg2.Int16_values[1]); + Assert.Equal(20, msg2.Int16_values[2]); + + // uint16_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint16_values_MaxCount); + Assert.Equal(3, msg2.Uint16_values.Count); + Assert.Equal(21, msg2.Uint16_values[0]); + Assert.Equal(22, msg2.Uint16_values[1]); + Assert.Equal(23, msg2.Uint16_values[2]); + + // int32_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int32_values_MaxCount); + Assert.Equal(3, msg2.Int32_values.Count); + Assert.Equal(24, msg2.Int32_values[0]); + Assert.Equal(25, msg2.Int32_values[1]); + Assert.Equal(26, msg2.Int32_values[2]); + + // uint32_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint32_values_MaxCount); + Assert.Equal(3, msg2.Uint32_values.Count); + Assert.Equal((uint)27, msg2.Uint32_values[0]); + Assert.Equal((uint)28, msg2.Uint32_values[1]); + Assert.Equal((uint)29, msg2.Uint32_values[2]); + + // int64_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int64_values_MaxCount); + Assert.Equal(3, msg2.Int64_values.Count); + Assert.Equal(30, msg2.Int64_values[0]); + Assert.Equal(31, msg2.Int64_values[1]); + Assert.Equal(32, msg2.Int64_values[2]); + + // uint64_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint64_values_MaxCount); + Assert.Equal(3, msg2.Uint64_values.Count); + Assert.Equal((ulong)33, msg2.Uint64_values[0]); + Assert.Equal((ulong)34, msg2.Uint64_values[1]); + Assert.Equal((ulong)35, msg2.Uint64_values[2]); + + // string_values + Assert.Equal(3, test_msgs.msg.BoundedSequences.String_values_MaxCount); + Assert.Equal(3, msg2.String_values.Count); + Assert.Equal("one", msg2.String_values[0]); + Assert.Equal("two", msg2.String_values[1]); + Assert.Equal("three", msg2.String_values[2]); + + Assert.Equal(3, test_msgs.msg.BoundedSequences.Basic_types_values_MaxCount); + Assert.Equal(3, msg2.Basic_types_values.Count); + Assert.True(msg2.Basic_types_values[0].Bool_value); + Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); + Assert.Equal(37, msg2.Basic_types_values[0].Char_value); + Assert.Equal(38.1f, msg2.Basic_types_values[0].Float32_value); + Assert.Equal(39.1, msg2.Basic_types_values[0].Float64_value); + Assert.Equal(40, msg2.Basic_types_values[0].Int8_value); + Assert.Equal(41, msg2.Basic_types_values[0].Uint8_value); + Assert.Equal(42, msg2.Basic_types_values[0].Int16_value); + Assert.Equal(43, msg2.Basic_types_values[0].Uint16_value); + Assert.Equal(44, msg2.Basic_types_values[0].Int32_value); + Assert.Equal((uint)45, msg2.Basic_types_values[0].Uint32_value); + Assert.Equal(46, msg2.Basic_types_values[0].Int64_value); + Assert.Equal((ulong)47, msg2.Basic_types_values[0].Uint64_value); + + Assert.False(msg2.Basic_types_values[1].Bool_value); + Assert.Equal(48, msg2.Basic_types_values[1].Byte_value); + Assert.Equal(49, msg2.Basic_types_values[1].Char_value); + Assert.Equal(50.1f, msg2.Basic_types_values[1].Float32_value); + Assert.Equal(51.1, msg2.Basic_types_values[1].Float64_value); + Assert.Equal(52, msg2.Basic_types_values[1].Int8_value); + Assert.Equal(53, msg2.Basic_types_values[1].Uint8_value); + Assert.Equal(54, msg2.Basic_types_values[1].Int16_value); + Assert.Equal(55, msg2.Basic_types_values[1].Uint16_value); + Assert.Equal(56, msg2.Basic_types_values[1].Int32_value); + Assert.Equal((uint)57, msg2.Basic_types_values[1].Uint32_value); + Assert.Equal(58, msg2.Basic_types_values[1].Int64_value); + Assert.Equal((ulong)59, msg2.Basic_types_values[1].Uint64_value); + + Assert.True(msg2.Basic_types_values[2].Bool_value); + Assert.Equal(60, msg2.Basic_types_values[2].Byte_value); + Assert.Equal(61, msg2.Basic_types_values[2].Char_value); + Assert.Equal(62.1f, msg2.Basic_types_values[2].Float32_value); + Assert.Equal(63.1, msg2.Basic_types_values[2].Float64_value); + Assert.Equal(64, msg2.Basic_types_values[2].Int8_value); + Assert.Equal(65, msg2.Basic_types_values[2].Uint8_value); + Assert.Equal(66, msg2.Basic_types_values[2].Int16_value); + Assert.Equal(67, msg2.Basic_types_values[2].Uint16_value); + Assert.Equal(68, msg2.Basic_types_values[2].Int32_value); + Assert.Equal((uint)69, msg2.Basic_types_values[2].Uint32_value); + Assert.Equal(70, msg2.Basic_types_values[2].Int64_value); + Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); + } } -} \ No newline at end of file +} From fb9a25fc4bcc81672c945fed8f4fea724d5028cc Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 26 Aug 2021 15:55:26 +0200 Subject: [PATCH 08/73] Initialize arrays and change mapped type Idl arrays now map as System.Array instead of List. This is the more natural C# type for them. --- rcldotnet/test/test_messages.cs | 155 +++++++++++---------- rosidl_generator_dotnet/resource/msg.cs.em | 67 ++++++--- 2 files changed, 127 insertions(+), 95 deletions(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index 71cbbcf4..b73d4f58 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -94,75 +94,76 @@ public void TestPublishArrays() test_msgs.msg.Arrays msg2 = new test_msgs.msg.Arrays (); // bool_values - msg.Bool_values.Add(true); - msg.Bool_values.Add(false); - msg.Bool_values.Add(true); + msg.Bool_values[0] = true; + msg.Bool_values[1] = false; + msg.Bool_values[2] = true; // byte_values - msg.Byte_values.Add(0); - msg.Byte_values.Add(1); - msg.Byte_values.Add(2); + msg.Byte_values[0] = 0; + msg.Byte_values[1] = 1; + msg.Byte_values[2] = 2; // char_values - msg.Char_values.Add(3); - msg.Char_values.Add(4); - msg.Char_values.Add(5); + msg.Char_values[0] = 3; + msg.Char_values[1] = 4; + msg.Char_values[2] = 5; // float32_values - msg.Float32_values.Add(6.1f); - msg.Float32_values.Add(7.1f); - msg.Float32_values.Add(8.1f); + msg.Float32_values[0] = 6.1f; + msg.Float32_values[1] = 7.1f; + msg.Float32_values[2] = 8.1f; // float64_values - msg.Float64_values.Add(9.1); - msg.Float64_values.Add(10.1); - msg.Float64_values.Add(11.1); + msg.Float64_values[0] = 9.1; + msg.Float64_values[1] = 10.1; + msg.Float64_values[2] = 11.1; // int8_values - msg.Int8_values.Add(12); - msg.Int8_values.Add(13); - msg.Int8_values.Add(14); + msg.Int8_values[0] = 12; + msg.Int8_values[1] = 13; + msg.Int8_values[2] = 14; // uint8_values - msg.Uint8_values.Add(15); - msg.Uint8_values.Add(16); - msg.Uint8_values.Add(17); + msg.Uint8_values[0] = 15; + msg.Uint8_values[1] = 16; + msg.Uint8_values[2] = 17; // int16_values - msg.Int16_values.Add(18); - msg.Int16_values.Add(19); - msg.Int16_values.Add(20); + msg.Int16_values[0] = 18; + msg.Int16_values[1] = 19; + msg.Int16_values[2] = 20; // uint16_values - msg.Uint16_values.Add(21); - msg.Uint16_values.Add(22); - msg.Uint16_values.Add(23); + msg.Uint16_values[0] = 21; + msg.Uint16_values[1] = 22; + msg.Uint16_values[2] = 23; // int32_values - msg.Int32_values.Add(24); - msg.Int32_values.Add(25); - msg.Int32_values.Add(26); + msg.Int32_values[0] = 24; + msg.Int32_values[1] = 25; + msg.Int32_values[2] = 26; // uint32_values - msg.Uint32_values.Add(27); - msg.Uint32_values.Add(28); - msg.Uint32_values.Add(29); + msg.Uint32_values[0] = 27; + msg.Uint32_values[1] = 28; + msg.Uint32_values[2] = 29; // int64_values - msg.Int64_values.Add(30); - msg.Int64_values.Add(31); - msg.Int64_values.Add(32); + msg.Int64_values[0] = 30; + msg.Int64_values[1] = 31; + msg.Int64_values[2] = 32; // uint64_values - msg.Uint64_values.Add(33); - msg.Uint64_values.Add(34); - msg.Uint64_values.Add(35); + msg.Uint64_values[0] = 33; + msg.Uint64_values[1] = 34; + msg.Uint64_values[2] = 35; // string_values - msg.String_values.Add("one"); - msg.String_values.Add("two"); - msg.String_values.Add("three"); + msg.String_values[0] = "one"; + msg.String_values[1] = "two"; + msg.String_values[2] = "three"; + // basic_types_values test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); basic_type_1.Bool_value = true; basic_type_1.Byte_value = 36; @@ -208,9 +209,9 @@ public void TestPublishArrays() basic_type_3.Int64_value = 70; basic_type_3.Uint64_value = 71; - msg.Basic_types_values.Add(basic_type_1); - msg.Basic_types_values.Add(basic_type_2); - msg.Basic_types_values.Add(basic_type_3); + msg.Basic_types_values[0] = basic_type_1; + msg.Basic_types_values[1] = basic_type_2; + msg.Basic_types_values[2] = basic_type_3; bool received=false; ISubscription chatter_sub = node_array_2.CreateSubscription ( @@ -230,105 +231,107 @@ public void TestPublishArrays() } // bool_values - Assert.Equal(3, test_msgs.msg.Arrays.Bool_values_Count); - Assert.Equal(3, msg2.Bool_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Bool_values_Length); + Assert.Equal(3, msg2.Bool_values.Length); Assert.True(msg2.Bool_values[0]); Assert.False(msg2.Bool_values[1]); Assert.True(msg2.Bool_values[2]); // byte_values - Assert.Equal(3, test_msgs.msg.Arrays.Byte_values_Count); - Assert.Equal(3, msg2.Byte_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Byte_values_Length); + Assert.Equal(3, msg2.Byte_values.Length); Assert.Equal(0, msg2.Byte_values[0]); Assert.Equal(1, msg2.Byte_values[1]); Assert.Equal(2, msg2.Byte_values[2]); // char_values - Assert.Equal(3, test_msgs.msg.Arrays.Char_values_Count); - Assert.Equal(3, msg2.Char_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Char_values_Length); + Assert.Equal(3, msg2.Char_values.Length); Assert.Equal(3, msg2.Char_values[0]); Assert.Equal(4, msg2.Char_values[1]); Assert.Equal(5, msg2.Char_values[2]); // float32_values - Assert.Equal(3, test_msgs.msg.Arrays.Float32_values_Count); - Assert.Equal(3, msg2.Float32_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Float32_values_Length); + Assert.Equal(3, msg2.Float32_values.Length); Assert.Equal(6.1f, msg2.Float32_values[0]); Assert.Equal(7.1f, msg2.Float32_values[1]); Assert.Equal(8.1f, msg2.Float32_values[2]); // float64_values - Assert.Equal(3, test_msgs.msg.Arrays.Float64_values_Count); - Assert.Equal(3, msg2.Float64_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Float64_values_Length); + Assert.Equal(3, msg2.Float64_values.Length); Assert.Equal(9.1, msg2.Float64_values[0]); Assert.Equal(10.1, msg2.Float64_values[1]); Assert.Equal(11.1, msg2.Float64_values[2]); // int8_values - Assert.Equal(3, test_msgs.msg.Arrays.Int8_values_Count); - Assert.Equal(3, msg2.Int8_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Int8_values_Length); + Assert.Equal(3, msg2.Int8_values.Length); Assert.Equal(12, msg2.Int8_values[0]); Assert.Equal(13, msg2.Int8_values[1]); Assert.Equal(14, msg2.Int8_values[2]); // uint8_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint8_values_Count); - Assert.Equal(3, msg2.Uint8_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Uint8_values_Length); + Assert.Equal(3, msg2.Uint8_values.Length); Assert.Equal(15, msg2.Uint8_values[0]); Assert.Equal(16, msg2.Uint8_values[1]); Assert.Equal(17, msg2.Uint8_values[2]); // int16_values - Assert.Equal(3, test_msgs.msg.Arrays.Int16_values_Count); - Assert.Equal(3, msg2.Int16_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Int16_values_Length); + Assert.Equal(3, msg2.Int16_values.Length); Assert.Equal(18, msg2.Int16_values[0]); Assert.Equal(19, msg2.Int16_values[1]); Assert.Equal(20, msg2.Int16_values[2]); // uint16_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint16_values_Count); - Assert.Equal(3, msg2.Uint16_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Uint16_values_Length); + Assert.Equal(3, msg2.Uint16_values.Length); Assert.Equal(21, msg2.Uint16_values[0]); Assert.Equal(22, msg2.Uint16_values[1]); Assert.Equal(23, msg2.Uint16_values[2]); // int32_values - Assert.Equal(3, test_msgs.msg.Arrays.Int32_values_Count); - Assert.Equal(3, msg2.Int32_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Int32_values_Length); + Assert.Equal(3, msg2.Int32_values.Length); Assert.Equal(24, msg2.Int32_values[0]); Assert.Equal(25, msg2.Int32_values[1]); Assert.Equal(26, msg2.Int32_values[2]); // uint32_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint32_values_Count); - Assert.Equal(3, msg2.Uint32_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Uint32_values_Length); + Assert.Equal(3, msg2.Uint32_values.Length); Assert.Equal((uint)27, msg2.Uint32_values[0]); Assert.Equal((uint)28, msg2.Uint32_values[1]); Assert.Equal((uint)29, msg2.Uint32_values[2]); // int64_values - Assert.Equal(3, test_msgs.msg.Arrays.Int64_values_Count); - Assert.Equal(3, msg2.Int64_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Int64_values_Length); + Assert.Equal(3, msg2.Int64_values.Length); Assert.Equal(30, msg2.Int64_values[0]); Assert.Equal(31, msg2.Int64_values[1]); Assert.Equal(32, msg2.Int64_values[2]); // uint64_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint64_values_Count); - Assert.Equal(3, msg2.Uint64_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.Uint64_values_Length); + Assert.Equal(3, msg2.Uint64_values.Length); Assert.Equal((ulong)33, msg2.Uint64_values[0]); Assert.Equal((ulong)34, msg2.Uint64_values[1]); Assert.Equal((ulong)35, msg2.Uint64_values[2]); // string_values - Assert.Equal(3, test_msgs.msg.Arrays.String_values_Count); - Assert.Equal(3, msg2.String_values.Count); + Assert.Equal(3, test_msgs.msg.Arrays.String_values_Length); + Assert.Equal(3, msg2.String_values.Length); Assert.Equal("one", msg2.String_values[0]); Assert.Equal("two", msg2.String_values[1]); Assert.Equal("three", msg2.String_values[2]); - - Assert.Equal(3, test_msgs.msg.Arrays.Basic_types_values_Count); - Assert.Equal(3, msg2.Basic_types_values.Count); + + // basic_types_values + Assert.Equal(3, test_msgs.msg.Arrays.Basic_types_values_Length); + Assert.Equal(3, msg2.Basic_types_values.Length); + Assert.True(msg2.Basic_types_values[0].Bool_value); Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); Assert.Equal(37, msg2.Basic_types_values[0].Char_value); diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 206fda07..c3a1f446 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -33,7 +33,7 @@ public class @(type_name) : IMessage { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ - public const int @(get_field_name(type_name, member.name))_Count = @(member.type.size); + public const int @(get_field_name(type_name, member.name))_Length = @(member.type.size); @[ elif isinstance(member.type, AbstractSequence) and member.type.has_maximum_size()]@ public const int @(get_field_name(type_name, member.name))_MaxCount = @(member.type.maximum_size); @[ end if]@ @@ -43,7 +43,22 @@ public class @(type_name) : IMessage { { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ - @(get_field_name(type_name, member.name)) = new List<@(get_dotnet_type(member.type.value_type))>(@(member.type.size)); + @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)]; +@[ if isinstance(member.type.value_type, AbstractString)]@ + for (var i__local_variable = 0; i__local_variable < @(member.type.size); i__local_variable++) + { + @(get_field_name(type_name, member.name))[i__local_variable] = ""; + } +@[ elif isinstance(member.type.value_type, BasicType)]@ +@# Basic types get initialized by the array constructor. +@[ elif isinstance(member.type.value_type, AbstractWString)]@ +// TODO: Unicode types are not supported +@[ else]@ + for (var i__local_variable = 0; i__local_variable < @(member.type.size); i__local_variable++) + { + @(get_field_name(type_name, member.name))[i__local_variable] = new @(get_dotnet_type(member.type.value_type))(); + } +@[ end if]@ @[ elif isinstance(member.type, AbstractSequence)]@ @(get_field_name(type_name, member.name)) = new List<@(get_dotnet_type(member.type.value_type))>(); @[ elif isinstance(member.type, AbstractWString)]@ @@ -221,31 +236,43 @@ public class @(type_name) : IMessage { public void _READ_HANDLE(IntPtr messageHandle) { @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ - +@[ if isinstance(member.type, Array)]@ { - @(get_field_name(type_name, member.name)).Clear(); -@[ if isinstance(member.type, AbstractSequence)]@ - int size__local_variable = native_getsize_field_@(member.name)_message(messageHandle); - for (int i = 0; i < size__local_variable; i++) + @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)]; + for (int i__local_variable = 0; i__local_variable < @(member.type.size); i__local_variable++) + { +@[ if isinstance(member.type.value_type, BasicType)]@ + @(get_field_name(type_name, member.name))[i__local_variable] = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); +@[ elif isinstance(member.type.value_type, AbstractString)]@ + IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); + @(get_field_name(type_name, member.name))[i__local_variable] = Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name))); +@[ elif isinstance(member.type.value_type, AbstractWString)]@ + // TODO: Unicode types are not supported @[ else]@ - for (int i = 0; i < @(member.type.size); i++) -@[ end if]@ + @(get_field_name(type_name, member.name))[i__local_variable] = new @(get_dotnet_type(member.type.value_type))(); + @(get_field_name(type_name, member.name))[i__local_variable]._READ_HANDLE(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); +@[ end if] + } + } +@[ elif isinstance(member.type, AbstractSequence)]@ + { + int size__local_variable = native_getsize_field_@(member.name)_message(messageHandle); + @(get_field_name(type_name, member.name)) = new List<@(get_dotnet_type(member.type.value_type))>(size__local_variable); + for (int i__local_variable = 0; i__local_variable < size__local_variable; i__local_variable++) { -@[ if isinstance(member.type.value_type, BasicType)] - @(get_field_name(type_name, member.name)).Add(native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i))); -@[ elif isinstance(member.type.value_type, AbstractString)] - IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i)); +@[ if isinstance(member.type.value_type, BasicType)]@ + @(get_field_name(type_name, member.name)).Add(native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable))); +@[ elif isinstance(member.type.value_type, AbstractString)]@ + IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @(get_field_name(type_name, member.name)).Add(Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name)))); - @[ elif isinstance(member.type.value_type, AbstractWString)] +@[ elif isinstance(member.type.value_type, AbstractWString)]@ // TODO: Unicode types are not supported - @[ else] +@[ else]@ @(get_field_name(type_name, member.name)).Add(new @(get_dotnet_type(member.type.value_type))()); - @(get_field_name(type_name, member.name))[@(get_field_name(type_name, member.name)).Count-1]._READ_HANDLE(native_get_field_@(member.name)_message(messageHandle, i)); + @(get_field_name(type_name, member.name))[i__local_variable]._READ_HANDLE(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if] } } - @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @@ -304,7 +331,9 @@ public class @(type_name) : IMessage { @[end for]@ @[for member in message.structure.members]@ -@[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ +@[ if isinstance(member.type, Array)]@ + public @(get_dotnet_type(member.type.value_type))[] @(get_field_name(type_name, member.name)) { get; set; } +@[ elif isinstance(member.type, AbstractSequence)]@ public List<@(get_dotnet_type(member.type.value_type))> @(get_field_name(type_name, member.name)) { get; set; } @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported From 922c24ddc0601b2d609f46b7550f882ccf1c453c Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 24 Aug 2021 09:54:12 +0200 Subject: [PATCH 09/73] Add size checks for array and bounded sequences - size gets checked on writing to the native handle of the message. - change foreach to for to avoid multi threaded race conditions while writing to native memory. (the upper bound is now local and cannot be changed from somewhere else) --- rcldotnet/test/test_messages.cs | 77 +++++++++++++++++++--- rosidl_generator_dotnet/resource/msg.cs.em | 25 +++++-- 2 files changed, 87 insertions(+), 15 deletions(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index b73d4f58..f486940c 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -1,4 +1,5 @@ using System; +using System.Collections.Generic; using System.Reflection; using System.Runtime; using System.Runtime.InteropServices; @@ -217,17 +218,17 @@ public void TestPublishArrays() ISubscription chatter_sub = node_array_2.CreateSubscription ( "topic_array", rcv_msg => { - received=true; - msg2 = rcv_msg; + received = true; + msg2 = rcv_msg; } ); - while(!received) + while (!received) { - chatter_pub.Publish (msg); + chatter_pub.Publish(msg); - RCLdotnet.SpinOnce(node_array_1, 500); - RCLdotnet.SpinOnce(node_array_2, 500); + RCLdotnet.SpinOnce(node_array_1, 500); + RCLdotnet.SpinOnce(node_array_2, 500); } // bool_values @@ -299,7 +300,7 @@ public void TestPublishArrays() Assert.Equal(24, msg2.Int32_values[0]); Assert.Equal(25, msg2.Int32_values[1]); Assert.Equal(26, msg2.Int32_values[2]); - + // uint32_values Assert.Equal(3, test_msgs.msg.Arrays.Uint32_values_Length); Assert.Equal(3, msg2.Uint32_values.Length); @@ -313,7 +314,7 @@ public void TestPublishArrays() Assert.Equal(30, msg2.Int64_values[0]); Assert.Equal(31, msg2.Int64_values[1]); Assert.Equal(32, msg2.Int64_values[2]); - + // uint64_values Assert.Equal(3, test_msgs.msg.Arrays.Uint64_values_Length); Assert.Equal(3, msg2.Uint64_values.Length); @@ -359,7 +360,7 @@ public void TestPublishArrays() Assert.Equal((uint)57, msg2.Basic_types_values[1].Uint32_value); Assert.Equal(58, msg2.Basic_types_values[1].Int64_value); Assert.Equal((ulong)59, msg2.Basic_types_values[1].Uint64_value); - + Assert.True(msg2.Basic_types_values[2].Bool_value); Assert.Equal(60, msg2.Basic_types_values[2].Byte_value); Assert.Equal(61, msg2.Basic_types_values[2].Char_value); @@ -375,6 +376,44 @@ public void TestPublishArrays() Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); } + [Fact] + public void TestPublishArraysSizeCheckToLittle() + { + RCLdotnet.Init(); + var nodeArraysSizeCheck = RCLdotnet.CreateNode("test_arrays_size_check_to_little"); + var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_little"); + + var msg = new test_msgs.msg.Arrays(); + msg.Bool_values = new bool[] + { + false, + true, + }; + + var exception = Assert.Throws(() => chatterPub.Publish(msg)); + Assert.Equal("Invalid size of array 'Bool_values'.", exception.Message); + } + + [Fact] + public void TestPublishArraysSizeCheckToMuch() + { + RCLdotnet.Init(); + var nodeArraysSizeCheck = RCLdotnet.CreateNode("test_arrays_size_check_to_much"); + var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_much"); + + var msg = new test_msgs.msg.Arrays(); + msg.String_values = new string[] + { + "0", + "1", + "2", + "3", + }; + + var exception = Assert.Throws(() => chatterPub.Publish(msg)); + Assert.Equal("Invalid size of array 'String_values'.", exception.Message); + } + [Fact] public void TestPublishUnboundedSequences() { @@ -939,5 +978,25 @@ public void TestPublishBoundedSequences() Assert.Equal(70, msg2.Basic_types_values[2].Int64_value); Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); } + + [Fact] + public void TestPublishBoundedSequencesSizeCheck() + { + RCLdotnet.Init(); + var nodeBoundedSequencesSizeCheck = RCLdotnet.CreateNode("test_bounded_sequences_size_check"); + var chatterPub = nodeBoundedSequencesSizeCheck.CreatePublisher("topic_bounded_sequences_size_check"); + + var msg = new test_msgs.msg.BoundedSequences(); + msg.String_values = new List + { + "0", + "1", + "2", + "3", + }; + + var exception = Assert.Throws(() => chatterPub.Publish(msg)); + Assert.Equal("Invalid size of bounded sequence 'String_values'.", exception.Message); + } } } diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index c3a1f446..77c4bd2f 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -292,21 +292,34 @@ public class @(type_name) : IMessage { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ { - int count__local_variable = 0; -@[ if isinstance(member.type, AbstractSequence)]@ - if (!native_init_seqence_field_@(member.name)_message(messageHandle, @(get_field_name(type_name, member.name)).Count)) +@[ if isinstance(member.type, Array)]@ + var count__local_variable = @(get_field_name(type_name, member.name)).Length; + if (count__local_variable != @(member.type.size)) + { + throw new Exception("Invalid size of array '@(get_field_name(type_name, member.name))'."); + } +@[ elif isinstance(member.type, AbstractSequence)]@ + var count__local_variable = @(get_field_name(type_name, member.name)).Count; +@[ if member.type.has_maximum_size()]@ + if (count__local_variable > @(member.type.maximum_size)) + { + throw new Exception("Invalid size of bounded sequence '@(get_field_name(type_name, member.name))'."); + } +@[ end if]@ + if (!native_init_seqence_field_@(member.name)_message(messageHandle, count__local_variable)) { throw new Exception("The method 'native_init_seqence_field_@(member.name)_message()' failed."); } @[ end if]@ - foreach(@(get_dotnet_type(member.type.value_type)) value in @(get_field_name(type_name, member.name))) + for (var i__local_variable = 0; i__local_variable < count__local_variable; i__local_variable++) { + var value__local_variable = @(get_field_name(type_name, member.name))[i__local_variable]; @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ - native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, count__local_variable++), value); + native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable), value__local_variable); @[ elif isinstance(member.type.value_type, AbstractWString)] // TODO: Unicode types are not supported @[ else]@ - value._WRITE_HANDLE(native_get_field_@(member.name)_message(messageHandle, count__local_variable++)); + value__local_variable._WRITE_HANDLE(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if]@ } } From 7dc4f12c46e52ec558e98615ba2e3ace106feb5b Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 24 Aug 2021 10:43:12 +0200 Subject: [PATCH 10/73] Use `int32_t` as size type to the c# side `List.Count` is of c# type `int`, wich is 32 bit signed. C does use `sizeof_t` wich is 32/64 bit dependent. So this does marshal at least correctly, ignoring cast from 64 bit to 32 bit and vice versa for now. getting marshaling/conversations 100% correct is anoher issue for another day. --- rosidl_generator_dotnet/resource/msg.c.em | 6 +++--- rosidl_generator_dotnet/resource/msg.h.em | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rosidl_generator_dotnet/resource/msg.c.em b/rosidl_generator_dotnet/resource/msg.c.em index 34ff7439..fc0d4c82 100644 --- a/rosidl_generator_dotnet/resource/msg.c.em +++ b/rosidl_generator_dotnet/resource/msg.c.em @@ -50,7 +50,7 @@ const void * @(msg_typename)__get_typesupport() { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ -void * @(msg_typename)__get_field_@(member.name)_message(void *message_handle, int index) { +void * @(msg_typename)__get_field_@(member.name)_message(void *message_handle, int32_t index) { @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; @[ if isinstance(member.type, Array)]@ return &(ros_message->@(member.name)[index]); @@ -60,12 +60,12 @@ void * @(msg_typename)__get_field_@(member.name)_message(void *message_handle, i } @[ if isinstance(member.type, AbstractSequence)]@ -int @(msg_typename)__getsize_field_@(member.name)_message(void *message_handle) { +int32_t @(msg_typename)__getsize_field_@(member.name)_message(void *message_handle) { @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; return ros_message->@(member.name).size; } -bool @(msg_typename)__init_sequence_field_@(member.name)_message(void *message_handle, int size) { +bool @(msg_typename)__init_sequence_field_@(member.name)_message(void *message_handle, int32_t size) { @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; rcutils_allocator_t allocator = rcutils_get_default_allocator(); diff --git a/rosidl_generator_dotnet/resource/msg.h.em b/rosidl_generator_dotnet/resource/msg.h.em index 39df84bd..0b7f0243 100644 --- a/rosidl_generator_dotnet/resource/msg.h.em +++ b/rosidl_generator_dotnet/resource/msg.h.em @@ -47,14 +47,14 @@ void @(msg_prefix)_CDECL @(msg_typename)__destroy_native_message(void *); @[for member in message.structure.members]@ @[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ @(msg_prefix)_EXPORT -void * @(msg_prefix)_CDECL @(msg_typename)__get_field_@(member.name)_message(void *, int); +void * @(msg_prefix)_CDECL @(msg_typename)__get_field_@(member.name)_message(void *, int32_t); @[ if isinstance(member.type, AbstractSequence)]@ @(msg_prefix)_EXPORT -int @(msg_prefix)_CDECL @(msg_typename)__getsize_field_@(member.name)_message(void *); +int32_t @(msg_prefix)_CDECL @(msg_typename)__getsize_field_@(member.name)_message(void *); @(msg_prefix)_EXPORT -bool @(msg_prefix)_CDECL @(msg_typename)__init_sequence_field_@(member.name)_message(void *, int); +bool @(msg_prefix)_CDECL @(msg_typename)__init_sequence_field_@(member.name)_message(void *, int32_t); @[ end if]@ @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ From ebbd9657745b76f9649021c78f939cf4e29be063 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 25 Aug 2021 12:24:52 +0200 Subject: [PATCH 11/73] Generate service request/response types --- ...generator_dotnet_generate_interfaces.cmake | 11 +++++ rosidl_generator_dotnet/resource/idl.c.em | 48 ++++++++++++++++++- rosidl_generator_dotnet/resource/idl.cs.em | 26 ++++++++-- rosidl_generator_dotnet/resource/idl.h.em | 17 ++++++- rosidl_generator_dotnet/resource/msg.c.em | 23 +-------- rosidl_generator_dotnet/resource/msg.cs.em | 15 +----- 6 files changed, 99 insertions(+), 41 deletions(-) diff --git a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake index d2cd0ac5..617654ce 100644 --- a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake +++ b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake @@ -56,6 +56,17 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_module_name}.c" ) elseif(_parent_folder STREQUAL "srv") + list(APPEND _generated_cs_files + "${_output_path}/${_parent_folder}/${_module_name}.cs" + ) + + list(APPEND _generated_h_files + "${_output_path}/${_parent_folder}/rcldotnet_${_module_name}.h" + ) + + list(APPEND _generated_c_ts_files + "${_output_path}/${_parent_folder}/${_module_name}.c" + ) elseif(_parent_folder STREQUAL "action") else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") diff --git a/rosidl_generator_dotnet/resource/idl.c.em b/rosidl_generator_dotnet/resource/idl.c.em index 62973993..2b34d90c 100644 --- a/rosidl_generator_dotnet/resource/idl.c.em +++ b/rosidl_generator_dotnet/resource/idl.c.em @@ -10,7 +10,36 @@ @# - interface_path (Path relative to the directory named after the package) @# - content (IdlContent, list of elements, e.g. Messages or Services) @####################################################################### -@ +@{ +from rosidl_cmake import convert_camel_case_to_lower_case_underscore + +c_header_parts = [package_name] + list(interface_path.parents[0].parts) + [ + convert_camel_case_to_lower_case_underscore(interface_path.stem) + ".h"] +c_header_filename = '/'.join(c_header_parts) + +dotnet_header_parts = [package_name] + list(interface_path.parents[0].parts) + [ + "rcldotnet_" + convert_camel_case_to_lower_case_underscore(interface_path.stem) + ".h"] +dotnet_header_filename = '/'.join(dotnet_header_parts) + +}@ +#include +#include +#include +#include + +#include + +#include "@(c_header_filename)" +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include +#include + +#include +#include + +#include "@(dotnet_header_filename)" + @ @####################################################################### @# Handle messages @@ -30,7 +59,22 @@ TEMPLATE( @# Handle services @####################################################################### @ -@#TODO - services not implemented +@{ +from rosidl_parser.definition import Service +}@ +@[for service in content.get_elements_of_type(Service)]@ +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=service.response_message) +}@ +@[end for]@ @ @ @####################################################################### diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index df5b3cbc..b8b7f15a 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -10,8 +10,13 @@ @# - interface_path (Path relative to the directory named after the package) @# - content (IdlContent, list of elements, e.g. Messages or Services) @####################################################################### -@ -@ +using System; +using System.Runtime.InteropServices; +using System.Collections.Generic; + +using ROS2.Interfaces; +using ROS2.Utils; + @####################################################################### @# Handle messages @####################################################################### @@ -30,7 +35,22 @@ TEMPLATE( @# Handle services @####################################################################### @ -@#TODO - services not implemented +@{ +from rosidl_parser.definition import Service +}@ +@[for service in content.get_elements_of_type(Service)]@ +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=service.response_message) +}@ +@[end for]@ @ @ @####################################################################### diff --git a/rosidl_generator_dotnet/resource/idl.h.em b/rosidl_generator_dotnet/resource/idl.h.em index 380add05..179ca24c 100644 --- a/rosidl_generator_dotnet/resource/idl.h.em +++ b/rosidl_generator_dotnet/resource/idl.h.em @@ -30,7 +30,22 @@ TEMPLATE( @# Handle services @####################################################################### @ -@# TODO - services not implemented +@{ +from rosidl_parser.definition import Service +}@ +@[for service in content.get_elements_of_type(Service)]@ +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=service.response_message) +}@ +@[end for]@ @ @ @####################################################################### diff --git a/rosidl_generator_dotnet/resource/msg.c.em b/rosidl_generator_dotnet/resource/msg.c.em index fc0d4c82..aeb451f9 100644 --- a/rosidl_generator_dotnet/resource/msg.c.em +++ b/rosidl_generator_dotnet/resource/msg.c.em @@ -16,35 +16,14 @@ from rosidl_cmake import convert_camel_case_to_lower_case_underscore type_name = message.structure.namespaced_type.name msg_typename = '%s__%s' % ('__'.join(message.structure.namespaced_type.namespaces), type_name) -header_filename = "{0}/rcldotnet_{1}.h".format('/'.join(message.structure.namespaced_type.namespaces), convert_camel_case_to_lower_case_underscore(type_name)) }@ - -#include -#include -#include -#include - -#include - -#include <@('/'.join(message.structure.namespaced_type.namespaces))/@(convert_camel_case_to_lower_case_underscore(type_name)).h> -#include "rosidl_runtime_c/message_type_support_struct.h" - -#include -#include - -#include -#include - - -#include "@(header_filename)" - void * @(msg_typename)__create_native_message() { @(msg_typename) * ros_message = @(msg_typename)__create(); return ros_message; } const void * @(msg_typename)__get_typesupport() { - const void * ptr = ROSIDL_GET_MSG_TYPE_SUPPORT(@(package_name), msg, @(type_name)); + const void * ptr = ROSIDL_GET_MSG_TYPE_SUPPORT(@(', '.join(message.structure.namespaced_type.namespaced_name()))); return ptr; } diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 77c4bd2f..61dfb5b7 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -16,17 +16,8 @@ from rosidl_parser.definition import NamespacedType type_name = message.structure.namespaced_type.name msg_typename = '%s__%s' % ('__'.join(message.structure.namespaced_type.namespaces), type_name) } -using System; -using System.Runtime.InteropServices; -using System.Collections.Generic; - -using ROS2.Interfaces; -using ROS2.Utils; - -@[for ns in message.structure.namespaced_type.namespaces]@ -namespace @(ns) +namespace @('.'.join(message.structure.namespaced_type.namespaces)) { -@[end for]@ public class @(type_name) : IMessage { private static readonly DllLoadUtils dllLoadUtils; @@ -356,6 +347,4 @@ public class @(type_name) : IMessage { @[end for]@ } -@[for ns in reversed(message.structure.namespaced_type.namespaces)]@ -} // namespace @(ns) -@[end for]@ \ No newline at end of file +} From 2229e71e05b78863d920292a02f4376ec33160d0 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 26 Aug 2021 13:38:54 +0200 Subject: [PATCH 12/73] Generate service definition classes --- rcldotnet_common/Interfaces.cs | 14 +++++ rosidl_generator_dotnet/resource/idl.c.em | 6 +++ rosidl_generator_dotnet/resource/idl.cs.em | 6 +++ rosidl_generator_dotnet/resource/idl.h.em | 22 +++++++- rosidl_generator_dotnet/resource/msg.h.em | 5 -- rosidl_generator_dotnet/resource/srv.c.em | 8 +++ rosidl_generator_dotnet/resource/srv.cs.em | 60 +++++++++++++++++++++- rosidl_generator_dotnet/resource/srv.h.em | 25 +++++++++ 8 files changed, 138 insertions(+), 8 deletions(-) create mode 100644 rosidl_generator_dotnet/resource/srv.c.em create mode 100644 rosidl_generator_dotnet/resource/srv.h.em diff --git a/rcldotnet_common/Interfaces.cs b/rcldotnet_common/Interfaces.cs index 9c32eb5a..e67489bf 100644 --- a/rcldotnet_common/Interfaces.cs +++ b/rcldotnet_common/Interfaces.cs @@ -20,12 +20,26 @@ namespace Interfaces { public interface IMessageStruct { } public interface IMessage { + // must be implemented on deriving types, gets called via reflection + // (static abstract interface members are not supported yet.) + // public static abstract IntPtr _GET_TYPE_SUPPORT(); + IntPtr _CREATE_NATIVE_MESSAGE (); void _READ_HANDLE (IntPtr messageHandle); void _DESTROY_NATIVE_MESSAGE (IntPtr messageHandle); void _WRITE_HANDLE (IntPtr messageHandle); } + public interface IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new() + { + // must be implemented on deriving types, gets called via reflection + // (static abstract interface members are not supported yet.) + // public static abstract IntPtr _GET_TYPE_SUPPORT(); + } + + // TODO: (sh) Rename missleading IDisposable interface public interface IDisposable { IntPtr Handle { get; diff --git a/rosidl_generator_dotnet/resource/idl.c.em b/rosidl_generator_dotnet/resource/idl.c.em index 2b34d90c..14efb40c 100644 --- a/rosidl_generator_dotnet/resource/idl.c.em +++ b/rosidl_generator_dotnet/resource/idl.c.em @@ -74,6 +74,12 @@ TEMPLATE( 'msg.c.em', package_name=package_name, interface_path=interface_path, message=service.response_message) }@ + +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, interface_path=interface_path, service=service) +}@ @[end for]@ @ @ diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index b8b7f15a..5bca49d2 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -50,6 +50,12 @@ TEMPLATE( 'msg.cs.em', package_name=package_name, interface_path=interface_path, message=service.response_message) }@ + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, interface_path=interface_path, service=service) +}@ @[end for]@ @ @ diff --git a/rosidl_generator_dotnet/resource/idl.h.em b/rosidl_generator_dotnet/resource/idl.h.em index 179ca24c..cf1fc096 100644 --- a/rosidl_generator_dotnet/resource/idl.h.em +++ b/rosidl_generator_dotnet/resource/idl.h.em @@ -10,7 +10,17 @@ @# - interface_path (Path relative to the directory named after the package) @# - content (IdlContent, list of elements, e.g. Messages or Services) @####################################################################### -@ +@{ +from rosidl_cmake import convert_camel_case_to_lower_case_underscore + +header_guard_parts = ["RCLDONTET", package_name] + list(interface_path.parents[0].parts) + [ + convert_camel_case_to_lower_case_underscore(interface_path.stem)] +header_guard = '__'.join([x.upper() for x in header_guard_parts]) + '__H' + +}@ +#ifndef @(header_guard) +#define @(header_guard) + @ @####################################################################### @# Handle messages @@ -45,6 +55,12 @@ TEMPLATE( 'msg.h.em', package_name=package_name, interface_path=interface_path, message=service.response_message) }@ + +@{ +TEMPLATE( + 'srv.h.em', + package_name=package_name, interface_path=interface_path, service=service) +}@ @[end for]@ @ @ @@ -53,4 +69,6 @@ TEMPLATE( @####################################################################### @ @# TODO - actions not implemented -@ \ No newline at end of file +@ + +#endif // @(header_guard) diff --git a/rosidl_generator_dotnet/resource/msg.h.em b/rosidl_generator_dotnet/resource/msg.h.em index 0b7f0243..2d113edb 100644 --- a/rosidl_generator_dotnet/resource/msg.h.em +++ b/rosidl_generator_dotnet/resource/msg.h.em @@ -12,10 +12,7 @@ from rosidl_generator_dotnet import msg_type_to_c type_name = message.structure.namespaced_type.name msg_typename = '%s__%s' % ('__'.join(message.structure.namespaced_type.namespaces), type_name) msg_prefix = "RCLDOTNET_{0}_{1}_{2}".format(package_name, '_'.join(message.structure.namespaced_type.namespaces), type_name).upper() -header_guard = "{0}_H".format(msg_prefix) }@ -#ifndef @(header_guard) -#define @(header_guard) #if defined(_MSC_VER) // Microsoft @@ -77,5 +74,3 @@ void @(msg_typename)__write_field_@(member.name)(void *, @(msg_type_to_c(member. void * @(msg_prefix)_CDECL @(msg_typename)__get_field_@(member.name)_HANDLE(void *); @[ end if]@ @[end for]@ - -#endif // @(header_guard) diff --git a/rosidl_generator_dotnet/resource/srv.c.em b/rosidl_generator_dotnet/resource/srv.c.em new file mode 100644 index 00000000..b554248c --- /dev/null +++ b/rosidl_generator_dotnet/resource/srv.c.em @@ -0,0 +1,8 @@ +@{ +type_name = service.namespaced_type.name +srv_typename = '%s__%s' % ('__'.join(service.namespaced_type.namespaces), type_name) +}@ +const void * @(srv_typename)__get_typesupport(void) { + const void * ptr = ROSIDL_GET_SRV_TYPE_SUPPORT(@(', '.join(service.namespaced_type.namespaced_name()))); + return ptr; +} diff --git a/rosidl_generator_dotnet/resource/srv.cs.em b/rosidl_generator_dotnet/resource/srv.cs.em index d15abba5..9ec5ed73 100644 --- a/rosidl_generator_dotnet/resource/srv.cs.em +++ b/rosidl_generator_dotnet/resource/srv.cs.em @@ -1 +1,59 @@ -// Empty +@{ +from rosidl_generator_dotnet import get_field_name +from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_builtin_dotnet_type +from rosidl_generator_dotnet import constant_value_to_dotnet + +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractString +from rosidl_parser.definition import AbstractWString +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import Array +from rosidl_parser.definition import BasicType +from rosidl_parser.definition import NamespacedType + +type_name = service.namespaced_type.name +request_type_name = service.request_message.structure.namespaced_type.name +response_type_name = service.response_message.structure.namespaced_type.name +srv_typename = '%s__%s' % ('__'.join(service.namespaced_type.namespaces), type_name) +} +namespace @('.'.join(service.namespaced_type.namespaces)) +{ +@# sealed class with private constructor -> no instance can be created +@# static classes can't implement an interface (or any other basetype), +@# but we need some type that can hold the typesupport and be passed to the Node.CreateService() method. +@# So sealed + private constructor is as static as it gets. +@# static abstract interface members are currently in preview, so maybe we could use the feature in the future. +@# (if hey add support to derive from static only interfaces in static classes) +@# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. + public sealed class @(type_name) : IRosServiceDefinition<@(request_type_name), @(response_type_name)> + { + private static readonly DllLoadUtils dllLoadUtils; + + private @(type_name)() + { + } + + static @(type_name)() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativelibrary = dllLoadUtils.LoadLibrary("@(package_name)__dotnetext"); + + IntPtr native_get_typesupport_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(srv_typename)__get_typesupport"); + + @(type_name).native_get_typesupport = (NativeGetTypeSupportType)Marshal.GetDelegateForFunctionPointer( + native_get_typesupport_ptr, typeof(NativeGetTypeSupportType)); + } + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + private delegate IntPtr NativeGetTypeSupportType(); + + private static NativeGetTypeSupportType native_get_typesupport = null; + +@# This method gets called via reflection as static abstract interface members are not supported yet. + public static IntPtr _GET_TYPE_SUPPORT() { + return native_get_typesupport(); + } + } +} diff --git a/rosidl_generator_dotnet/resource/srv.h.em b/rosidl_generator_dotnet/resource/srv.h.em new file mode 100644 index 00000000..46c279ba --- /dev/null +++ b/rosidl_generator_dotnet/resource/srv.h.em @@ -0,0 +1,25 @@ +@{ +type_name = service.namespaced_type.name +srv_typename = '%s__%s' % ('__'.join(service.namespaced_type.namespaces), type_name) +srv_prefix = "RCLDOTNET_{0}_{1}_{2}".format(package_name, '_'.join(service.namespaced_type.namespaces), type_name).upper() +}@ +#if defined(_MSC_VER) + // Microsoft + #define @(srv_prefix)_EXPORT __declspec(dllexport) + #define @(srv_prefix)_IMPORT __declspec(dllimport) + #define @(srv_prefix)_CDECL __cdecl +#elif defined(__GNUC__) + // GCC + #define @(srv_prefix)_EXPORT __attribute__((visibility("default"))) + #define @(srv_prefix)_IMPORT + #define @(srv_prefix)_CDECL __attribute__((__cdecl__)) +#else + // do nothing and hope for the best? + #define @(srv_prefix)_EXPORT + #define @(srv_prefix)_IMPORT + #define @(srv_prefix)_CDECL + #pragma warning Unknown dynamic link import/export semantics. +#endif + +@(srv_prefix)_EXPORT +const void * @(srv_prefix)_CDECL @(srv_typename)__get_typesupport(void); From 41f46a01870dfd6c5cc36db6c99fcd83325f735f Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 26 Aug 2021 13:39:54 +0200 Subject: [PATCH 13/73] Add initial support for services --- rcldotnet/CMakeLists.txt | 3 + rcldotnet/INode.cs | 7 +++ rcldotnet/IService.cs | 11 ++++ rcldotnet/IServiceBase.cs | 13 ++++ rcldotnet/Node.cs | 37 +++++++++++ rcldotnet/RCLdotnet.cs | 123 ++++++++++++++++++++++++++++++++++++- rcldotnet/Service.cs | 36 +++++++++++ rcldotnet/rcldotnet.c | 34 ++++++++++ rcldotnet/rcldotnet.h | 15 +++++ rcldotnet/rcldotnet_node.c | 23 +++++++ rcldotnet/rcldotnet_node.h | 6 ++ 11 files changed, 307 insertions(+), 1 deletion(-) create mode 100644 rcldotnet/IService.cs create mode 100644 rcldotnet/IServiceBase.cs create mode 100644 rcldotnet/Service.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 30b01cc6..e92c6e0a 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -28,11 +28,14 @@ endif() set(CS_SOURCES INode.cs IPublisher.cs + IService.cs + IServiceBase.cs ISubscription.cs ISubscriptionBase.cs Node.cs Publisher.cs RCLdotnet.cs + Service.cs Subscription.cs ) diff --git a/rcldotnet/INode.cs b/rcldotnet/INode.cs index 5dba1a09..b346476b 100644 --- a/rcldotnet/INode.cs +++ b/rcldotnet/INode.cs @@ -21,8 +21,15 @@ namespace ROS2 { public interface INode { IList Subscriptions { get; } + IList Services { get; } + IPublisher CreatePublisher (string topic) where T : IMessage; ISubscription CreateSubscription (string topic, Action callback) where T : IMessage, new (); + + Service CreateService(string topic, Action callback) + where TService : IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new(); } } diff --git a/rcldotnet/IService.cs b/rcldotnet/IService.cs new file mode 100644 index 00000000..39b214ae --- /dev/null +++ b/rcldotnet/IService.cs @@ -0,0 +1,11 @@ +using ROS2.Interfaces; + +namespace ROS2 +{ + public interface IService : IServiceBase + where TService : IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new() + { + } +} diff --git a/rcldotnet/IServiceBase.cs b/rcldotnet/IServiceBase.cs new file mode 100644 index 00000000..40512f35 --- /dev/null +++ b/rcldotnet/IServiceBase.cs @@ -0,0 +1,13 @@ +using ROS2.Interfaces; + +namespace ROS2 +{ + public interface IServiceBase : ROS2.Interfaces.IDisposable + { + IMessage CreateRequest(); + + IMessage CreateResponse(); + + void TriggerCallback (IMessage request, IMessage response); + } +} diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 5f5756b0..f8388732 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -42,6 +42,12 @@ internal delegate int NativeRCLCreateSubscriptionHandleType ( internal static NativeRCLCreateSubscriptionHandleType native_rcl_create_subscription_handle = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLCreateServiceHandleType ( + ref IntPtr serviceHandle, IntPtr nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); + + internal static NativeRCLCreateServiceHandleType native_rcl_create_service_handle = null; + static NodeDelegates () { dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); IntPtr nativelibrary = dllLoadUtils.LoadLibrary ("rcldotnet_node"); @@ -59,6 +65,13 @@ static NodeDelegates () { NodeDelegates.native_rcl_create_subscription_handle = (NativeRCLCreateSubscriptionHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_create_subscription_handle_ptr, typeof (NativeRCLCreateSubscriptionHandleType)); + + IntPtr native_rcl_create_service_handle_ptr = dllLoadUtils.GetProcAddress ( + nativelibrary, "native_rcl_create_service_handle"); + + NodeDelegates.native_rcl_create_service_handle = + (NativeRCLCreateServiceHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_create_service_handle_ptr, typeof (NativeRCLCreateServiceHandleType)); } } @@ -66,13 +79,19 @@ public class Node : INode { private IList subscriptions_; + private IList services_; + public Node (IntPtr handle) { Handle = handle; subscriptions_ = new List (); + services_ = new List(); } public IList Subscriptions { get { return subscriptions_; } } + // TODO: (sh) wrap in readonly collection + public IList Services => services_; + public IntPtr Handle { get; } public IPublisher CreatePublisher (string topic) where T : IMessage { @@ -95,5 +114,23 @@ public IPublisher CreatePublisher (string topic) where T : IMessage { this.subscriptions_.Add (subscription); return subscription; } + + public Service CreateService(string serviceName, Action callback) + where TService : IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new() + { + MethodInfo m = typeof(TService).GetTypeInfo().GetDeclaredMethod("_GET_TYPE_SUPPORT"); + IntPtr typesupport = (IntPtr)m.Invoke (null, new object[] { }); + + // TODO: (sh) check return value + // TODO: (sh) natvie memory managment + IntPtr serviceHandle = IntPtr.Zero; + RCLRet ret = (RCLRet)NodeDelegates.native_rcl_create_service_handle(ref serviceHandle, Handle, serviceName, typesupport); + + var service = new Service(serviceHandle, callback); + this.services_.Add(service); + return service; + } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index afab452a..f8fd2ecd 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -68,11 +68,17 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLWaitSetClearType native_rcl_wait_set_clear = null; + // TODO: (sh) fix return type [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate void NativeRCLWaitSetAddSubscriptionType (IntPtr waitSetHandle, IntPtr subscriptionHandle); internal static NativeRCLWaitSetAddSubscriptionType native_rcl_wait_set_add_subscription = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLWaitSetAddServiceType (IntPtr waitSetHandle, IntPtr serviceHandle); + + internal static NativeRCLWaitSetAddServiceType native_rcl_wait_set_add_service = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate void NativeRCLDestroyWaitSetType (IntPtr waitSetHandle); @@ -88,6 +94,26 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLTakeType native_rcl_take = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate IntPtr NativeRCLCreateRequestHeaderHandleType (); + + internal static NativeRCLCreateRequestHeaderHandleType native_rcl_create_request_header_handle = null; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate void NativeRCLDestroyRequestHeaderHandleType (IntPtr requestHeaderHandle); + + internal static NativeRCLDestroyRequestHeaderHandleType native_rcl_destroy_request_header_handle = null; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLTakeRequestType (IntPtr serviceHandle, IntPtr requestHeaderHandle, IntPtr requestHandle); + + internal static NativeRCLTakeRequestType native_rcl_take_request = null; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLSendResponseType (IntPtr serviceHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); + + internal static NativeRCLSendResponseType native_rcl_send_response = null; + static RCLdotnetDelegates () { dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); string library_name = "rcldotnet"; @@ -141,6 +167,12 @@ static RCLdotnetDelegates () { (NativeRCLWaitSetAddSubscriptionType) Marshal.GetDelegateForFunctionPointer ( native_rcl_wait_set_add_subscription_ptr, typeof (NativeRCLWaitSetAddSubscriptionType)); + IntPtr native_rcl_wait_set_add_service_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_add_service"); + RCLdotnetDelegates.native_rcl_wait_set_add_service = + (NativeRCLWaitSetAddServiceType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_wait_set_add_service_ptr, typeof (NativeRCLWaitSetAddServiceType)); + IntPtr native_rcl_destroy_wait_set_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_wait_set"); RCLdotnetDelegates.native_rcl_destroy_wait_set = @@ -159,6 +191,30 @@ static RCLdotnetDelegates () { RCLdotnetDelegates.native_rcl_take = (NativeRCLTakeType) Marshal.GetDelegateForFunctionPointer ( native_rcl_take_ptr, typeof (NativeRCLTakeType)); + + IntPtr native_rcl_create_request_header_handle_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_create_request_header_handle"); + RCLdotnetDelegates.native_rcl_create_request_header_handle = + (NativeRCLCreateRequestHeaderHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_create_request_header_handle_ptr, typeof (NativeRCLCreateRequestHeaderHandleType)); + + IntPtr native_rcl_destroy_request_header_handle_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_request_header_handle"); + RCLdotnetDelegates.native_rcl_destroy_request_header_handle = + (NativeRCLDestroyRequestHeaderHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_request_header_handle_ptr, typeof (NativeRCLDestroyRequestHeaderHandleType)); + + IntPtr native_rcl_take_request_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_take_request"); + RCLdotnetDelegates.native_rcl_take_request = + (NativeRCLTakeRequestType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_take_request_ptr, typeof (NativeRCLTakeRequestType)); + + IntPtr native_rcl_send_response_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_send_response"); + RCLdotnetDelegates.native_rcl_send_response = + (NativeRCLSendResponseType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_send_response_ptr, typeof (NativeRCLSendResponseType)); } } @@ -212,6 +268,10 @@ private static void WaitSetAddSubscription (IntPtr waitSetHandle, IntPtr subscri RCLdotnetDelegates.native_rcl_wait_set_add_subscription (waitSetHandle, subscriptionHandle); } + private static void WaitSetAddService (IntPtr waitSetHandle, IntPtr serviceHandle) { + RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); + } + private static void DestroyWaitSet (IntPtr waitSetHandle) { RCLdotnetDelegates.native_rcl_destroy_wait_set (waitSetHandle); } @@ -241,6 +301,40 @@ private static bool Take (IntPtr subscriptionHandle, IMessage message) { return status; } + private static bool TakeRequest(IntPtr serviceHandle, IntPtr requestHeaderHandle, IMessage request) { + bool status = false; + IntPtr requestHandle = request._CREATE_NATIVE_MESSAGE(); + RCLRet ret = (RCLRet)RCLdotnetDelegates.native_rcl_take_request(serviceHandle, requestHeaderHandle, requestHandle); + switch (ret) + { + case RCLRet.Ok: + request._READ_HANDLE(requestHandle); + status = true; + break; + case RCLRet.ServiceTakeFailed: + status = false; + break; + default: + break; + } + + // TODO: (sh) don't leak memory on exceptions + request._DESTROY_NATIVE_MESSAGE (requestHandle); + return status; + } + + public static void SendResponse(IntPtr serviceHandle, IntPtr requestHeaderHandle, IMessage response) + { + IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); + response._WRITE_HANDLE (responseHandle); + + // TODO: (sh) check return values + RCLRet ret = (RCLRet)RCLdotnetDelegates.native_rcl_send_response(serviceHandle, requestHeaderHandle, responseHandle); + + // TODO: (sh) don't leak memory on exceptions + response._DESTROY_NATIVE_MESSAGE (responseHandle); + } + public static void SpinOnce (INode node, long timeout) { IntPtr waitSetHandle = GetZeroInitializedWaitSet (); @@ -248,7 +342,7 @@ public static void SpinOnce (INode node, long timeout) { long numberOfGuardConditions = 0; long numberOfTimers = 0; long numberOfClients = 0; - long numberOfServices = 0; + long numberOfServices = node.Services.Count; long numberOfEvents = 0; WaitSetInit ( @@ -267,6 +361,11 @@ public static void SpinOnce (INode node, long timeout) { WaitSetAddSubscription (waitSetHandle, subscription.Handle); } + foreach (var service in node.Services) + { + WaitSetAddService(waitSetHandle, service.Handle); + } + Wait (waitSetHandle, timeout); foreach (ISubscriptionBase subscription in node.Subscriptions) { @@ -276,6 +375,28 @@ public static void SpinOnce (INode node, long timeout) { subscription.TriggerCallback (message); } } + + // gets reused for each element in the for loop. + IntPtr requestHeaderHandle = RCLdotnetDelegates.native_rcl_create_request_header_handle(); + + foreach (var service in node.Services) + { + var request = service.CreateRequest(); + var response = service.CreateResponse(); + + var result = TakeRequest(service.Handle, requestHeaderHandle, request); + if (result) + { + // TODO: (sh) catch exceptions + service.TriggerCallback(request, response); + + SendResponse(service.Handle, requestHeaderHandle, response); + } + } + + // TODO: (sh) don't leak memory on exceptions + RCLdotnetDelegates.native_rcl_destroy_request_header_handle(requestHeaderHandle); + DestroyWaitSet (waitSetHandle); } diff --git a/rcldotnet/Service.cs b/rcldotnet/Service.cs new file mode 100644 index 00000000..8c30bd1a --- /dev/null +++ b/rcldotnet/Service.cs @@ -0,0 +1,36 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Runtime.InteropServices; +using System.Text; +using System.Threading.Tasks; +using ROS2.Interfaces; +using ROS2.Utils; + +namespace ROS2 +{ + public sealed class Service : IService + where TService : IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new() + { + private Action _callback; + + public Service(IntPtr handle, Action callback) + { + Handle = handle; + _callback = callback; + } + + public IntPtr Handle { get; } + + public IMessage CreateRequest() => (IMessage)new TRequest(); + + public IMessage CreateResponse() => (IMessage)new TResponse(); + + public void TriggerCallback(IMessage request, IMessage response) + { + _callback((TRequest)request, (TResponse)response); + } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 6ea74086..74940335 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -102,6 +102,14 @@ int32_t native_rcl_wait_set_add_subscription(void *wait_set_handle, void *subscr return ret; } +int32_t native_rcl_wait_set_add_service(void *wait_set_handle, void *service_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_service_t *service = (rcl_service_t *)service_handle; + rcl_ret_t ret = rcl_wait_set_add_service(wait_set, service, NULL); + + return ret; +} + void native_rcl_destroy_wait_set(void *wait_set_handle) { free((rcl_wait_set_t *)wait_set_handle); } @@ -120,6 +128,32 @@ int32_t native_rcl_take(void *subscription_handle, void *message_handle) { return ret; } +void * native_rcl_create_request_header_handle(void) { + rmw_request_id_t *request_header = (rmw_request_id_t *)malloc(sizeof(rmw_request_id_t)); + memset(request_header, 0, sizeof(rmw_request_id_t)); + return (void *)request_header; +} + +void native_rcl_destroy_request_header_handle(void *request_header_handle) { + free((rmw_request_id_t *)request_header_handle); +} + +int32_t native_rcl_take_request(void *service_handle, void *request_header_handle, void *request_handle) { + rcl_service_t * service = (rcl_service_t *)service_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_take_request(service, request_header, request_handle); + return ret; +} + +int32_t native_rcl_send_response(void *service_handle, void *request_header_handle, void *resopnse_handle) { + rcl_service_t * service = (rcl_service_t *)service_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_send_response(service, request_header, resopnse_handle); + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t * wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 7677c8c5..1ff69e31 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -48,6 +48,9 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_clear(void *); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_subscription(void *, void *); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_service(void *wait_set_handle, void *service_handle); + RCLDOTNET_EXPORT void RCLDOTNET_CDECL native_rcl_destroy_wait_set(void *); @@ -60,4 +63,16 @@ int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); +RCLDOTNET_EXPORT +void * RCLDOTNET_CDECL native_rcl_create_request_header_handle(void); + +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_destroy_request_header_handle(void *request_header_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_take_request(void *service_handle, void *request_header_handle, void *request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_send_response(void *service_handle, void *request_header_handle, void *resopnse_handle); + #endif // RCLDOTNET_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index 3e16fec3..dc410100 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -68,3 +68,26 @@ int32_t native_rcl_create_subscription_handle(void **subscription_handle, return ret; } + +int32_t native_rcl_create_service_handle(void **service_handle, + void *node_handle, + const char *service_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_service_type_support_t *ts = + (rosidl_service_type_support_t *)typesupport; + + rcl_service_t *service = + (rcl_service_t *)malloc(sizeof(rcl_service_t)); + service->impl = NULL; + rcl_service_options_t service_ops = + rcl_service_get_default_options(); + + rcl_ret_t ret = + rcl_service_init(service, node, ts, service_name, &service_ops); + + *service_handle = (void *)service; + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index 1ea46bc4..71d5222c 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -27,4 +27,10 @@ int32_t RCLDOTNET_CDECL native_rcl_create_subscription_handle(void **, void *, const char *, void *); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_create_service_handle(void **service_handle, + void *node_handle, + const char *service_name, + void *typesupport); + #endif // RCLDOTNET_NODE_H From 8722462bece45b5c56f38e197738c4bd0f3995ce Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 26 Aug 2021 13:40:14 +0200 Subject: [PATCH 14/73] Add service example --- rcldotnet_examples/CMakeLists.txt | 9 +++++++- rcldotnet_examples/RCLDotnetService.cs | 29 ++++++++++++++++++++++++++ rcldotnet_examples/package.xml | 2 ++ 3 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 rcldotnet_examples/RCLDotnetService.cs diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index 25c3ce4e..a3d2b1a2 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(rcldotnet REQUIRED) find_package(rcldotnet_common REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(rcldotnet REQUIRED) find_package(dotnet_cmake_module REQUIRED) @@ -19,7 +20,7 @@ set(_assemblies_dep_dlls ${rcldotnet_common_ASSEMBLIES_DLL} ${rcldotnet_ASSEMBLIES_DLL} ${std_msgs_ASSEMBLIES_DLL} - ${rcldotnet_ASSEMBLIES_DLL} + ${std_srvs_ASSEMBLIES_DLL} ) message("Included assemblies: ${_assemblies_dep_dlls}") @@ -34,8 +35,14 @@ add_dotnet_executable(rcldotnet_listener INCLUDE_DLLS ${_assemblies_dep_dlls} ) +add_dotnet_executable(rcldotnet_example_service + RCLDotnetService.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) install_dotnet(rcldotnet_talker DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_listener DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_service DESTINATION lib/${PROJECT_NAME}/dotnet) ament_package() diff --git a/rcldotnet_examples/RCLDotnetService.cs b/rcldotnet_examples/RCLDotnetService.cs new file mode 100644 index 00000000..a74c7d5b --- /dev/null +++ b/rcldotnet_examples/RCLDotnetService.cs @@ -0,0 +1,29 @@ +using System; +using System.Reflection; +using System.Runtime; +using System.Runtime.InteropServices; +using System.Threading; +using ROS2; + +namespace ConsoleApplication +{ + public static class RCLDotnetService + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("service"); + + var service = node.CreateService("test_dotnet_service_name", HandleRequest); + + RCLdotnet.Spin(node); + } + + private static void HandleRequest(std_srvs.srv.SetBool_Request request, std_srvs.srv.SetBool_Response response) + { + Console.WriteLine($"Service got called with data {request.Data}"); + response.Success = true; + response.Message = $"Got data {request.Data}"; + } + } +} diff --git a/rcldotnet_examples/package.xml b/rcldotnet_examples/package.xml index 6bc8f73d..1a4ef781 100644 --- a/rcldotnet_examples/package.xml +++ b/rcldotnet_examples/package.xml @@ -16,12 +16,14 @@ example_interfaces rcldotnet std_msgs + std_srvs sensor_msgs rcldotnet_common example_interfaces rcldotnet std_msgs + std_srvs sensor_msgs From 1e8521d497f8adf160911a193e67cfbf15633f22 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 31 Aug 2021 15:33:58 +0200 Subject: [PATCH 15/73] Add initial support for clients --- rcldotnet/CMakeLists.txt | 3 +- rcldotnet/Client.cs | 104 +++++++++++++++++++++++++++++++++++ rcldotnet/INode.cs | 7 +++ rcldotnet/Node.cs | 36 ++++++++++++ rcldotnet/RCLdotnet.cs | 78 +++++++++++++++++++++++++- rcldotnet/rcldotnet.c | 21 +++++++ rcldotnet/rcldotnet.h | 9 +++ rcldotnet/rcldotnet_client.c | 33 +++++++++++ rcldotnet/rcldotnet_client.h | 23 ++++++++ rcldotnet/rcldotnet_node.c | 23 ++++++++ rcldotnet/rcldotnet_node.h | 6 ++ 11 files changed, 341 insertions(+), 2 deletions(-) create mode 100644 rcldotnet/Client.cs create mode 100644 rcldotnet/rcldotnet_client.c create mode 100644 rcldotnet/rcldotnet_client.h diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index e92c6e0a..c330b783 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -26,6 +26,7 @@ if(NOT WIN32) endif() set(CS_SOURCES + Client.cs INode.cs IPublisher.cs IService.cs @@ -54,7 +55,7 @@ add_dotnet_library(${PROJECT_NAME}_assemblies install_dotnet(${PROJECT_NAME}_assemblies DESTINATION lib/${PROJECT_NAME}/dotnet) ament_export_assemblies_dll("lib/${PROJECT_NAME}/dotnet/${PROJECT_NAME}_assemblies.dll") -set(_native_sources "rcldotnet;rcldotnet_node;rcldotnet_publisher") +set(_native_sources "rcldotnet;rcldotnet_node;rcldotnet_publisher;rcldotnet_client") foreach(_target_name ${_native_sources}) add_library(${_target_name} SHARED diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs new file mode 100644 index 00000000..37f7109a --- /dev/null +++ b/rcldotnet/Client.cs @@ -0,0 +1,104 @@ +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Linq; +using System.Runtime.InteropServices; +using System.Text; +using System.Threading.Tasks; +using ROS2.Common; +using ROS2.Interfaces; +using ROS2.Utils; + +namespace ROS2 +{ + internal class ClientDelegates + { + internal static readonly DllLoadUtils dllLoadUtils; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLSendRequestType( + IntPtr clientHandle, IntPtr requestHandle, out long seqneceNumber); + + internal static NativeRCLSendRequestType native_rcl_send_request = null; + + static ClientDelegates() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativelibrary = dllLoadUtils.LoadLibrary("rcldotnet_client"); + + IntPtr native_rcl_send_request_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "native_rcl_send_request"); + ClientDelegates.native_rcl_send_request = (NativeRCLSendRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_send_request_ptr, typeof(NativeRCLSendRequestType)); + } + } + + public abstract class Client : ROS2.Interfaces.IDisposable + { + public abstract IntPtr Handle { get; } + + public abstract IMessage CreateResponse(); + + internal abstract void HandleResponse(long sequenceNumber, IMessage response); + } + + public sealed class Client : Client + where TService : IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new() + { + private ConcurrentDictionary _pendingRequests = new ConcurrentDictionary(); + public Client(IntPtr handle) + { + Handle = handle; + } + + public override IntPtr Handle { get; } + + public override IMessage CreateResponse() => (IMessage)new TResponse(); + + public Task SendRequestAsync(TRequest request) + { + // TODO: (sh) Add cancellationToken(?), timeout (via cancellationToken?) and cleanup of pending requests. + // TODO: (sh) Catch all exceptions and return Task with error? + // How should this be done according to best practices. + IntPtr requestHandle = request._CREATE_NATIVE_MESSAGE(); + + request._WRITE_HANDLE(requestHandle); + + RCLRet ret = (RCLRet)ClientDelegates.native_rcl_send_request(Handle, requestHandle, out var sequenceNumber); + + // TODO: (sh) don't leak memory on exceptions + request._DESTROY_NATIVE_MESSAGE(requestHandle); + + var taskCompletionSource = new TaskCompletionSource(); + var pendingRequest = new PendingRequest(taskCompletionSource); + if (!_pendingRequests.TryAdd(sequenceNumber, pendingRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + return taskCompletionSource.Task; + } + + internal override void HandleResponse(long sequenceNumber, IMessage response) + { + if (!_pendingRequests.TryRemove(sequenceNumber, out var pendingRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + pendingRequest.TaskCompletionSource.TrySetResult((TResponse)response); + } + + private sealed class PendingRequest + { + public PendingRequest(TaskCompletionSource taskCompletionSource) + { + TaskCompletionSource = taskCompletionSource; + } + + public TaskCompletionSource TaskCompletionSource { get; } + } + } +} diff --git a/rcldotnet/INode.cs b/rcldotnet/INode.cs index b346476b..6ed71a30 100644 --- a/rcldotnet/INode.cs +++ b/rcldotnet/INode.cs @@ -23,6 +23,8 @@ public interface INode { IList Services { get; } + IList Clients { get; } + IPublisher CreatePublisher (string topic) where T : IMessage; ISubscription CreateSubscription (string topic, Action callback) where T : IMessage, new (); @@ -31,5 +33,10 @@ Service CreateService where TRequest : IMessage, new() where TResponse : IMessage, new(); + + Client CreateClient(string serviceName) + where TService : IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new(); } } diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index f8388732..dc23e717 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -48,6 +48,12 @@ internal delegate int NativeRCLCreateServiceHandleType ( internal static NativeRCLCreateServiceHandleType native_rcl_create_service_handle = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLCreateClientHandleType ( + ref IntPtr clientHandle, IntPtr nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); + + internal static NativeRCLCreateClientHandleType native_rcl_create_client_handle = null; + static NodeDelegates () { dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); IntPtr nativelibrary = dllLoadUtils.LoadLibrary ("rcldotnet_node"); @@ -72,6 +78,13 @@ static NodeDelegates () { NodeDelegates.native_rcl_create_service_handle = (NativeRCLCreateServiceHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_create_service_handle_ptr, typeof (NativeRCLCreateServiceHandleType)); + + IntPtr native_rcl_create_client_handle_ptr = dllLoadUtils.GetProcAddress ( + nativelibrary, "native_rcl_create_client_handle"); + + NodeDelegates.native_rcl_create_client_handle = + (NativeRCLCreateClientHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_create_client_handle_ptr, typeof (NativeRCLCreateClientHandleType)); } } @@ -81,10 +94,13 @@ public class Node : INode { private IList services_; + private IList clients_; + public Node (IntPtr handle) { Handle = handle; subscriptions_ = new List (); services_ = new List(); + clients_ = new List(); } public IList Subscriptions { get { return subscriptions_; } } @@ -92,6 +108,8 @@ public Node (IntPtr handle) { // TODO: (sh) wrap in readonly collection public IList Services => services_; + public IList Clients => clients_; + public IntPtr Handle { get; } public IPublisher CreatePublisher (string topic) where T : IMessage { @@ -132,5 +150,23 @@ public Service CreateService CreateClient(string serviceName) + where TService : IRosServiceDefinition + where TRequest : IMessage, new() + where TResponse : IMessage, new() + { + MethodInfo m = typeof(TService).GetTypeInfo().GetDeclaredMethod("_GET_TYPE_SUPPORT"); + IntPtr typesupport = (IntPtr)m.Invoke (null, new object[] { }); + + // TODO: (sh) check return value + // TODO: (sh) natvie memory managment + IntPtr clientHandle = IntPtr.Zero; + RCLRet ret = (RCLRet)NodeDelegates.native_rcl_create_client_handle(ref clientHandle, Handle, serviceName, typesupport); + + var client = new Client(clientHandle); + this.clients_.Add(client); + return client; + } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index f8fd2ecd..a102c78b 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -79,6 +79,11 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLWaitSetAddServiceType native_rcl_wait_set_add_service = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLWaitSetAddClientType (IntPtr waitSetHandle, IntPtr clientHandle); + + internal static NativeRCLWaitSetAddClientType native_rcl_wait_set_add_client = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate void NativeRCLDestroyWaitSetType (IntPtr waitSetHandle); @@ -104,6 +109,11 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLDestroyRequestHeaderHandleType native_rcl_destroy_request_header_handle = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate long NativeRCLRequestHeaderGetSequenceNumberType (IntPtr requestHeaderHandle); + + internal static NativeRCLRequestHeaderGetSequenceNumberType native_rcl_request_header_get_sequence_number = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate int NativeRCLTakeRequestType (IntPtr serviceHandle, IntPtr requestHeaderHandle, IntPtr requestHandle); @@ -114,6 +124,11 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLSendResponseType native_rcl_send_response = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLTakeResponseType (IntPtr clientHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); + + internal static NativeRCLTakeResponseType native_rcl_take_response = null; + static RCLdotnetDelegates () { dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); string library_name = "rcldotnet"; @@ -173,6 +188,12 @@ static RCLdotnetDelegates () { (NativeRCLWaitSetAddServiceType) Marshal.GetDelegateForFunctionPointer ( native_rcl_wait_set_add_service_ptr, typeof (NativeRCLWaitSetAddServiceType)); + IntPtr native_rcl_wait_set_add_client_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_add_client"); + RCLdotnetDelegates.native_rcl_wait_set_add_client = + (NativeRCLWaitSetAddClientType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_wait_set_add_client_ptr, typeof (NativeRCLWaitSetAddClientType)); + IntPtr native_rcl_destroy_wait_set_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_wait_set"); RCLdotnetDelegates.native_rcl_destroy_wait_set = @@ -204,6 +225,12 @@ static RCLdotnetDelegates () { (NativeRCLDestroyRequestHeaderHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_destroy_request_header_handle_ptr, typeof (NativeRCLDestroyRequestHeaderHandleType)); + IntPtr native_rcl_request_header_get_sequence_number_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_request_header_get_sequence_number"); + RCLdotnetDelegates.native_rcl_request_header_get_sequence_number = + (NativeRCLRequestHeaderGetSequenceNumberType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_request_header_get_sequence_number_ptr, typeof (NativeRCLRequestHeaderGetSequenceNumberType)); + IntPtr native_rcl_take_request_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_take_request"); RCLdotnetDelegates.native_rcl_take_request = @@ -215,6 +242,12 @@ static RCLdotnetDelegates () { RCLdotnetDelegates.native_rcl_send_response = (NativeRCLSendResponseType) Marshal.GetDelegateForFunctionPointer ( native_rcl_send_response_ptr, typeof (NativeRCLSendResponseType)); + + IntPtr native_rcl_take_response_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_take_response"); + RCLdotnetDelegates.native_rcl_take_response = + (NativeRCLTakeResponseType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_take_response_ptr, typeof (NativeRCLTakeResponseType)); } } @@ -272,6 +305,10 @@ private static void WaitSetAddService (IntPtr waitSetHandle, IntPtr serviceHandl RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); } + private static void WaitSetAddClient (IntPtr waitSetHandle, IntPtr clientHandle) { + RCLdotnetDelegates.native_rcl_wait_set_add_client(waitSetHandle, clientHandle); + } + private static void DestroyWaitSet (IntPtr waitSetHandle) { RCLdotnetDelegates.native_rcl_destroy_wait_set (waitSetHandle); } @@ -323,6 +360,28 @@ private static bool TakeRequest(IntPtr serviceHandle, IntPtr requestHeaderHandle return status; } + private static bool TakeResponse(IntPtr clientHandle, IntPtr requestHeaderHandle, IMessage response) { + bool status = false; + IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); + RCLRet ret = (RCLRet)RCLdotnetDelegates.native_rcl_take_response(clientHandle, requestHeaderHandle, responseHandle); + switch (ret) + { + case RCLRet.Ok: + response._READ_HANDLE(responseHandle); + status = true; + break; + case RCLRet.ClientTakeFailed: + status = false; + break; + default: + break; + } + + // TODO: (sh) don't leak memory on exceptions + response._DESTROY_NATIVE_MESSAGE (responseHandle); + return status; + } + public static void SendResponse(IntPtr serviceHandle, IntPtr requestHeaderHandle, IMessage response) { IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); @@ -341,7 +400,7 @@ public static void SpinOnce (INode node, long timeout) { long numberOfSubscriptions = node.Subscriptions.Count; long numberOfGuardConditions = 0; long numberOfTimers = 0; - long numberOfClients = 0; + long numberOfClients = node.Clients.Count; long numberOfServices = node.Services.Count; long numberOfEvents = 0; @@ -366,6 +425,11 @@ public static void SpinOnce (INode node, long timeout) { WaitSetAddService(waitSetHandle, service.Handle); } + foreach (var client in node.Clients) + { + WaitSetAddClient(waitSetHandle, client.Handle); + } + Wait (waitSetHandle, timeout); foreach (ISubscriptionBase subscription in node.Subscriptions) { @@ -394,6 +458,18 @@ public static void SpinOnce (INode node, long timeout) { } } + foreach (var client in node.Clients) + { + var response = client.CreateResponse(); + + var result = TakeResponse(client.Handle, requestHeaderHandle, response); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_header_get_sequence_number(requestHeaderHandle); + client.HandleResponse(sequenceNumber, response); + } + } + // TODO: (sh) don't leak memory on exceptions RCLdotnetDelegates.native_rcl_destroy_request_header_handle(requestHeaderHandle); diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 74940335..2a0a6756 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -110,6 +110,14 @@ int32_t native_rcl_wait_set_add_service(void *wait_set_handle, void *service_han return ret; } +int32_t native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_client_t *service = (rcl_client_t *)client_handle; + rcl_ret_t ret = rcl_wait_set_add_client(wait_set, service, NULL); + + return ret; +} + void native_rcl_destroy_wait_set(void *wait_set_handle) { free((rcl_wait_set_t *)wait_set_handle); } @@ -138,6 +146,11 @@ void native_rcl_destroy_request_header_handle(void *request_header_handle) { free((rmw_request_id_t *)request_header_handle); } +int64_t native_rcl_request_header_get_sequence_number(void *request_header_handle) { + rmw_request_id_t *request_header = (rmw_request_id_t *)request_header_handle; + return request_header->sequence_number; +} + int32_t native_rcl_take_request(void *service_handle, void *request_header_handle, void *request_handle) { rcl_service_t * service = (rcl_service_t *)service_handle; rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; @@ -154,6 +167,14 @@ int32_t native_rcl_send_response(void *service_handle, void *request_header_hand return ret; } +int32_t native_rcl_take_response(void *client_handle, void *request_header_handle, void *response_handle) { + rcl_client_t * client = (rcl_client_t *)client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_take_response(client, request_header, response_handle); + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t * wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 1ff69e31..b5b739ab 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -51,6 +51,9 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_subscription(void *, void *); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_service(void *wait_set_handle, void *service_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handle); + RCLDOTNET_EXPORT void RCLDOTNET_CDECL native_rcl_destroy_wait_set(void *); @@ -69,10 +72,16 @@ void * RCLDOTNET_CDECL native_rcl_create_request_header_handle(void); RCLDOTNET_EXPORT void RCLDOTNET_CDECL native_rcl_destroy_request_header_handle(void *request_header_handle); +RCLDOTNET_EXPORT +int64_t RCLDOTNET_CDECL native_rcl_request_header_get_sequence_number(void *request_header_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take_request(void *service_handle, void *request_header_handle, void *request_handle); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_send_response(void *service_handle, void *request_header_handle, void *resopnse_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_take_response(void *client_handle, void *request_header_handle, void *response_handle); + #endif // RCLDOTNET_H diff --git a/rcldotnet/rcldotnet_client.c b/rcldotnet/rcldotnet_client.c new file mode 100644 index 00000000..248e032a --- /dev/null +++ b/rcldotnet/rcldotnet_client.c @@ -0,0 +1,33 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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 +#include + +#include +#include +#include +#include + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcldotnet_client.h" + +int32_t native_rcl_send_request(void *client_handle, void *request_handle, int64_t *sequence_number) { + rcl_client_t * client = (rcl_client_t *)client_handle; + + rcl_ret_t ret = rcl_send_request(client, request_handle, sequence_number); + return ret; +} diff --git a/rcldotnet/rcldotnet_client.h b/rcldotnet/rcldotnet_client.h new file mode 100644 index 00000000..1451ec8a --- /dev/null +++ b/rcldotnet/rcldotnet_client.h @@ -0,0 +1,23 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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_CLIENT_H +#define RCLDOTNET_CLIENT_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_send_request(void *client_handle, void *request_handle, int64_t *sequence_number); + +#endif // RCLDOTNET_CLIENT_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index dc410100..52bbc3cf 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -91,3 +91,26 @@ int32_t native_rcl_create_service_handle(void **service_handle, return ret; } + +int32_t native_rcl_create_client_handle(void **client_handle, + void *node_handle, + const char *service_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_service_type_support_t *ts = + (rosidl_service_type_support_t *)typesupport; + + rcl_client_t *client = + (rcl_client_t *)malloc(sizeof(rcl_client_t)); + client->impl = NULL; + rcl_client_options_t client_ops = + rcl_client_get_default_options(); + + rcl_ret_t ret = + rcl_client_init(client, node, ts, service_name, &client_ops); + + *client_handle = (void *)client; + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index 71d5222c..f5daa294 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -33,4 +33,10 @@ int32_t RCLDOTNET_CDECL native_rcl_create_service_handle(void **service_handle, const char *service_name, void *typesupport); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_create_client_handle(void **client_handle, + void *node_handle, + const char *service_name, + void *typesupport); + #endif // RCLDOTNET_NODE_H From 0c2197798ea553e1833433bfc70a724c86b52f09 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 31 Aug 2021 15:34:15 +0200 Subject: [PATCH 16/73] Add client example --- rcldotnet_examples/CMakeLists.txt | 7 ++++ rcldotnet_examples/RCLDotnetClient.cs | 53 +++++++++++++++++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 rcldotnet_examples/RCLDotnetClient.cs diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index a3d2b1a2..d2383d34 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -40,9 +40,16 @@ add_dotnet_executable(rcldotnet_example_service INCLUDE_DLLS ${_assemblies_dep_dlls} ) +# avoid naming conflict with rcldotnet_client library +add_dotnet_executable(rcldotnet_example_client + RCLDotnetClient.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) install_dotnet(rcldotnet_talker DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_listener DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_example_service DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_client DESTINATION lib/${PROJECT_NAME}/dotnet) ament_package() diff --git a/rcldotnet_examples/RCLDotnetClient.cs b/rcldotnet_examples/RCLDotnetClient.cs new file mode 100644 index 00000000..f92218a0 --- /dev/null +++ b/rcldotnet_examples/RCLDotnetClient.cs @@ -0,0 +1,53 @@ +using System; +using System.Reflection; +using System.Runtime; +using System.Runtime.InteropServices; +using System.Threading; +using System.Threading.Tasks; +using ROS2; + +namespace ConsoleApplication +{ + public static class RCLDotnetClient + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("client"); + + var client = node.CreateClient("test_dotnet_service_name"); + + // TODO: (sh) WaitForService(timeout) + Thread.Sleep(2000); + + var request = new std_srvs.srv.SetBool_Request(); + request.Data = true; + + var task = client.SendRequestAsync(request); + + Console.WriteLine("Sent request."); + + while (RCLdotnet.Ok()) + { + RCLdotnet.SpinOnce(node, 500); + + if (task.IsCompletedSuccessfully) + { + var response = task.Result; + Console.WriteLine($"Service responded with: Success '{response.Success}' Message '{response.Message}'"); + break; + } + else if (task.IsFaulted) + { + Console.WriteLine($"Task faulted. Exception {task.Exception}"); + break; + } + else if (task.IsCanceled) + { + Console.WriteLine("Task canceld."); + break; + } + } + } + } +} From 262ffea8a262b2cd2a34bfb289a479c70e1741d3 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 31 Aug 2021 16:28:41 +0200 Subject: [PATCH 17/73] Add unittest for services --- rcldotnet/CMakeLists.txt | 1 + rcldotnet/test/test_services.cs | 105 ++++++++++++++++++++++++++++++++ 2 files changed, 106 insertions(+) create mode 100644 rcldotnet/test/test_services.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index c330b783..7de0fb64 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -104,6 +104,7 @@ if(BUILD_TESTING) add_dotnet_test(test_messages ${CS_SOURCES} test/test_messages.cs + test/test_services.cs INCLUDE_DLLS ${_assemblies_dep_dlls} INCLUDE_REFERENCES diff --git a/rcldotnet/test/test_services.cs b/rcldotnet/test/test_services.cs new file mode 100644 index 00000000..26153f79 --- /dev/null +++ b/rcldotnet/test/test_services.cs @@ -0,0 +1,105 @@ +using System; +using System.Collections.Generic; +using ROS2; +using ROS2.Utils; +using Xunit; + +namespace RCLdotnetTests +{ + public sealed class TestServices + { + [Fact] + public void TestServiceAndClient() + { + RCLdotnet.Init(); + var serviceNode = RCLdotnet.CreateNode("service_node"); + var clientNode = RCLdotnet.CreateNode("client_node"); + + test_msgs.srv.BasicTypes_Request serviceReceivedRequest = null; + test_msgs.srv.BasicTypes_Response clientReceivedResponse = null; + + var service = serviceNode.CreateService("unittest_dotnet_service", HandleRequest); + + var client = clientNode.CreateClient("unittest_dotnet_service"); + + var request = new test_msgs.srv.BasicTypes_Request(); + request.Bool_value = true; + request.Byte_value = 36; + request.Char_value = 37; + request.Float32_value = 38.1f; + request.Float64_value = 39.1; + request.Int8_value = 40; + request.Uint8_value = 41; + request.Int16_value = 42; + request.Uint16_value = 43; + request.Int32_value = 44; + request.Uint32_value = 45; + request.Int64_value = 46; + request.Uint64_value = 47; + request.String_value = "one"; + + var task = client.SendRequestAsync(request); + + while (clientReceivedResponse == null) + { + RCLdotnet.SpinOnce(serviceNode, 500); + RCLdotnet.SpinOnce(clientNode, 500); + + if (task.IsCompleted) + { + clientReceivedResponse = task.Result; + } + } + + Assert.True(serviceReceivedRequest.Bool_value); + Assert.Equal(36, serviceReceivedRequest.Byte_value); + Assert.Equal(37, serviceReceivedRequest.Char_value); + Assert.Equal(38.1f, serviceReceivedRequest.Float32_value); + Assert.Equal(39.1, serviceReceivedRequest.Float64_value); + Assert.Equal(40, serviceReceivedRequest.Int8_value); + Assert.Equal(41, serviceReceivedRequest.Uint8_value); + Assert.Equal(42, serviceReceivedRequest.Int16_value); + Assert.Equal(43, serviceReceivedRequest.Uint16_value); + Assert.Equal(44, serviceReceivedRequest.Int32_value); + Assert.Equal((uint)45, serviceReceivedRequest.Uint32_value); + Assert.Equal(46, serviceReceivedRequest.Int64_value); + Assert.Equal((ulong)47, serviceReceivedRequest.Uint64_value); + Assert.Equal("one", serviceReceivedRequest.String_value); + + Assert.True(clientReceivedResponse.Bool_value); + Assert.Equal(36, clientReceivedResponse.Byte_value); + Assert.Equal(37, clientReceivedResponse.Char_value); + Assert.Equal(38.1f, clientReceivedResponse.Float32_value); + Assert.Equal(39.1, clientReceivedResponse.Float64_value); + Assert.Equal(40, clientReceivedResponse.Int8_value); + Assert.Equal(41, clientReceivedResponse.Uint8_value); + Assert.Equal(42, clientReceivedResponse.Int16_value); + Assert.Equal(43, clientReceivedResponse.Uint16_value); + Assert.Equal(144, clientReceivedResponse.Int32_value); + Assert.Equal((uint)45, clientReceivedResponse.Uint32_value); + Assert.Equal(46, clientReceivedResponse.Int64_value); + Assert.Equal((ulong)47, clientReceivedResponse.Uint64_value); + Assert.Equal("one", clientReceivedResponse.String_value); + + void HandleRequest(test_msgs.srv.BasicTypes_Request request, test_msgs.srv.BasicTypes_Response response) + { + serviceReceivedRequest = request; + + response.Bool_value = request.Bool_value; + response.Byte_value = request.Byte_value; + response.Char_value = request.Char_value; + response.Float32_value = request.Float32_value; + response.Float64_value = request.Float64_value; + response.Int8_value = request.Int8_value; + response.Uint8_value = request.Uint8_value; + response.Int16_value = request.Int16_value; + response.Uint16_value = request.Uint16_value; + response.Int32_value = request.Int32_value + 100; + response.Uint32_value = request.Uint32_value; + response.Int64_value = request.Int64_value; + response.Uint64_value = request.Uint64_value; + response.String_value = request.String_value; + } + } + } +} From 680e3db9cbe657cbec7db71b6a2ffd17ddd99bfe Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 1 Sep 2021 11:07:42 +0200 Subject: [PATCH 18/73] Add Client.ServiceIsReady() mehtod --- rcldotnet/Client.cs | 33 +++++++++++++++++++++++---- rcldotnet/Node.cs | 2 +- rcldotnet/rcldotnet_client.c | 10 ++++++++ rcldotnet/rcldotnet_client.h | 3 +++ rcldotnet_examples/RCLDotnetClient.cs | 8 +++++-- 5 files changed, 49 insertions(+), 7 deletions(-) diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 37f7109a..1385014b 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -21,6 +21,12 @@ internal delegate int NativeRCLSendRequestType( internal static NativeRCLSendRequestType native_rcl_send_request = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate int NativeRCLServiceServerIsAvailableType( + IntPtr nodeHandle, IntPtr clientHandle, out bool isAvailable); + + internal static NativeRCLServiceServerIsAvailableType native_rcl_service_server_is_available = null; + static ClientDelegates() { dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -29,6 +35,10 @@ static ClientDelegates() IntPtr native_rcl_send_request_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "native_rcl_send_request"); ClientDelegates.native_rcl_send_request = (NativeRCLSendRequestType)Marshal.GetDelegateForFunctionPointer( native_rcl_send_request_ptr, typeof(NativeRCLSendRequestType)); + + IntPtr native_rcl_service_server_is_available_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "native_rcl_service_server_is_available"); + ClientDelegates.native_rcl_service_server_is_available = (NativeRCLServiceServerIsAvailableType)Marshal.GetDelegateForFunctionPointer( + native_rcl_service_server_is_available_ptr, typeof(NativeRCLServiceServerIsAvailableType)); } } @@ -36,7 +46,7 @@ public abstract class Client : ROS2.Interfaces.IDisposable { public abstract IntPtr Handle { get; } - public abstract IMessage CreateResponse(); + internal abstract IMessage CreateResponse(); internal abstract void HandleResponse(long sequenceNumber, IMessage response); } @@ -46,15 +56,28 @@ public sealed class Client : Client where TRequest : IMessage, new() where TResponse : IMessage, new() { - private ConcurrentDictionary _pendingRequests = new ConcurrentDictionary(); - public Client(IntPtr handle) + // ros2_java uses a WeakReference here. Not sure if its needed or not. + private readonly Node _node; + private readonly ConcurrentDictionary _pendingRequests = new ConcurrentDictionary(); + + internal Client(IntPtr handle, Node node) { Handle = handle; + _node = node; } public override IntPtr Handle { get; } - public override IMessage CreateResponse() => (IMessage)new TResponse(); + public bool ServiceIsReady() + { + var ret = (RCLRet)ClientDelegates.native_rcl_service_server_is_available(_node.Handle, Handle, out var serviceIsReady); + if (ret != RCLRet.Ok) + { + throw new Exception($"rcl_service_server_is_available() failed: {ret}"); + } + + return serviceIsReady; + } public Task SendRequestAsync(TRequest request) { @@ -80,6 +103,8 @@ public Task SendRequestAsync(TRequest request) return taskCompletionSource.Task; } + internal override IMessage CreateResponse() => (IMessage)new TResponse(); + internal override void HandleResponse(long sequenceNumber, IMessage response) { if (!_pendingRequests.TryRemove(sequenceNumber, out var pendingRequest)) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index dc23e717..794c0aea 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -164,7 +164,7 @@ public Client CreateClient(clientHandle); + var client = new Client(clientHandle, this); this.clients_.Add(client); return client; } diff --git a/rcldotnet/rcldotnet_client.c b/rcldotnet/rcldotnet_client.c index 248e032a..5529689d 100644 --- a/rcldotnet/rcldotnet_client.c +++ b/rcldotnet/rcldotnet_client.c @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include "rosidl_runtime_c/message_type_support_struct.h" @@ -31,3 +33,11 @@ int32_t native_rcl_send_request(void *client_handle, void *request_handle, int64 rcl_ret_t ret = rcl_send_request(client, request_handle, sequence_number); return ret; } + +int32_t native_rcl_service_server_is_available(void *node_handle, void *client_handle, bool *is_available) { + rcl_node_t * node = (rcl_node_t *)node_handle; + rcl_client_t * client = (rcl_client_t *)client_handle; + + rcl_ret_t ret = rcl_service_server_is_available(node, client, is_available); + return ret; +} diff --git a/rcldotnet/rcldotnet_client.h b/rcldotnet/rcldotnet_client.h index 1451ec8a..8e29773a 100644 --- a/rcldotnet/rcldotnet_client.h +++ b/rcldotnet/rcldotnet_client.h @@ -20,4 +20,7 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_send_request(void *client_handle, void *request_handle, int64_t *sequence_number); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_service_server_is_available(void *node_handle, void *client_handle, bool *is_available); + #endif // RCLDOTNET_CLIENT_H diff --git a/rcldotnet_examples/RCLDotnetClient.cs b/rcldotnet_examples/RCLDotnetClient.cs index f92218a0..08868ced 100644 --- a/rcldotnet_examples/RCLDotnetClient.cs +++ b/rcldotnet_examples/RCLDotnetClient.cs @@ -17,8 +17,12 @@ public static void Main(string[] args) var client = node.CreateClient("test_dotnet_service_name"); - // TODO: (sh) WaitForService(timeout) - Thread.Sleep(2000); + // TODO: (sh) Add WaitForService(timeout) method that observes the node graph. + Console.WriteLine("Waiting for service..."); + while (RCLdotnet.Ok() && !client.ServiceIsReady()) + { + Thread.Sleep(500); + } var request = new std_srvs.srv.SetBool_Request(); request.Data = true; From bd37a93ac19b34a452a0e53d1dfff5f7c7954f65 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Mon, 6 Sep 2021 15:57:49 +0200 Subject: [PATCH 19/73] Remove Interfaces that expose internal implementation details Most importent the `IntPtr Handle` property from `ROS2.Interfaces.IDisposable` should not be public. `ROS2.Interfaces.IDisposable` was missleading as well. (`System.IDisposable` is the well kown one) Made appropriate memers internal as well and added some sealed and static modifiers. This was done in preperation for adding SafeHandles to avoid memory leaks, as this would change the implementation detail that was made public by the interfaces. Abstractions such as interfaces have to be designed for extensability. See https://docs.microsoft.com/en-us/dotnet/standard/design-guidelines/abstractions-abstract-types-and-interfaces --- rcldotnet/CMakeLists.txt | 6 ---- rcldotnet/Client.cs | 19 ++++++---- rcldotnet/INode.cs | 42 ---------------------- rcldotnet/IPublisher.cs | 23 ------------ rcldotnet/IService.cs | 11 ------ rcldotnet/IServiceBase.cs | 13 ------- rcldotnet/ISubscription.cs | 21 ----------- rcldotnet/ISubscriptionBase.cs | 23 ------------ rcldotnet/Node.cs | 37 ++++++++++--------- rcldotnet/Publisher.cs | 25 +++++++------ rcldotnet/RCLdotnet.cs | 20 ++++------- rcldotnet/Service.cs | 37 ++++++++++++------- rcldotnet/Subscription.cs | 34 ++++++++++++------ rcldotnet/test/test_messages.cs | 47 +++++++++++-------------- rcldotnet/test/test_services.cs | 3 -- rcldotnet_common/Interfaces.cs | 9 ----- rcldotnet_examples/RCLDotnetListener.cs | 4 +-- rcldotnet_examples/RCLDotnetTalker.cs | 4 +-- 18 files changed, 126 insertions(+), 252 deletions(-) delete mode 100644 rcldotnet/INode.cs delete mode 100644 rcldotnet/IPublisher.cs delete mode 100644 rcldotnet/IService.cs delete mode 100644 rcldotnet/IServiceBase.cs delete mode 100644 rcldotnet/ISubscription.cs delete mode 100644 rcldotnet/ISubscriptionBase.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 7de0fb64..665ac3c6 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -27,12 +27,6 @@ endif() set(CS_SOURCES Client.cs - INode.cs - IPublisher.cs - IService.cs - IServiceBase.cs - ISubscription.cs - ISubscriptionBase.cs Node.cs Publisher.cs RCLdotnet.cs diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 1385014b..a0c10e48 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -1,9 +1,6 @@ using System; using System.Collections.Concurrent; -using System.Collections.Generic; -using System.Linq; using System.Runtime.InteropServices; -using System.Text; using System.Threading.Tasks; using ROS2.Common; using ROS2.Interfaces; @@ -11,7 +8,7 @@ namespace ROS2 { - internal class ClientDelegates + internal static class ClientDelegates { internal static readonly DllLoadUtils dllLoadUtils; @@ -42,9 +39,17 @@ static ClientDelegates() } } - public abstract class Client : ROS2.Interfaces.IDisposable + /// + /// Base class of a Client without generic type arguments for use in collections or so. + /// + public abstract class Client { - public abstract IntPtr Handle { get; } + // Only allow internal subclasses. + internal Client() + { + } + + internal abstract IntPtr Handle { get; } internal abstract IMessage CreateResponse(); @@ -66,7 +71,7 @@ internal Client(IntPtr handle, Node node) _node = node; } - public override IntPtr Handle { get; } + internal override IntPtr Handle { get; } public bool ServiceIsReady() { diff --git a/rcldotnet/INode.cs b/rcldotnet/INode.cs deleted file mode 100644 index 6ed71a30..00000000 --- a/rcldotnet/INode.cs +++ /dev/null @@ -1,42 +0,0 @@ -/* Copyright 2018 Esteve Fernandez - * - * 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.Collections.Generic; -using ROS2.Interfaces; - -namespace ROS2 { - public interface INode { - IList Subscriptions { get; } - - IList Services { get; } - - IList Clients { get; } - - IPublisher CreatePublisher (string topic) where T : IMessage; - - ISubscription CreateSubscription (string topic, Action callback) where T : IMessage, new (); - - Service CreateService(string topic, Action callback) - where TService : IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new(); - - Client CreateClient(string serviceName) - where TService : IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new(); - } -} diff --git a/rcldotnet/IPublisher.cs b/rcldotnet/IPublisher.cs deleted file mode 100644 index b3e00f95..00000000 --- a/rcldotnet/IPublisher.cs +++ /dev/null @@ -1,23 +0,0 @@ -/* Copyright 2018 Esteve Fernandez - * - * 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 ROS2.Interfaces; - -namespace ROS2 { - public interface IPublisher : IDisposable - where T : IMessage { - void Publish (T msg); - } -} diff --git a/rcldotnet/IService.cs b/rcldotnet/IService.cs deleted file mode 100644 index 39b214ae..00000000 --- a/rcldotnet/IService.cs +++ /dev/null @@ -1,11 +0,0 @@ -using ROS2.Interfaces; - -namespace ROS2 -{ - public interface IService : IServiceBase - where TService : IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new() - { - } -} diff --git a/rcldotnet/IServiceBase.cs b/rcldotnet/IServiceBase.cs deleted file mode 100644 index 40512f35..00000000 --- a/rcldotnet/IServiceBase.cs +++ /dev/null @@ -1,13 +0,0 @@ -using ROS2.Interfaces; - -namespace ROS2 -{ - public interface IServiceBase : ROS2.Interfaces.IDisposable - { - IMessage CreateRequest(); - - IMessage CreateResponse(); - - void TriggerCallback (IMessage request, IMessage response); - } -} diff --git a/rcldotnet/ISubscription.cs b/rcldotnet/ISubscription.cs deleted file mode 100644 index e211ec8f..00000000 --- a/rcldotnet/ISubscription.cs +++ /dev/null @@ -1,21 +0,0 @@ -/* Copyright 2018 Esteve Fernandez - * - * 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 ROS2.Interfaces; - -namespace ROS2 { - public interface ISubscription : ISubscriptionBase - where T : IMessage, new () { } -} diff --git a/rcldotnet/ISubscriptionBase.cs b/rcldotnet/ISubscriptionBase.cs deleted file mode 100644 index ddfc7f3a..00000000 --- a/rcldotnet/ISubscriptionBase.cs +++ /dev/null @@ -1,23 +0,0 @@ -/* Copyright 2018 Esteve Fernandez - * - * 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 ROS2.Interfaces; - -namespace ROS2 { - public interface ISubscriptionBase : IDisposable { - IMessage CreateMessage (); - void TriggerCallback (IMessage message); - } -} diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 794c0aea..0ff8fb81 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -15,19 +15,14 @@ using System; using System.Collections.Generic; -using System.Linq; using System.Reflection; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Text; -using System.Threading.Tasks; - using ROS2.Common; using ROS2.Interfaces; using ROS2.Utils; namespace ROS2 { - internal class NodeDelegates { + internal static class NodeDelegates { private static readonly DllLoadUtils dllLoadUtils; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] @@ -88,48 +83,54 @@ static NodeDelegates () { } } - public class Node : INode { + public sealed class Node { - private IList subscriptions_; + private IList subscriptions_; - private IList services_; + private IList services_; private IList clients_; - public Node (IntPtr handle) { + internal Node (IntPtr handle) { Handle = handle; - subscriptions_ = new List (); - services_ = new List(); + subscriptions_ = new List(); + services_ = new List(); clients_ = new List(); } - public IList Subscriptions { get { return subscriptions_; } } + public IList Subscriptions => subscriptions_; // TODO: (sh) wrap in readonly collection - public IList Services => services_; + // TODO: (sh) Add posibility to remove Subscriptions/Services/Clients from Node (if this is a thing in ROS) + // Now there is no Api to remove these Objects, and there is a strong reference from these fileds to them so they dont't get collected. + public IList Services => services_; public IList Clients => clients_; public IntPtr Handle { get; } - public IPublisher CreatePublisher (string topic) where T : IMessage { + public Publisher CreatePublisher (string topic) where T : IMessage { MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("_GET_TYPE_SUPPORT"); IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); IntPtr publisherHandle = IntPtr.Zero; RCLRet ret = (RCLRet) NodeDelegates.native_rcl_create_publisher_handle (ref publisherHandle, Handle, topic, typesupport); + + // TODO: (sh) Add topic as propety to Publisher. Publisher publisher = new Publisher (publisherHandle); return publisher; } - public ISubscription CreateSubscription (string topic, Action callback) where T : IMessage, new () { + public Subscription CreateSubscription (string topic, Action callback) where T : IMessage, new () { MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("_GET_TYPE_SUPPORT"); IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); IntPtr subscriptionHandle = IntPtr.Zero; RCLRet ret = (RCLRet) NodeDelegates.native_rcl_create_subscription_handle (ref subscriptionHandle, Handle, topic, typesupport); + + // TODO: (sh) Add topic as propety to Subscription. Subscription subscription = new Subscription (subscriptionHandle, callback); - this.subscriptions_.Add (subscription); + this.subscriptions_.Add(subscription); return subscription; } @@ -146,6 +147,7 @@ public Service CreateService(serviceHandle, callback); this.services_.Add(service); return service; @@ -164,6 +166,7 @@ public Client CreateClient(clientHandle, this); this.clients_.Add(client); return client; diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index 78342435..f9d8cf10 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -14,18 +14,12 @@ */ using System; -using System.Collections.Generic; -using System.Linq; -using System.Reflection; -using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -using System.Text; -using System.Threading.Tasks; using ROS2.Interfaces; using ROS2.Utils; namespace ROS2 { - internal class PublisherDelegates { + internal static class PublisherDelegates { internal static readonly DllLoadUtils dllLoadUtils; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] @@ -43,14 +37,25 @@ static PublisherDelegates () { } } - public class Publisher : IPublisher + /// + /// Base class of a Publisher without generic type arguments for use in collections or so. + /// + public abstract class Publisher + { + // Only allow internal subclasses. + internal Publisher() + { + } + } + + public sealed class Publisher : Publisher where T : IMessage { - public Publisher (IntPtr handle) { + internal Publisher (IntPtr handle) { Handle = handle; } - public IntPtr Handle { get; } + internal IntPtr Handle { get; } public void Publish (T msg) { IntPtr messageHandle = msg._CREATE_NATIVE_MESSAGE (); diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index a102c78b..8dd5bb67 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -14,19 +14,13 @@ */ using System; -using System.Collections.Concurrent; -using System.Collections.Generic; -using System.Linq; using System.Runtime.InteropServices; -using System.Text; -using System.Threading.Tasks; - using ROS2.Common; using ROS2.Interfaces; using ROS2.Utils; namespace ROS2 { - internal class RCLdotnetDelegates { + internal static class RCLdotnetDelegates { internal static readonly DllLoadUtils dllLoadUtils; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] @@ -251,7 +245,7 @@ static RCLdotnetDelegates () { } } - public class RCLdotnet { + public static class RCLdotnet { private static bool initialized = false; private static readonly object syncLock = new object (); @@ -270,7 +264,7 @@ public static Node CreateNode (string nodeName, string nodeNamespace) { return node; } - public static void Spin (INode node) { + public static void Spin (Node node) { while (Ok ()) { SpinOnce (node, 500); } @@ -382,7 +376,7 @@ private static bool TakeResponse(IntPtr clientHandle, IntPtr requestHeaderHandle return status; } - public static void SendResponse(IntPtr serviceHandle, IntPtr requestHeaderHandle, IMessage response) + private static void SendResponse(IntPtr serviceHandle, IntPtr requestHeaderHandle, IMessage response) { IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); response._WRITE_HANDLE (responseHandle); @@ -394,7 +388,7 @@ public static void SendResponse(IntPtr serviceHandle, IntPtr requestHeaderHandle response._DESTROY_NATIVE_MESSAGE (responseHandle); } - public static void SpinOnce (INode node, long timeout) { + public static void SpinOnce (Node node, long timeout) { IntPtr waitSetHandle = GetZeroInitializedWaitSet (); long numberOfSubscriptions = node.Subscriptions.Count; @@ -416,7 +410,7 @@ public static void SpinOnce (INode node, long timeout) { WaitSetClear (waitSetHandle); - foreach (ISubscriptionBase subscription in node.Subscriptions) { + foreach (Subscription subscription in node.Subscriptions) { WaitSetAddSubscription (waitSetHandle, subscription.Handle); } @@ -432,7 +426,7 @@ public static void SpinOnce (INode node, long timeout) { Wait (waitSetHandle, timeout); - foreach (ISubscriptionBase subscription in node.Subscriptions) { + foreach (Subscription subscription in node.Subscriptions) { IMessage message = subscription.CreateMessage (); bool result = Take (subscription.Handle, message); if (result) { diff --git a/rcldotnet/Service.cs b/rcldotnet/Service.cs index 8c30bd1a..b86cd379 100644 --- a/rcldotnet/Service.cs +++ b/rcldotnet/Service.cs @@ -1,34 +1,47 @@ using System; -using System.Collections.Generic; -using System.Linq; -using System.Runtime.InteropServices; -using System.Text; -using System.Threading.Tasks; using ROS2.Interfaces; -using ROS2.Utils; namespace ROS2 { - public sealed class Service : IService + /// + /// Base class of a Service without generic type arguments for use in collections or so. + /// + public abstract class Service + { + // Only allow internal subclasses. + internal Service() + { + } + + abstract internal IntPtr Handle { get; } + + abstract internal IMessage CreateRequest(); + + abstract internal IMessage CreateResponse(); + + abstract internal void TriggerCallback(IMessage request, IMessage response); + } + + public sealed class Service : Service where TService : IRosServiceDefinition where TRequest : IMessage, new() where TResponse : IMessage, new() { private Action _callback; - public Service(IntPtr handle, Action callback) + internal Service(IntPtr handle, Action callback) { Handle = handle; _callback = callback; } - public IntPtr Handle { get; } + internal override IntPtr Handle { get; } - public IMessage CreateRequest() => (IMessage)new TRequest(); + internal override IMessage CreateRequest() => (IMessage)new TRequest(); - public IMessage CreateResponse() => (IMessage)new TResponse(); + internal override IMessage CreateResponse() => (IMessage)new TResponse(); - public void TriggerCallback(IMessage request, IMessage response) + internal override void TriggerCallback(IMessage request, IMessage response) { _callback((TRequest)request, (TResponse)response); } diff --git a/rcldotnet/Subscription.cs b/rcldotnet/Subscription.cs index bf22a0c9..b9ce7e8b 100644 --- a/rcldotnet/Subscription.cs +++ b/rcldotnet/Subscription.cs @@ -14,32 +14,44 @@ */ using System; -using System.Collections.Generic; -using System.Linq; -using System.Runtime.InteropServices; -using System.Text; -using System.Threading.Tasks; using ROS2.Interfaces; -using ROS2.Utils; namespace ROS2 { - public class Subscription : ISubscription + + /// + /// Base class of a Subscription without generic type arguments for use in collections or so. + /// + public abstract class Subscription + { + // Only allow internal subclasses. + internal Subscription() + { + } + + internal abstract IntPtr Handle { get; } + + internal abstract IMessage CreateMessage(); + + internal abstract void TriggerCallback(IMessage message); + } + + public class Subscription : Subscription where T : IMessage, new () { private Action callback_; - public Subscription (IntPtr handle, Action callback) { + internal Subscription(IntPtr handle, Action callback) { Handle = handle; callback_ = callback; } - public IntPtr Handle { get; } + internal override IntPtr Handle { get; } - public IMessage CreateMessage () { + internal override IMessage CreateMessage() { IMessage msg = (IMessage) new T (); return msg; } - public void TriggerCallback (IMessage message) { + internal override void TriggerCallback(IMessage message) { callback_ ((T) message); } } diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index f486940c..f0668fcc 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -1,13 +1,6 @@ using System; using System.Collections.Generic; -using System.Reflection; -using System.Runtime; -using System.Runtime.InteropServices; -using System.Threading; - using ROS2; -using ROS2.Utils; - using Xunit; namespace RCLdotnetTests @@ -18,16 +11,16 @@ public class TestMessages public void TestPublishString() { RCLdotnet.Init (); - INode node_string_1 = RCLdotnet.CreateNode ("test_string_1"); - INode node_string_2 = RCLdotnet.CreateNode ("test_string_2"); - IPublisher chatter_pub = node_string_1.CreatePublisher ("topic_string"); + Node node_string_1 = RCLdotnet.CreateNode ("test_string_1"); + Node node_string_2 = RCLdotnet.CreateNode ("test_string_2"); + Publisher chatter_pub = node_string_1.CreatePublisher ("topic_string"); std_msgs.msg.String msg = new std_msgs.msg.String (); std_msgs.msg.String msg2 = new std_msgs.msg.String (); msg.Data = "Hello"; bool received=false; - ISubscription chatter_sub = node_string_2.CreateSubscription ( + Subscription chatter_sub = node_string_2.CreateSubscription ( "topic_string", rcv_msg => { received=true; @@ -49,9 +42,9 @@ public void TestPublishString() public void TestPublishBuiltins() { RCLdotnet.Init (); - INode node_builtins_1 = RCLdotnet.CreateNode ("test_builtins_1"); - INode node_builtins_2 = RCLdotnet.CreateNode ("test_builtins_2"); - IPublisher chatter_pub = node_builtins_1.CreatePublisher ("topic_builtins"); + Node node_builtins_1 = RCLdotnet.CreateNode ("test_builtins_1"); + Node node_builtins_2 = RCLdotnet.CreateNode ("test_builtins_2"); + Publisher chatter_pub = node_builtins_1.CreatePublisher ("topic_builtins"); test_msgs.msg.Builtins msg = new test_msgs.msg.Builtins (); test_msgs.msg.Builtins msg2 = new test_msgs.msg.Builtins (); @@ -61,7 +54,7 @@ public void TestPublishBuiltins() msg.Time_value.Nanosec = 4u; bool received=false; - ISubscription chatter_sub = node_builtins_2.CreateSubscription ( + Subscription chatter_sub = node_builtins_2.CreateSubscription ( "topic_builtins", rcv_msg => { received=true; @@ -87,9 +80,9 @@ public void TestPublishBuiltins() public void TestPublishArrays() { RCLdotnet.Init (); - INode node_array_1 = RCLdotnet.CreateNode ("test_arrays_1"); - INode node_array_2 = RCLdotnet.CreateNode ("test_arrays_2"); - IPublisher chatter_pub = node_array_1.CreatePublisher ("topic_array"); + Node node_array_1 = RCLdotnet.CreateNode ("test_arrays_1"); + Node node_array_2 = RCLdotnet.CreateNode ("test_arrays_2"); + Publisher chatter_pub = node_array_1.CreatePublisher ("topic_array"); test_msgs.msg.Arrays msg = new test_msgs.msg.Arrays (); test_msgs.msg.Arrays msg2 = new test_msgs.msg.Arrays (); @@ -215,7 +208,7 @@ public void TestPublishArrays() msg.Basic_types_values[2] = basic_type_3; bool received=false; - ISubscription chatter_sub = node_array_2.CreateSubscription ( + Subscription chatter_sub = node_array_2.CreateSubscription ( "topic_array", rcv_msg => { received = true; @@ -418,9 +411,9 @@ public void TestPublishArraysSizeCheckToMuch() public void TestPublishUnboundedSequences() { RCLdotnet.Init(); - INode node_array_1 = RCLdotnet.CreateNode("test_unbounded_sequences_1"); - INode node_array_2 = RCLdotnet.CreateNode("test_unbounded_sequences_2"); - IPublisher chatter_pub = node_array_1.CreatePublisher("topic_unbounded_sequences"); + Node node_array_1 = RCLdotnet.CreateNode("test_unbounded_sequences_1"); + Node node_array_2 = RCLdotnet.CreateNode("test_unbounded_sequences_2"); + Publisher chatter_pub = node_array_1.CreatePublisher("topic_unbounded_sequences"); test_msgs.msg.UnboundedSequences msg = new test_msgs.msg.UnboundedSequences(); test_msgs.msg.UnboundedSequences msg2 = new test_msgs.msg.UnboundedSequences(); @@ -545,7 +538,7 @@ public void TestPublishUnboundedSequences() msg.Basic_types_values.Add(basic_type_3); bool received=false; - ISubscription chatter_sub = node_array_2.CreateSubscription( + Subscription chatter_sub = node_array_2.CreateSubscription( "topic_unbounded_sequences", rcv_msg => { received=true; @@ -693,9 +686,9 @@ public void TestPublishUnboundedSequences() public void TestPublishBoundedSequences() { RCLdotnet.Init(); - INode node_array_1 = RCLdotnet.CreateNode("test_bounded_sequences_1"); - INode node_array_2 = RCLdotnet.CreateNode("test_bounded_sequences_2"); - IPublisher chatter_pub = node_array_1.CreatePublisher("topic_bounded_sequences"); + Node node_array_1 = RCLdotnet.CreateNode("test_bounded_sequences_1"); + Node node_array_2 = RCLdotnet.CreateNode("test_bounded_sequences_2"); + Publisher chatter_pub = node_array_1.CreatePublisher("topic_bounded_sequences"); test_msgs.msg.BoundedSequences msg = new test_msgs.msg.BoundedSequences(); test_msgs.msg.BoundedSequences msg2 = new test_msgs.msg.BoundedSequences(); @@ -820,7 +813,7 @@ public void TestPublishBoundedSequences() msg.Basic_types_values.Add(basic_type_3); bool received=false; - ISubscription chatter_sub = node_array_2.CreateSubscription( + Subscription chatter_sub = node_array_2.CreateSubscription( "topic_bounded_sequences", rcv_msg => { received=true; diff --git a/rcldotnet/test/test_services.cs b/rcldotnet/test/test_services.cs index 26153f79..e1743193 100644 --- a/rcldotnet/test/test_services.cs +++ b/rcldotnet/test/test_services.cs @@ -1,7 +1,4 @@ -using System; -using System.Collections.Generic; using ROS2; -using ROS2.Utils; using Xunit; namespace RCLdotnetTests diff --git a/rcldotnet_common/Interfaces.cs b/rcldotnet_common/Interfaces.cs index e67489bf..f3069c9b 100644 --- a/rcldotnet_common/Interfaces.cs +++ b/rcldotnet_common/Interfaces.cs @@ -17,8 +17,6 @@ namespace ROS2 { namespace Interfaces { - public interface IMessageStruct { } - public interface IMessage { // must be implemented on deriving types, gets called via reflection // (static abstract interface members are not supported yet.) @@ -38,12 +36,5 @@ public interface IRosServiceDefinition // (static abstract interface members are not supported yet.) // public static abstract IntPtr _GET_TYPE_SUPPORT(); } - - // TODO: (sh) Rename missleading IDisposable interface - public interface IDisposable { - IntPtr Handle { - get; - } - } } } diff --git a/rcldotnet_examples/RCLDotnetListener.cs b/rcldotnet_examples/RCLDotnetListener.cs index a85733c5..34a56589 100644 --- a/rcldotnet_examples/RCLDotnetListener.cs +++ b/rcldotnet_examples/RCLDotnetListener.cs @@ -11,9 +11,9 @@ public class RCLDotnetListener { public static void Main (string[] args) { RCLdotnet.Init (); - INode node = RCLdotnet.CreateNode ("listener"); + Node node = RCLdotnet.CreateNode ("listener"); - ISubscription chatter_sub = node.CreateSubscription ( + Subscription chatter_sub = node.CreateSubscription ( "chatter", msg => Console.WriteLine ("I heard: [" + msg.Data + "]")); RCLdotnet.Spin (node); diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index 9d366538..8744bf07 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -12,9 +12,9 @@ public class RCLDotnetTalker { public static void Main (string[] args) { RCLdotnet.Init (); - INode node = RCLdotnet.CreateNode ("talker"); + Node node = RCLdotnet.CreateNode ("talker"); - IPublisher chatter_pub = node.CreatePublisher ("chatter"); + Publisher chatter_pub = node.CreatePublisher ("chatter"); std_msgs.msg.String msg = new std_msgs.msg.String (); From a65e5e928ee79e800415bcb651ca5264f35dac34 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 8 Sep 2021 12:38:14 +0200 Subject: [PATCH 20/73] Add SafeHandles for Node and related types As of before this commit native recources for rcl_node_t, rcl_client_t, rcl_publisher_t, rcl_service_t and rcl_subscription_t didn't get released. Used a trick from dotnet/runtime to handle the requirement that node has to be released last. Also added some TODOs to check return values and for checks near allocations use the new throw helper. --- rcldotnet/CMakeLists.txt | 7 ++ rcldotnet/Client.cs | 15 ++- rcldotnet/Node.cs | 129 ++++++++++++++++++++----- rcldotnet/Publisher.cs | 15 ++- rcldotnet/RCLdotnet.cs | 89 +++++++++++------ rcldotnet/SafeClientHandle.cs | 77 +++++++++++++++ rcldotnet/SafeNodeHandle.cs | 33 +++++++ rcldotnet/SafePublisherHandle.cs | 77 +++++++++++++++ rcldotnet/SafeServiceHandle.cs | 77 +++++++++++++++ rcldotnet/SafeSubscriptionHandle.cs | 77 +++++++++++++++ rcldotnet/Service.cs | 11 ++- rcldotnet/Subscription.cs | 11 ++- rcldotnet/rcldotnet.c | 10 ++ rcldotnet/rcldotnet.h | 3 + rcldotnet/rcldotnet_node.c | 48 ++++++++- rcldotnet/rcldotnet_node.h | 12 +++ rcldotnet_common/CMakeLists.txt | 1 + rcldotnet_common/RCLExceptionHelper.cs | 36 +++++++ 18 files changed, 653 insertions(+), 75 deletions(-) create mode 100644 rcldotnet/SafeClientHandle.cs create mode 100644 rcldotnet/SafeNodeHandle.cs create mode 100644 rcldotnet/SafePublisherHandle.cs create mode 100644 rcldotnet/SafeServiceHandle.cs create mode 100644 rcldotnet/SafeSubscriptionHandle.cs create mode 100644 rcldotnet_common/RCLExceptionHelper.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 665ac3c6..4850e806 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -25,11 +25,18 @@ if(NOT WIN32) endif() endif() +set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") + set(CS_SOURCES Client.cs Node.cs Publisher.cs RCLdotnet.cs + SafeClientHandle.cs + SafeNodeHandle.cs + SafePublisherHandle.cs + SafeServiceHandle.cs + SafeSubscriptionHandle.cs Service.cs Subscription.cs ) diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index a0c10e48..b72b98d6 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -14,13 +14,13 @@ internal static class ClientDelegates [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate int NativeRCLSendRequestType( - IntPtr clientHandle, IntPtr requestHandle, out long seqneceNumber); + SafeClientHandle clientHandle, IntPtr requestHandle, out long seqneceNumber); internal static NativeRCLSendRequestType native_rcl_send_request = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate int NativeRCLServiceServerIsAvailableType( - IntPtr nodeHandle, IntPtr clientHandle, out bool isAvailable); + SafeNodeHandle nodeHandle, SafeClientHandle clientHandle, out bool isAvailable); internal static NativeRCLServiceServerIsAvailableType native_rcl_service_server_is_available = null; @@ -49,7 +49,12 @@ internal Client() { } - internal abstract IntPtr Handle { get; } + // Client does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the client is not live anymore. + internal abstract SafeClientHandle Handle { get; } internal abstract IMessage CreateResponse(); @@ -65,13 +70,13 @@ public sealed class Client : Client private readonly Node _node; private readonly ConcurrentDictionary _pendingRequests = new ConcurrentDictionary(); - internal Client(IntPtr handle, Node node) + internal Client(SafeClientHandle handle, Node node) { Handle = handle; _node = node; } - internal override IntPtr Handle { get; } + internal override SafeClientHandle Handle { get; } public bool ServiceIsReady() { diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 0ff8fb81..11045235 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -26,29 +26,53 @@ internal static class NodeDelegates { private static readonly DllLoadUtils dllLoadUtils; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLCreatePublisherHandleType ( - ref IntPtr publisherHandle, IntPtr nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); + internal delegate RCLRet NativeRCLCreatePublisherHandleType ( + ref SafePublisherHandle publisherHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); internal static NativeRCLCreatePublisherHandleType native_rcl_create_publisher_handle = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLCreateSubscriptionHandleType ( - ref IntPtr subscriptionHandle, IntPtr nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); + internal delegate RCLRet NativeRCLDestroyPublisherHandleType ( + IntPtr publisherHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLDestroyPublisherHandleType native_rcl_destroy_publisher_handle = null; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateSubscriptionHandleType ( + ref SafeSubscriptionHandle subscriptionHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); internal static NativeRCLCreateSubscriptionHandleType native_rcl_create_subscription_handle = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLCreateServiceHandleType ( - ref IntPtr serviceHandle, IntPtr nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); + internal delegate RCLRet NativeRCLDestroySubscriptionHandleType ( + IntPtr subscriptionHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLDestroySubscriptionHandleType native_rcl_destroy_subscription_handle = null; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateServiceHandleType ( + ref SafeServiceHandle serviceHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); internal static NativeRCLCreateServiceHandleType native_rcl_create_service_handle = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLCreateClientHandleType ( - ref IntPtr clientHandle, IntPtr nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); + internal delegate RCLRet NativeRCLDestroyServiceHandleType ( + IntPtr serviceHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLDestroyServiceHandleType native_rcl_destroy_service_handle = null; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateClientHandleType ( + ref SafeClientHandle clientHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); internal static NativeRCLCreateClientHandleType native_rcl_create_client_handle = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroyClientHandleType ( + IntPtr clientHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLDestroyClientHandleType native_rcl_destroy_client_handle = null; + static NodeDelegates () { dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); IntPtr nativelibrary = dllLoadUtils.LoadLibrary ("rcldotnet_node"); @@ -60,6 +84,13 @@ static NodeDelegates () { (NativeRCLCreatePublisherHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_create_publisher_handle_ptr, typeof (NativeRCLCreatePublisherHandleType)); + IntPtr native_rcl_destroy_publisher_handle_ptr = dllLoadUtils.GetProcAddress ( + nativelibrary, "native_rcl_destroy_publisher_handle"); + + NodeDelegates.native_rcl_destroy_publisher_handle = + (NativeRCLDestroyPublisherHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_publisher_handle_ptr, typeof (NativeRCLDestroyPublisherHandleType)); + IntPtr native_rcl_create_subscription_handle_ptr = dllLoadUtils.GetProcAddress ( nativelibrary, "native_rcl_create_subscription_handle"); @@ -67,6 +98,13 @@ static NodeDelegates () { (NativeRCLCreateSubscriptionHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_create_subscription_handle_ptr, typeof (NativeRCLCreateSubscriptionHandleType)); + IntPtr native_rcl_destroy_subscription_handle_ptr = dllLoadUtils.GetProcAddress ( + nativelibrary, "native_rcl_destroy_subscription_handle"); + + NodeDelegates.native_rcl_destroy_subscription_handle = + (NativeRCLDestroySubscriptionHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_subscription_handle_ptr, typeof (NativeRCLDestroySubscriptionHandleType)); + IntPtr native_rcl_create_service_handle_ptr = dllLoadUtils.GetProcAddress ( nativelibrary, "native_rcl_create_service_handle"); @@ -74,12 +112,26 @@ static NodeDelegates () { (NativeRCLCreateServiceHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_create_service_handle_ptr, typeof (NativeRCLCreateServiceHandleType)); + IntPtr native_rcl_destroy_service_handle_ptr = dllLoadUtils.GetProcAddress ( + nativelibrary, "native_rcl_destroy_service_handle"); + + NodeDelegates.native_rcl_destroy_service_handle = + (NativeRCLDestroyServiceHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_service_handle_ptr, typeof (NativeRCLDestroyServiceHandleType)); + IntPtr native_rcl_create_client_handle_ptr = dllLoadUtils.GetProcAddress ( nativelibrary, "native_rcl_create_client_handle"); NodeDelegates.native_rcl_create_client_handle = (NativeRCLCreateClientHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_create_client_handle_ptr, typeof (NativeRCLCreateClientHandleType)); + + IntPtr native_rcl_destroy_client_handle_ptr = dllLoadUtils.GetProcAddress ( + nativelibrary, "native_rcl_destroy_client_handle"); + + NodeDelegates.native_rcl_destroy_client_handle = + (NativeRCLDestroyClientHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_client_handle_ptr, typeof (NativeRCLDestroyClientHandleType)); } } @@ -91,7 +143,7 @@ public sealed class Node { private IList clients_; - internal Node (IntPtr handle) { + internal Node (SafeNodeHandle handle) { Handle = handle; subscriptions_ = new List(); services_ = new List(); @@ -107,14 +159,25 @@ internal Node (IntPtr handle) { public IList Clients => clients_; - public IntPtr Handle { get; } + // Node does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the node is not live anymore. + internal SafeNodeHandle Handle { get; } public Publisher CreatePublisher (string topic) where T : IMessage { MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("_GET_TYPE_SUPPORT"); - IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); - IntPtr publisherHandle = IntPtr.Zero; - RCLRet ret = (RCLRet) NodeDelegates.native_rcl_create_publisher_handle (ref publisherHandle, Handle, topic, typesupport); + + var publisherHandle = new SafePublisherHandle(); + RCLRet ret = NodeDelegates.native_rcl_create_publisher_handle (ref publisherHandle, Handle, topic, typesupport); + publisherHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + publisherHandle.Dispose(); + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_publisher_handle)}() failed."); + } // TODO: (sh) Add topic as propety to Publisher. Publisher publisher = new Publisher (publisherHandle); @@ -123,10 +186,16 @@ public Publisher CreatePublisher (string topic) where T : IMessage { public Subscription CreateSubscription (string topic, Action callback) where T : IMessage, new () { MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("_GET_TYPE_SUPPORT"); - IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); - IntPtr subscriptionHandle = IntPtr.Zero; - RCLRet ret = (RCLRet) NodeDelegates.native_rcl_create_subscription_handle (ref subscriptionHandle, Handle, topic, typesupport); + + var subscriptionHandle = new SafeSubscriptionHandle(); + RCLRet ret = NodeDelegates.native_rcl_create_subscription_handle (ref subscriptionHandle, Handle, topic, typesupport); + subscriptionHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + subscriptionHandle.Dispose(); + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_subscription_handle)}() failed."); + } // TODO: (sh) Add topic as propety to Subscription. Subscription subscription = new Subscription (subscriptionHandle, callback); @@ -142,11 +211,15 @@ public Service CreateService(serviceHandle, callback); this.services_.Add(service); @@ -161,11 +234,15 @@ public Client CreateClient(clientHandle, this); this.clients_.Add(client); diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index f9d8cf10..1d9a3d0a 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -24,7 +24,7 @@ internal static class PublisherDelegates { [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate void NativeRCLPublishType ( - IntPtr publisherHandle, IntPtr messageHandle); + SafePublisherHandle publisherHandle, IntPtr messageHandle); internal static NativeRCLPublishType native_rcl_publish = null; @@ -46,22 +46,31 @@ public abstract class Publisher internal Publisher() { } + + // Publisher does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the publisher is not live anymore. + internal abstract SafePublisherHandle Handle { get; } + } public sealed class Publisher : Publisher where T : IMessage { - internal Publisher (IntPtr handle) { + internal Publisher (SafePublisherHandle handle) { Handle = handle; } - internal IntPtr Handle { get; } + internal override SafePublisherHandle Handle { get; } public void Publish (T msg) { IntPtr messageHandle = msg._CREATE_NATIVE_MESSAGE (); msg._WRITE_HANDLE (messageHandle); + // TODO: (sh) Pass and handle return value. PublisherDelegates.native_rcl_publish (Handle, messageHandle); msg._DESTROY_NATIVE_MESSAGE (messageHandle); diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 8dd5bb67..dce6c889 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -24,7 +24,7 @@ internal static class RCLdotnetDelegates { internal static readonly DllLoadUtils dllLoadUtils; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLInitType (); + internal delegate RCLRet NativeRCLInitType (); internal static NativeRCLInitType native_rcl_init = null; @@ -34,11 +34,17 @@ internal static class RCLdotnetDelegates { internal static NativeRCLOkType native_rcl_ok = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - internal delegate int NativeRCLCreateNodeHandleType ( - ref IntPtr nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, [MarshalAs (UnmanagedType.LPStr)] string nodeNamespace); + internal delegate RCLRet NativeRCLCreateNodeHandleType ( + ref SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, [MarshalAs (UnmanagedType.LPStr)] string nodeNamespace); internal static NativeRCLCreateNodeHandleType native_rcl_create_node_handle = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + internal delegate RCLRet NativeRCLDestroyNodeHandleType ( + IntPtr nodeHandle); + + internal static NativeRCLDestroyNodeHandleType native_rcl_destroy_node_handle = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate IntPtr NativeRCLGetRMWIdentifierType (); @@ -50,7 +56,7 @@ internal delegate int NativeRCLCreateNodeHandleType ( internal static NativeRCLGetZeroInitializedWaitSetType native_rcl_get_zero_initialized_wait_set = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitSetInitType ( + internal delegate RCLRet NativeRCLWaitSetInitType ( IntPtr waitSetHandle, long numberOfSubscriptions, long numberOfGuardConditions, long numberOfTimers, long numberOfClients, long numberOfServices, long numberOfEvents); @@ -62,19 +68,18 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLWaitSetClearType native_rcl_wait_set_clear = null; - // TODO: (sh) fix return type [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate void NativeRCLWaitSetAddSubscriptionType (IntPtr waitSetHandle, IntPtr subscriptionHandle); + internal delegate RCLRet NativeRCLWaitSetAddSubscriptionType (IntPtr waitSetHandle, SafeSubscriptionHandle subscriptionHandle); internal static NativeRCLWaitSetAddSubscriptionType native_rcl_wait_set_add_subscription = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitSetAddServiceType (IntPtr waitSetHandle, IntPtr serviceHandle); + internal delegate RCLRet NativeRCLWaitSetAddServiceType (IntPtr waitSetHandle, SafeServiceHandle serviceHandle); internal static NativeRCLWaitSetAddServiceType native_rcl_wait_set_add_service = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitSetAddClientType (IntPtr waitSetHandle, IntPtr clientHandle); + internal delegate RCLRet NativeRCLWaitSetAddClientType (IntPtr waitSetHandle, SafeClientHandle clientHandle); internal static NativeRCLWaitSetAddClientType native_rcl_wait_set_add_client = null; @@ -84,12 +89,12 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLDestroyWaitSetType native_rcl_destroy_wait_set = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitType (IntPtr waitSetHandle, long timeout); + internal delegate RCLRet NativeRCLWaitType (IntPtr waitSetHandle, long timeout); internal static NativeRCLWaitType native_rcl_wait = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLTakeType (IntPtr subscriptionHandle, IntPtr messageHandle); + internal delegate RCLRet NativeRCLTakeType (SafeSubscriptionHandle subscriptionHandle, IntPtr messageHandle); internal static NativeRCLTakeType native_rcl_take = null; @@ -109,17 +114,17 @@ internal delegate int NativeRCLWaitSetInitType ( internal static NativeRCLRequestHeaderGetSequenceNumberType native_rcl_request_header_get_sequence_number = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLTakeRequestType (IntPtr serviceHandle, IntPtr requestHeaderHandle, IntPtr requestHandle); + internal delegate RCLRet NativeRCLTakeRequestType (SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IntPtr requestHandle); internal static NativeRCLTakeRequestType native_rcl_take_request = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLSendResponseType (IntPtr serviceHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); + internal delegate RCLRet NativeRCLSendResponseType (SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); internal static NativeRCLSendResponseType native_rcl_send_response = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLTakeResponseType (IntPtr clientHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); + internal delegate RCLRet NativeRCLTakeResponseType (SafeClientHandle clientHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); internal static NativeRCLTakeResponseType native_rcl_take_response = null; @@ -152,6 +157,12 @@ static RCLdotnetDelegates () { (NativeRCLCreateNodeHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_create_node_handle_ptr, typeof (NativeRCLCreateNodeHandleType)); + IntPtr native_rcl_destroy_node_handle_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_node_handle"); + RCLdotnetDelegates.native_rcl_destroy_node_handle = + (NativeRCLDestroyNodeHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_node_handle_ptr, typeof (NativeRCLDestroyNodeHandleType)); + IntPtr native_rcl_get_zero_initialized_wait_set_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_get_zero_initialized_wait_set"); RCLdotnetDelegates.native_rcl_get_zero_initialized_wait_set = @@ -258,8 +269,14 @@ public static Node CreateNode (string nodeName) { } public static Node CreateNode (string nodeName, string nodeNamespace) { - IntPtr nodeHandle = IntPtr.Zero; - int ret = RCLdotnetDelegates.native_rcl_create_node_handle (ref nodeHandle, nodeName, nodeNamespace); + var nodeHandle = new SafeNodeHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_node_handle (ref nodeHandle, nodeName, nodeNamespace); + if (ret != RCLRet.Ok) + { + nodeHandle.Dispose(); + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_node_handle)}() failed."); + } + Node node = new Node (nodeHandle); return node; } @@ -291,16 +308,19 @@ private static void WaitSetClear (IntPtr waitSetHandle) { RCLdotnetDelegates.native_rcl_wait_set_clear (waitSetHandle); } - private static void WaitSetAddSubscription (IntPtr waitSetHandle, IntPtr subscriptionHandle) { - RCLdotnetDelegates.native_rcl_wait_set_add_subscription (waitSetHandle, subscriptionHandle); + private static void WaitSetAddSubscription (IntPtr waitSetHandle, SafeSubscriptionHandle subscriptionHandle) { + // TODO: (sh) Handle return value + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_subscription (waitSetHandle, subscriptionHandle); } - private static void WaitSetAddService (IntPtr waitSetHandle, IntPtr serviceHandle) { - RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); + private static void WaitSetAddService (IntPtr waitSetHandle, SafeServiceHandle serviceHandle) { + // TODO: (sh) Handle return value + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); } - private static void WaitSetAddClient (IntPtr waitSetHandle, IntPtr clientHandle) { - RCLdotnetDelegates.native_rcl_wait_set_add_client(waitSetHandle, clientHandle); + private static void WaitSetAddClient (IntPtr waitSetHandle, SafeClientHandle clientHandle) { + // TODO: (sh) Handle return value + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_client(waitSetHandle, clientHandle); } private static void DestroyWaitSet (IntPtr waitSetHandle) { @@ -309,14 +329,16 @@ private static void DestroyWaitSet (IntPtr waitSetHandle) { private static void Wait (IntPtr waitSetHandle, long timeout) { long ns_timeout = timeout * 1000000; - RCLRet ret = (RCLRet) RCLdotnetDelegates.native_rcl_wait (waitSetHandle, ns_timeout); + RCLRet ret = RCLdotnetDelegates.native_rcl_wait (waitSetHandle, ns_timeout); // TODO(esteve): do something with ret } - private static bool Take (IntPtr subscriptionHandle, IMessage message) { + private static bool Take (SafeSubscriptionHandle subscriptionHandle, IMessage message) { bool status = false; IntPtr messageHandle = message._CREATE_NATIVE_MESSAGE (); - RCLRet ret = (RCLRet) RCLdotnetDelegates.native_rcl_take (subscriptionHandle, messageHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_take (subscriptionHandle, messageHandle); + + // TODO: (sh) Handle all return values (exceptions) switch (ret) { case RCLRet.Ok: message._READ_HANDLE (messageHandle); @@ -332,10 +354,12 @@ private static bool Take (IntPtr subscriptionHandle, IMessage message) { return status; } - private static bool TakeRequest(IntPtr serviceHandle, IntPtr requestHeaderHandle, IMessage request) { + private static bool TakeRequest(SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IMessage request) { bool status = false; IntPtr requestHandle = request._CREATE_NATIVE_MESSAGE(); - RCLRet ret = (RCLRet)RCLdotnetDelegates.native_rcl_take_request(serviceHandle, requestHeaderHandle, requestHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(serviceHandle, requestHeaderHandle, requestHandle); + + // TODO: (sh) Handle all return values (exceptions) switch (ret) { case RCLRet.Ok: @@ -354,10 +378,12 @@ private static bool TakeRequest(IntPtr serviceHandle, IntPtr requestHeaderHandle return status; } - private static bool TakeResponse(IntPtr clientHandle, IntPtr requestHeaderHandle, IMessage response) { + private static bool TakeResponse(SafeClientHandle clientHandle, IntPtr requestHeaderHandle, IMessage response) { bool status = false; IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); - RCLRet ret = (RCLRet)RCLdotnetDelegates.native_rcl_take_response(clientHandle, requestHeaderHandle, responseHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(clientHandle, requestHeaderHandle, responseHandle); + + // TODO: (sh) Handle all return values (exceptions) switch (ret) { case RCLRet.Ok: @@ -376,13 +402,13 @@ private static bool TakeResponse(IntPtr clientHandle, IntPtr requestHeaderHandle return status; } - private static void SendResponse(IntPtr serviceHandle, IntPtr requestHeaderHandle, IMessage response) + private static void SendResponse(SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IMessage response) { IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); response._WRITE_HANDLE (responseHandle); // TODO: (sh) check return values - RCLRet ret = (RCLRet)RCLdotnetDelegates.native_rcl_send_response(serviceHandle, requestHeaderHandle, responseHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(serviceHandle, requestHeaderHandle, responseHandle); // TODO: (sh) don't leak memory on exceptions response._DESTROY_NATIVE_MESSAGE (responseHandle); @@ -473,7 +499,8 @@ public static void SpinOnce (Node node, long timeout) { public static void Init () { lock (syncLock) { if (!initialized) { - RCLdotnetDelegates.native_rcl_init (); + // TODO: (sh) Handle return value + RCLRet ret = RCLdotnetDelegates.native_rcl_init (); initialized = true; } } diff --git a/rcldotnet/SafeClientHandle.cs b/rcldotnet/SafeClientHandle.cs new file mode 100644 index 00000000..a59b7acc --- /dev/null +++ b/rcldotnet/SafeClientHandle.cs @@ -0,0 +1,77 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; +using ROS2.Common; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_client_t + /// + /// + /// Since we need to delete the client handle before the node is released we need to actually hold a + /// pointer to a rcl_client_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the client handle and node handle. This also applies to publisher, subscription and service handles, + /// which point to a rcl_publisher_t, rcl_subscription_t or rcl_service_t. + /// + internal sealed class SafeClientHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_client_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeClientHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_destroy_client_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/SafeNodeHandle.cs b/rcldotnet/SafeNodeHandle.cs new file mode 100644 index 00000000..8aabc0b5 --- /dev/null +++ b/rcldotnet/SafeNodeHandle.cs @@ -0,0 +1,33 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; +using ROS2.Common; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_node_t + /// + /// + /// SafePublisherHandle, SafeSubscriptionHandle, SafeServiceHandle and SafeClientHandle + /// all increase the refCount of their parrent SafeNodeHandles. + /// This means that the Node does not immediately get released if there is still + /// one of those alive. + /// + internal sealed class SafeNodeHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeNodeHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_destroy_node_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/SafePublisherHandle.cs b/rcldotnet/SafePublisherHandle.cs new file mode 100644 index 00000000..6fe7a132 --- /dev/null +++ b/rcldotnet/SafePublisherHandle.cs @@ -0,0 +1,77 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; +using ROS2.Common; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_publisher_t + /// + /// + /// Since we need to delete the publisher handle before the node is released we need to actually hold a + /// pointer to a rcl_publisher_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the publisher handle and node handle. This also applies to subscription, service and client handles, + /// which point to a rcl_subscription_t, rcl_service_t or rcl_client_t. + /// + internal sealed class SafePublisherHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_publisher_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafePublisherHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_destroy_publisher_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/SafeServiceHandle.cs b/rcldotnet/SafeServiceHandle.cs new file mode 100644 index 00000000..94878797 --- /dev/null +++ b/rcldotnet/SafeServiceHandle.cs @@ -0,0 +1,77 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; +using ROS2.Common; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_service_t + /// + /// + /// Since we need to delete the service handle before the node is released we need to actually hold a + /// pointer to a rcl_service_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the service handle and node handle. This also applies to publisher, subscription and client handles, + /// which point to a rcl_publisher_t, rcl_subscription_t or rcl_client_t. + /// + internal sealed class SafeServiceHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_service_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeServiceHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_destroy_service_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/SafeSubscriptionHandle.cs b/rcldotnet/SafeSubscriptionHandle.cs new file mode 100644 index 00000000..eae81db7 --- /dev/null +++ b/rcldotnet/SafeSubscriptionHandle.cs @@ -0,0 +1,77 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; +using ROS2.Common; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_subscription_t + /// + /// + /// Since we need to delete the subscription handle before the node is released we need to actually hold a + /// pointer to a rcl_subscription_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the subscription handle and node handle. This also applies to publisher, service and client handles, + /// which point to a rcl_publisher_t, rcl_service_t or rcl_client_t. + /// + internal sealed class SafeSubscriptionHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_subscription_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeSubscriptionHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_destroy_subscription_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/Service.cs b/rcldotnet/Service.cs index b86cd379..6add4c97 100644 --- a/rcldotnet/Service.cs +++ b/rcldotnet/Service.cs @@ -13,7 +13,12 @@ internal Service() { } - abstract internal IntPtr Handle { get; } + // Service does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the service is not live anymore. + abstract internal SafeServiceHandle Handle { get; } abstract internal IMessage CreateRequest(); @@ -29,13 +34,13 @@ public sealed class Service : Service { private Action _callback; - internal Service(IntPtr handle, Action callback) + internal Service(SafeServiceHandle handle, Action callback) { Handle = handle; _callback = callback; } - internal override IntPtr Handle { get; } + internal override SafeServiceHandle Handle { get; } internal override IMessage CreateRequest() => (IMessage)new TRequest(); diff --git a/rcldotnet/Subscription.cs b/rcldotnet/Subscription.cs index b9ce7e8b..1b149e52 100644 --- a/rcldotnet/Subscription.cs +++ b/rcldotnet/Subscription.cs @@ -28,7 +28,12 @@ internal Subscription() { } - internal abstract IntPtr Handle { get; } + // Subscription does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the subscription is not live anymore. + internal abstract SafeSubscriptionHandle Handle { get; } internal abstract IMessage CreateMessage(); @@ -39,12 +44,12 @@ public class Subscription : Subscription where T : IMessage, new () { private Action callback_; - internal Subscription(IntPtr handle, Action callback) { + internal Subscription(SafeSubscriptionHandle handle, Action callback) { Handle = handle; callback_ = callback; } - internal override IntPtr Handle { get; } + internal override SafeSubscriptionHandle Handle { get; } internal override IMessage CreateMessage() { IMessage msg = (IMessage) new T (); diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 2a0a6756..f6979fd8 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -63,6 +63,15 @@ int32_t native_rcl_create_node_handle(void **node_handle, const char *name, cons return ret; } +int32_t native_rcl_destroy_node_handle(void *node_handle) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_node_fini(node); + free(node); + + return ret; +} + void *native_rcl_get_zero_initialized_wait_set() { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)malloc(sizeof(rcl_wait_set_t)); *wait_set = rcl_get_zero_initialized_wait_set(); @@ -119,6 +128,7 @@ int32_t native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handl } void native_rcl_destroy_wait_set(void *wait_set_handle) { + // TODO: (sh) MemoryLeak: call rcl_wait_set_fini to free all pointers in the rcl_wait_set_t type. free((rcl_wait_set_t *)wait_set_handle); } diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index b5b739ab..b0fee841 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -29,6 +29,9 @@ bool RCLDOTNET_CDECL native_rcl_ok(); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_node_handle(void **, const char *, const char *); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_node_handle(void *node_handle); + RCLDOTNET_EXPORT void * RCLDOTNET_CDECL native_rcl_get_zero_initialized_wait_set(); diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index 52bbc3cf..40879caa 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -35,7 +35,7 @@ int32_t native_rcl_create_publisher_handle(void **publisher_handle, rcl_publisher_t *publisher = (rcl_publisher_t *)malloc(sizeof(rcl_publisher_t)); - publisher->impl = NULL; + *publisher = rcl_get_zero_initialized_publisher(); rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); rcl_ret_t ret = @@ -46,6 +46,16 @@ int32_t native_rcl_create_publisher_handle(void **publisher_handle, return ret; } +int32_t native_rcl_destroy_publisher_handle(void *publisher_handle, void *node_handle) { + rcl_publisher_t *publisher = (rcl_publisher_t *)publisher_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_publisher_fini(publisher, node); + free(publisher); + + return ret; +} + int32_t native_rcl_create_subscription_handle(void **subscription_handle, void *node_handle, const char *topic, @@ -57,7 +67,7 @@ int32_t native_rcl_create_subscription_handle(void **subscription_handle, rcl_subscription_t *subscription = (rcl_subscription_t *)malloc(sizeof(rcl_subscription_t)); - subscription->impl = NULL; + *subscription = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); @@ -69,6 +79,16 @@ int32_t native_rcl_create_subscription_handle(void **subscription_handle, return ret; } +int32_t native_rcl_destroy_subscription_handle(void *subscription_handle, void *node_handle) { + rcl_subscription_t *subscription = (rcl_subscription_t *)subscription_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_subscription_fini(subscription, node); + free(subscription); + + return ret; +} + int32_t native_rcl_create_service_handle(void **service_handle, void *node_handle, const char *service_name, @@ -80,7 +100,7 @@ int32_t native_rcl_create_service_handle(void **service_handle, rcl_service_t *service = (rcl_service_t *)malloc(sizeof(rcl_service_t)); - service->impl = NULL; + *service = rcl_get_zero_initialized_service(); rcl_service_options_t service_ops = rcl_service_get_default_options(); @@ -92,6 +112,16 @@ int32_t native_rcl_create_service_handle(void **service_handle, return ret; } +int32_t native_rcl_destroy_service_handle(void *service_handle, void *node_handle) { + rcl_service_t *service = (rcl_service_t *)service_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_service_fini(service, node); + free(service); + + return ret; +} + int32_t native_rcl_create_client_handle(void **client_handle, void *node_handle, const char *service_name, @@ -103,7 +133,7 @@ int32_t native_rcl_create_client_handle(void **client_handle, rcl_client_t *client = (rcl_client_t *)malloc(sizeof(rcl_client_t)); - client->impl = NULL; + *client = rcl_get_zero_initialized_client(); rcl_client_options_t client_ops = rcl_client_get_default_options(); @@ -114,3 +144,13 @@ int32_t native_rcl_create_client_handle(void **client_handle, return ret; } + +int32_t native_rcl_destroy_client_handle(void *client_handle, void *node_handle) { + rcl_client_t *client = (rcl_client_t *)client_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_client_fini(client, node); + free(client); + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index f5daa294..43523984 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -22,21 +22,33 @@ int32_t RCLDOTNET_CDECL native_rcl_create_publisher_handle(void **, void *, const char *, void *); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_publisher_handle(void *publisher_handle, void *node_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_subscription_handle(void **, void *, const char *, void *); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_subscription_handle(void *subscription_handle, void *node_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_service_handle(void **service_handle, void *node_handle, const char *service_name, void *typesupport); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_service_handle(void *service_handle, void *node_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_client_handle(void **client_handle, void *node_handle, const char *service_name, void *typesupport); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_client_handle(void *client_handle, void *node_handle); + #endif // RCLDOTNET_NODE_H diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index de30cc41..18057004 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(DotNETExtra REQUIRED) set(CS_SOURCES DllLoadUtils.cs Interfaces.cs + RCLExceptionHelper.cs RCLRet.cs ) diff --git a/rcldotnet_common/RCLExceptionHelper.cs b/rcldotnet_common/RCLExceptionHelper.cs new file mode 100644 index 00000000..e8df7299 --- /dev/null +++ b/rcldotnet_common/RCLExceptionHelper.cs @@ -0,0 +1,36 @@ +using System; + +namespace ROS2.Common +{ + // TODO: (sh) Check return values to throw the appropirate Exceptions arcording + // to the Framework Design Guidlines. + // https://docs.microsoft.com/en-us/dotnet/standard/design-guidelines/exceptions + + // TODO: (sh) Use rcl_get_error_state() for additional information. + public static class RCLExceptionHelper + { + public static void ThrowFromReturnValue(RCLRet value, string messagePrefix = null) + { + if (value == RCLRet.Ok) + { + throw new ArgumentException($"The return value must not be Ok.", nameof(value)); + } + + var message = messagePrefix == null + ? value.ToString() + : messagePrefix + " " + value.ToString(); + + throw new Exception(message); + } + + public static void CheckReturnValue(RCLRet value, string messagePrefix = null) + { + if (value == RCLRet.Ok) + { + return; + } + + ThrowFromReturnValue(value, messagePrefix); + } + } +} From 3b3350460334b6fd5b5ed34aa927c6d0ad9afa19 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 8 Sep 2021 16:27:48 +0200 Subject: [PATCH 21/73] Add SafeHandles for request id and wait set - Refactored and renamed some methods to be more consistent. - Added some error handling. - Don't try to take things if the wait set did return a timeout. --- rcldotnet/CMakeLists.txt | 2 + rcldotnet/RCLdotnet.cs | 323 +++++++++++++++++-------------- rcldotnet/SafeRequestIdHandle.cs | 27 +++ rcldotnet/SafeWaitSetHandle.cs | 27 +++ rcldotnet/rcldotnet.c | 66 +++---- rcldotnet/rcldotnet.h | 32 ++- 6 files changed, 276 insertions(+), 201 deletions(-) create mode 100644 rcldotnet/SafeRequestIdHandle.cs create mode 100644 rcldotnet/SafeWaitSetHandle.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 4850e806..0bd5be57 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -35,8 +35,10 @@ set(CS_SOURCES SafeClientHandle.cs SafeNodeHandle.cs SafePublisherHandle.cs + SafeRequestIdHandle.cs SafeServiceHandle.cs SafeSubscriptionHandle.cs + SafeWaitSetHandle.cs Service.cs Subscription.cs ) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index dce6c889..ea8a3341 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -51,45 +51,40 @@ internal delegate RCLRet NativeRCLDestroyNodeHandleType ( internal static NativeRCLGetRMWIdentifierType native_rcl_get_rmw_identifier = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate IntPtr NativeRCLGetZeroInitializedWaitSetType (); + internal delegate RCLRet NativeRCLCreateWaitSetHandleType ( + ref SafeWaitSetHandle waitSetHandle, int numberOfSubscriptions, + int numberOfGuardConditions, int numberOfTimers, + int numberOfClients, int numberOfServices, int numberOfEvents); - internal static NativeRCLGetZeroInitializedWaitSetType native_rcl_get_zero_initialized_wait_set = null; + internal static NativeRCLCreateWaitSetHandleType native_rcl_create_wait_set_handle = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetInitType ( - IntPtr waitSetHandle, long numberOfSubscriptions, - long numberOfGuardConditions, long numberOfTimers, - long numberOfClients, long numberOfServices, long numberOfEvents); + internal delegate RCLRet NativeRCLDestroyWaitSetType (IntPtr waitSetHandle); - internal static NativeRCLWaitSetInitType native_rcl_wait_set_init = null; + internal static NativeRCLDestroyWaitSetType native_rcl_destroy_wait_set_handle = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate void NativeRCLWaitSetClearType (IntPtr waitSetHandle); + internal delegate RCLRet NativeRCLWaitSetClearType (SafeWaitSetHandle waitSetHandle); internal static NativeRCLWaitSetClearType native_rcl_wait_set_clear = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetAddSubscriptionType (IntPtr waitSetHandle, SafeSubscriptionHandle subscriptionHandle); + internal delegate RCLRet NativeRCLWaitSetAddSubscriptionType (SafeWaitSetHandle waitSetHandle, SafeSubscriptionHandle subscriptionHandle); internal static NativeRCLWaitSetAddSubscriptionType native_rcl_wait_set_add_subscription = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetAddServiceType (IntPtr waitSetHandle, SafeServiceHandle serviceHandle); + internal delegate RCLRet NativeRCLWaitSetAddServiceType (SafeWaitSetHandle waitSetHandle, SafeServiceHandle serviceHandle); internal static NativeRCLWaitSetAddServiceType native_rcl_wait_set_add_service = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetAddClientType (IntPtr waitSetHandle, SafeClientHandle clientHandle); + internal delegate RCLRet NativeRCLWaitSetAddClientType (SafeWaitSetHandle waitSetHandle, SafeClientHandle clientHandle); internal static NativeRCLWaitSetAddClientType native_rcl_wait_set_add_client = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate void NativeRCLDestroyWaitSetType (IntPtr waitSetHandle); - - internal static NativeRCLDestroyWaitSetType native_rcl_destroy_wait_set = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitType (IntPtr waitSetHandle, long timeout); + internal delegate RCLRet NativeRCLWaitType (SafeWaitSetHandle waitSetHandle, long timeout); internal static NativeRCLWaitType native_rcl_wait = null; @@ -99,32 +94,33 @@ internal delegate RCLRet NativeRCLWaitSetInitType ( internal static NativeRCLTakeType native_rcl_take = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate IntPtr NativeRCLCreateRequestHeaderHandleType (); + internal delegate RCLRet NativeRCLCreateRequestIdHandleType ( + ref SafeRequestIdHandle requestIdHandle); - internal static NativeRCLCreateRequestHeaderHandleType native_rcl_create_request_header_handle = null; + internal static NativeRCLCreateRequestIdHandleType native_rcl_create_request_id_handle = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate void NativeRCLDestroyRequestHeaderHandleType (IntPtr requestHeaderHandle); + internal delegate RCLRet NativeRCLDestroyRequestIdHandleType (IntPtr requestIdHandle); - internal static NativeRCLDestroyRequestHeaderHandleType native_rcl_destroy_request_header_handle = null; + internal static NativeRCLDestroyRequestIdHandleType native_rcl_destroy_request_id_handle = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate long NativeRCLRequestHeaderGetSequenceNumberType (IntPtr requestHeaderHandle); + internal delegate long NativeRCLRequestIdGetSequenceNumberType (SafeRequestIdHandle requestIdHandle); - internal static NativeRCLRequestHeaderGetSequenceNumberType native_rcl_request_header_get_sequence_number = null; + internal static NativeRCLRequestIdGetSequenceNumberType native_rcl_request_id_get_sequence_number = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeRequestType (SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IntPtr requestHandle); + internal delegate RCLRet NativeRCLTakeRequestType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IntPtr requestHandle); internal static NativeRCLTakeRequestType native_rcl_take_request = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLSendResponseType (SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); + internal delegate RCLRet NativeRCLSendResponseType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IntPtr responseHandle); internal static NativeRCLSendResponseType native_rcl_send_response = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeResponseType (SafeClientHandle clientHandle, IntPtr requestHeaderHandle, IntPtr responseHandle); + internal delegate RCLRet NativeRCLTakeResponseType (SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, IntPtr responseHandle); internal static NativeRCLTakeResponseType native_rcl_take_response = null; @@ -163,17 +159,17 @@ static RCLdotnetDelegates () { (NativeRCLDestroyNodeHandleType) Marshal.GetDelegateForFunctionPointer ( native_rcl_destroy_node_handle_ptr, typeof (NativeRCLDestroyNodeHandleType)); - IntPtr native_rcl_get_zero_initialized_wait_set_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_get_zero_initialized_wait_set"); - RCLdotnetDelegates.native_rcl_get_zero_initialized_wait_set = - (NativeRCLGetZeroInitializedWaitSetType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_get_zero_initialized_wait_set_ptr, typeof (NativeRCLGetZeroInitializedWaitSetType)); + IntPtr native_rcl_create_wait_set_handle_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_create_wait_set_handle"); + RCLdotnetDelegates.native_rcl_create_wait_set_handle = + (NativeRCLCreateWaitSetHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_create_wait_set_handle_ptr, typeof (NativeRCLCreateWaitSetHandleType)); - IntPtr native_rcl_wait_set_init_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_init"); - RCLdotnetDelegates.native_rcl_wait_set_init = - (NativeRCLWaitSetInitType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_wait_set_init_ptr, typeof (NativeRCLWaitSetInitType)); + IntPtr native_rcl_destroy_wait_set_handle_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_wait_set_handle"); + RCLdotnetDelegates.native_rcl_destroy_wait_set_handle = + (NativeRCLDestroyWaitSetType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_wait_set_handle_ptr, typeof (NativeRCLDestroyWaitSetType)); IntPtr native_rcl_wait_set_clear_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_clear"); @@ -199,18 +195,11 @@ static RCLdotnetDelegates () { (NativeRCLWaitSetAddClientType) Marshal.GetDelegateForFunctionPointer ( native_rcl_wait_set_add_client_ptr, typeof (NativeRCLWaitSetAddClientType)); - IntPtr native_rcl_destroy_wait_set_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_wait_set"); - RCLdotnetDelegates.native_rcl_destroy_wait_set = - (NativeRCLDestroyWaitSetType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_wait_set_ptr, typeof (NativeRCLDestroyWaitSetType)); - IntPtr native_rcl_wait_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait"); RCLdotnetDelegates.native_rcl_wait = (NativeRCLWaitType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_wait_ptr, typeof (NativeRCLWaitType) - ); + native_rcl_wait_ptr, typeof (NativeRCLWaitType)); IntPtr native_rcl_take_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_take"); @@ -218,23 +207,23 @@ static RCLdotnetDelegates () { (NativeRCLTakeType) Marshal.GetDelegateForFunctionPointer ( native_rcl_take_ptr, typeof (NativeRCLTakeType)); - IntPtr native_rcl_create_request_header_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_create_request_header_handle"); - RCLdotnetDelegates.native_rcl_create_request_header_handle = - (NativeRCLCreateRequestHeaderHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_request_header_handle_ptr, typeof (NativeRCLCreateRequestHeaderHandleType)); + IntPtr native_rcl_create_request_id_handle_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_create_request_id_handle"); + RCLdotnetDelegates.native_rcl_create_request_id_handle = + (NativeRCLCreateRequestIdHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_create_request_id_handle_ptr, typeof (NativeRCLCreateRequestIdHandleType)); - IntPtr native_rcl_destroy_request_header_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_request_header_handle"); - RCLdotnetDelegates.native_rcl_destroy_request_header_handle = - (NativeRCLDestroyRequestHeaderHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_request_header_handle_ptr, typeof (NativeRCLDestroyRequestHeaderHandleType)); + IntPtr native_rcl_destroy_request_id_handle_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_request_id_handle"); + RCLdotnetDelegates.native_rcl_destroy_request_id_handle = + (NativeRCLDestroyRequestIdHandleType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_destroy_request_id_handle_ptr, typeof (NativeRCLDestroyRequestIdHandleType)); - IntPtr native_rcl_request_header_get_sequence_number_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_request_header_get_sequence_number"); - RCLdotnetDelegates.native_rcl_request_header_get_sequence_number = - (NativeRCLRequestHeaderGetSequenceNumberType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_request_header_get_sequence_number_ptr, typeof (NativeRCLRequestHeaderGetSequenceNumberType)); + IntPtr native_rcl_request_id_get_sequence_number_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_request_id_get_sequence_number"); + RCLdotnetDelegates.native_rcl_request_id_get_sequence_number = + (NativeRCLRequestIdGetSequenceNumberType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_request_id_get_sequence_number_ptr, typeof (NativeRCLRequestIdGetSequenceNumberType)); IntPtr native_rcl_take_request_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_take_request"); @@ -287,50 +276,77 @@ public static void Spin (Node node) { } } - private static IntPtr GetZeroInitializedWaitSet () { - return RCLdotnetDelegates.native_rcl_get_zero_initialized_wait_set (); - } - - private static void WaitSetInit ( - IntPtr waitSetHandle, - long numberOfSubscriptions, - long numberOfGuardConditions, - long numberOfTimers, - long numberOfClients, - long numberOfServices, - long numberOfEvents) { - RCLdotnetDelegates.native_rcl_wait_set_init ( - waitSetHandle, numberOfSubscriptions, numberOfGuardConditions, + private static SafeWaitSetHandle CreateWaitSet( + int numberOfSubscriptions, + int numberOfGuardConditions, + int numberOfTimers, + int numberOfClients, + int numberOfServices, + int numberOfEvents) { + var waitSetHandle = new SafeWaitSetHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_wait_set_handle( + ref waitSetHandle, numberOfSubscriptions, numberOfGuardConditions, numberOfTimers, numberOfClients, numberOfServices, numberOfEvents); + if (ret != RCLRet.Ok) + { + waitSetHandle.Dispose(); + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_wait_set_handle)}() failed."); + } + + return waitSetHandle; } - private static void WaitSetClear (IntPtr waitSetHandle) { - RCLdotnetDelegates.native_rcl_wait_set_clear (waitSetHandle); + private static void WaitSetClear(SafeWaitSetHandle waitSetHandle) { + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_clear(waitSetHandle); + RCLExceptionHelper.CheckReturnValue(ret); } - private static void WaitSetAddSubscription (IntPtr waitSetHandle, SafeSubscriptionHandle subscriptionHandle) { - // TODO: (sh) Handle return value - RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_subscription (waitSetHandle, subscriptionHandle); + private static void WaitSetAddSubscription(SafeWaitSetHandle waitSetHandle, SafeSubscriptionHandle subscriptionHandle) { + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_subscription(waitSetHandle, subscriptionHandle); + RCLExceptionHelper.CheckReturnValue(ret); } - private static void WaitSetAddService (IntPtr waitSetHandle, SafeServiceHandle serviceHandle) { - // TODO: (sh) Handle return value + private static void WaitSetAddService(SafeWaitSetHandle waitSetHandle, SafeServiceHandle serviceHandle) { RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); + RCLExceptionHelper.CheckReturnValue(ret); } - private static void WaitSetAddClient (IntPtr waitSetHandle, SafeClientHandle clientHandle) { - // TODO: (sh) Handle return value + private static void WaitSetAddClient(SafeWaitSetHandle waitSetHandle, SafeClientHandle clientHandle) { RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_client(waitSetHandle, clientHandle); + RCLExceptionHelper.CheckReturnValue(ret); } - private static void DestroyWaitSet (IntPtr waitSetHandle) { - RCLdotnetDelegates.native_rcl_destroy_wait_set (waitSetHandle); + /// + /// Block until the wait set is ready or until the timeout has been exceeded. + /// + /// The wait set. + /// Timeout in ms. + /// True if wait set is ready, False on timeout. + private static bool Wait(SafeWaitSetHandle waitSetHandle, long timeout) { + long ns_timeout = timeout * 1000000; + RCLRet ret = RCLdotnetDelegates.native_rcl_wait(waitSetHandle, ns_timeout); + if (ret == RCLRet.Ok || ret == RCLRet.Timeout) + { + return ret == RCLRet.Ok; + } + else + { + RCLExceptionHelper.ThrowFromReturnValue(ret); + return false; // unreachable + } } - private static void Wait (IntPtr waitSetHandle, long timeout) { - long ns_timeout = timeout * 1000000; - RCLRet ret = RCLdotnetDelegates.native_rcl_wait (waitSetHandle, ns_timeout); - // TODO(esteve): do something with ret + private static SafeRequestIdHandle CreateRequestId() + { + var requestIdHandle = new SafeRequestIdHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_request_id_handle(ref requestIdHandle); + if (ret != RCLRet.Ok) + { + requestIdHandle.Dispose(); + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_request_id_handle)}() failed."); + } + + return requestIdHandle; } private static bool Take (SafeSubscriptionHandle subscriptionHandle, IMessage message) { @@ -354,7 +370,7 @@ private static bool Take (SafeSubscriptionHandle subscriptionHandle, IMessage me return status; } - private static bool TakeRequest(SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IMessage request) { + private static bool TakeRequest(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IMessage request) { bool status = false; IntPtr requestHandle = request._CREATE_NATIVE_MESSAGE(); RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(serviceHandle, requestHeaderHandle, requestHandle); @@ -378,7 +394,7 @@ private static bool TakeRequest(SafeServiceHandle serviceHandle, IntPtr requestH return status; } - private static bool TakeResponse(SafeClientHandle clientHandle, IntPtr requestHeaderHandle, IMessage response) { + private static bool TakeResponse(SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, IMessage response) { bool status = false; IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(clientHandle, requestHeaderHandle, responseHandle); @@ -402,7 +418,7 @@ private static bool TakeResponse(SafeClientHandle clientHandle, IntPtr requestHe return status; } - private static void SendResponse(SafeServiceHandle serviceHandle, IntPtr requestHeaderHandle, IMessage response) + private static void SendResponse(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IMessage response) { IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); response._WRITE_HANDLE (responseHandle); @@ -415,42 +431,57 @@ private static void SendResponse(SafeServiceHandle serviceHandle, IntPtr request } public static void SpinOnce (Node node, long timeout) { - IntPtr waitSetHandle = GetZeroInitializedWaitSet (); - - long numberOfSubscriptions = node.Subscriptions.Count; - long numberOfGuardConditions = 0; - long numberOfTimers = 0; - long numberOfClients = node.Clients.Count; - long numberOfServices = node.Services.Count; - long numberOfEvents = 0; - - WaitSetInit ( - waitSetHandle, - numberOfSubscriptions, - numberOfGuardConditions, - numberOfTimers, - numberOfClients, - numberOfServices, - numberOfEvents - ); - - WaitSetClear (waitSetHandle); - - foreach (Subscription subscription in node.Subscriptions) { - WaitSetAddSubscription (waitSetHandle, subscription.Handle); - } - - foreach (var service in node.Services) + int numberOfSubscriptions = node.Subscriptions.Count; + int numberOfGuardConditions = 0; + int numberOfTimers = 0; + int numberOfClients = node.Clients.Count; + int numberOfServices = node.Services.Count; + int numberOfEvents = 0; + + bool waitSetEmpty = numberOfSubscriptions == 0 + && numberOfGuardConditions == 0 + && numberOfTimers == 0 + && numberOfClients == 0 + && numberOfServices == 0 + && numberOfEvents == 0; + + if (waitSetEmpty) { - WaitSetAddService(waitSetHandle, service.Handle); + // TODO: (sh) Should we sleep here for the timeout to avoid loops without sleep? + // Avoid WaitSetEmpty return value from rcl_wait. + return; } - foreach (var client in node.Clients) + // TODO: (sh) Reuse wait set (see executer in rclc, rclcpp and rclpy) + using (SafeWaitSetHandle waitSetHandle = CreateWaitSet(numberOfSubscriptions, + numberOfGuardConditions, + numberOfTimers, + numberOfClients, + numberOfServices, + numberOfEvents)) { - WaitSetAddClient(waitSetHandle, client.Handle); - } + WaitSetClear(waitSetHandle); + + foreach (Subscription subscription in node.Subscriptions) { + WaitSetAddSubscription(waitSetHandle, subscription.Handle); + } + + foreach (var service in node.Services) + { + WaitSetAddService(waitSetHandle, service.Handle); + } + + foreach (var client in node.Clients) + { + WaitSetAddClient(waitSetHandle, client.Handle); + } - Wait (waitSetHandle, timeout); + bool ready = Wait(waitSetHandle, timeout); + if (!ready) + { + return; // timeout + } + } foreach (Subscription subscription in node.Subscriptions) { IMessage message = subscription.CreateMessage (); @@ -460,40 +491,36 @@ public static void SpinOnce (Node node, long timeout) { } } - // gets reused for each element in the for loop. - IntPtr requestHeaderHandle = RCLdotnetDelegates.native_rcl_create_request_header_handle(); - - foreach (var service in node.Services) + // requestIdHandle gets reused for each element in the loop. + using (SafeRequestIdHandle requestIdHandle = CreateRequestId()) { - var request = service.CreateRequest(); - var response = service.CreateResponse(); - - var result = TakeRequest(service.Handle, requestHeaderHandle, request); - if (result) + foreach (var service in node.Services) { - // TODO: (sh) catch exceptions - service.TriggerCallback(request, response); + var request = service.CreateRequest(); + var response = service.CreateResponse(); - SendResponse(service.Handle, requestHeaderHandle, response); - } - } + var result = TakeRequest(service.Handle, requestIdHandle, request); + if (result) + { + // TODO: (sh) catch exceptions + service.TriggerCallback(request, response); - foreach (var client in node.Clients) - { - var response = client.CreateResponse(); + SendResponse(service.Handle, requestIdHandle, response); + } + } - var result = TakeResponse(client.Handle, requestHeaderHandle, response); - if (result) + foreach (var client in node.Clients) { - var sequenceNumber = RCLdotnetDelegates.native_rcl_request_header_get_sequence_number(requestHeaderHandle); - client.HandleResponse(sequenceNumber, response); + var response = client.CreateResponse(); + + var result = TakeResponse(client.Handle, requestIdHandle, response); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + client.HandleResponse(sequenceNumber, response); + } } } - - // TODO: (sh) don't leak memory on exceptions - RCLdotnetDelegates.native_rcl_destroy_request_header_handle(requestHeaderHandle); - - DestroyWaitSet (waitSetHandle); } public static void Init () { diff --git a/rcldotnet/SafeRequestIdHandle.cs b/rcldotnet/SafeRequestIdHandle.cs new file mode 100644 index 00000000..31acb39c --- /dev/null +++ b/rcldotnet/SafeRequestIdHandle.cs @@ -0,0 +1,27 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; +using ROS2.Common; + +namespace ROS2 +{ + /// + /// Safe handle representing a rmw_request_id_t + /// + internal sealed class SafeRequestIdHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeRequestIdHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_destroy_request_id_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/SafeWaitSetHandle.cs b/rcldotnet/SafeWaitSetHandle.cs new file mode 100644 index 00000000..87a67b45 --- /dev/null +++ b/rcldotnet/SafeWaitSetHandle.cs @@ -0,0 +1,27 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; +using ROS2.Common; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_wait_set_t + /// + internal sealed class SafeWaitSetHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeWaitSetHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_destroy_wait_set_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index f6979fd8..bb20e8fe 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -72,27 +72,33 @@ int32_t native_rcl_destroy_node_handle(void *node_handle) { return ret; } -void *native_rcl_get_zero_initialized_wait_set() { +int32_t native_rcl_create_wait_set_handle( + void **wait_set_handle, + int32_t number_of_subscriptions, + int32_t number_of_guard_conditions, + int32_t number_of_timers, + int32_t number_of_clients, + int32_t number_of_services, + int32_t number_of_events) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)malloc(sizeof(rcl_wait_set_t)); *wait_set = rcl_get_zero_initialized_wait_set(); - return (void *)wait_set; -} - -int32_t native_rcl_wait_set_init( - void *wait_set_handle, - long number_of_subscriptions, - long number_of_guard_conditions, - long number_of_timers, - long number_of_clients, - long number_of_services, - long number_of_events) { - rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait_set_init( wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, number_of_clients, number_of_services, number_of_events, &context, rcl_get_default_allocator()); + *wait_set_handle = (void *)wait_set; + + return ret; +} + +int32_t native_rcl_destroy_wait_set_handle(void *wait_set_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + + rcl_ret_t ret = rcl_wait_set_fini(wait_set); + free(wait_set); + return ret; } @@ -127,12 +133,7 @@ int32_t native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handl return ret; } -void native_rcl_destroy_wait_set(void *wait_set_handle) { - // TODO: (sh) MemoryLeak: call rcl_wait_set_fini to free all pointers in the rcl_wait_set_t type. - free((rcl_wait_set_t *)wait_set_handle); -} - -int32_t native_rcl_wait_set(void *wait_set_handle, long timeout) { +int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); @@ -146,19 +147,22 @@ int32_t native_rcl_take(void *subscription_handle, void *message_handle) { return ret; } -void * native_rcl_create_request_header_handle(void) { - rmw_request_id_t *request_header = (rmw_request_id_t *)malloc(sizeof(rmw_request_id_t)); - memset(request_header, 0, sizeof(rmw_request_id_t)); - return (void *)request_header; +int32_t native_rcl_create_request_id_handle(void **request_id_handle) { + rmw_request_id_t *request_id = (rmw_request_id_t *)malloc(sizeof(rmw_request_id_t)); + memset(request_id, 0, sizeof(rmw_request_id_t)); + + *request_id_handle = (void *)request_id; + return RCL_RET_OK; } -void native_rcl_destroy_request_header_handle(void *request_header_handle) { - free((rmw_request_id_t *)request_header_handle); +int32_t native_rcl_destroy_request_id_handle(void *request_id_handle) { + free((rmw_request_id_t *)request_id_handle); + return RCL_RET_OK; } -int64_t native_rcl_request_header_get_sequence_number(void *request_header_handle) { - rmw_request_id_t *request_header = (rmw_request_id_t *)request_header_handle; - return request_header->sequence_number; +int64_t native_rcl_request_id_get_sequence_number(void *request_id_handle) { + rmw_request_id_t *request_id = (rmw_request_id_t *)request_id_handle; + return request_id->sequence_number; } int32_t native_rcl_take_request(void *service_handle, void *request_header_handle, void *request_handle) { @@ -185,9 +189,3 @@ int32_t native_rcl_take_response(void *client_handle, void *request_header_handl return ret; } -int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { - rcl_wait_set_t * wait_set = (rcl_wait_set_t *)wait_set_handle; - rcl_ret_t ret = rcl_wait(wait_set, timeout); - - return ret; -} diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index b0fee841..4a44df31 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -33,17 +33,17 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_destroy_node_handle(void *node_handle); RCLDOTNET_EXPORT -void * RCLDOTNET_CDECL native_rcl_get_zero_initialized_wait_set(); +int32_t RCLDOTNET_CDECL native_rcl_create_wait_set_handle( + void **wait_set_handle, + int32_t numberOfSubscriptions, + int32_t numberOfGuardConditions, + int32_t numberOfTimers, + int32_t numberOfClients, + int32_t numberOfServices, + int32_t numberOfEvents); RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_wait_set_init( - void *, - long numberOfSubscriptions, - long numberOfGuardConditions, - long numberOfTimers, - long numberOfClients, - long numberOfServices, - long numberOfEvents); +int32_t RCLDOTNET_CDECL native_rcl_destroy_wait_set_handle(void *wait_set_handle); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_clear(void *); @@ -58,25 +58,19 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handle); RCLDOTNET_EXPORT -void RCLDOTNET_CDECL native_rcl_destroy_wait_set(void *); - -RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_wait_set(void *, long); +int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); - -RCLDOTNET_EXPORT -void * RCLDOTNET_CDECL native_rcl_create_request_header_handle(void); +int32_t RCLDOTNET_CDECL native_rcl_create_request_id_handle(void **request_id_handle); RCLDOTNET_EXPORT -void RCLDOTNET_CDECL native_rcl_destroy_request_header_handle(void *request_header_handle); +int32_t RCLDOTNET_CDECL native_rcl_destroy_request_id_handle(void *request_id_handle); RCLDOTNET_EXPORT -int64_t RCLDOTNET_CDECL native_rcl_request_header_get_sequence_number(void *request_header_handle); +int64_t RCLDOTNET_CDECL native_rcl_request_id_get_sequence_number(void *request_id_handle); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take_request(void *service_handle, void *request_header_handle, void *request_handle); From ca71795eed1ba6949ebe21ae733b258358c24e3d Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 9 Sep 2021 08:47:07 +0200 Subject: [PATCH 22/73] Add SafeHandles to generated message types - renamed IMessage to IRosMessage - moved interfaces for generated types to ROS2 namespace - renamed interface members - used `global::*` in some places to avoid name colisions - added editor browsable never to the members of the interfaces that schould not be used outside of rcldotnet --- rcldotnet/Client.cs | 61 ++++-- rcldotnet/Node.cs | 21 +- rcldotnet/Publisher.cs | 56 ++++-- rcldotnet/RCLdotnet.cs | 219 ++++++++++++++------- rcldotnet/Service.cs | 19 +- rcldotnet/Subscription.cs | 30 ++- rcldotnet/rcldotnet_publisher.c | 9 +- rcldotnet/rcldotnet_publisher.h | 2 +- rcldotnet_common/CMakeLists.txt | 3 +- rcldotnet_common/IRosMessage.cs | 16 ++ rcldotnet_common/IRosServiceDefinition.cs | 11 ++ rcldotnet_common/Interfaces.cs | 40 ---- rosidl_generator_dotnet/resource/idl.cs.em | 1 - rosidl_generator_dotnet/resource/msg.cs.em | 49 +++-- rosidl_generator_dotnet/resource/srv.cs.em | 5 +- 15 files changed, 326 insertions(+), 216 deletions(-) create mode 100644 rcldotnet_common/IRosMessage.cs create mode 100644 rcldotnet_common/IRosServiceDefinition.cs delete mode 100644 rcldotnet_common/Interfaces.cs diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index b72b98d6..a5e28fa3 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -1,9 +1,9 @@ using System; using System.Collections.Concurrent; +using System.Reflection; using System.Runtime.InteropServices; using System.Threading.Tasks; using ROS2.Common; -using ROS2.Interfaces; using ROS2.Utils; namespace ROS2 @@ -13,13 +13,13 @@ internal static class ClientDelegates internal static readonly DllLoadUtils dllLoadUtils; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLSendRequestType( - SafeClientHandle clientHandle, IntPtr requestHandle, out long seqneceNumber); + internal delegate RCLRet NativeRCLSendRequestType( + SafeClientHandle clientHandle, SafeHandle requestHandle, out long seqneceNumber); internal static NativeRCLSendRequestType native_rcl_send_request = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate int NativeRCLServiceServerIsAvailableType( + internal delegate RCLRet NativeRCLServiceServerIsAvailableType( SafeNodeHandle nodeHandle, SafeClientHandle clientHandle, out bool isAvailable); internal static NativeRCLServiceServerIsAvailableType native_rcl_service_server_is_available = null; @@ -56,15 +56,15 @@ internal Client() // Disposed if the client is not live anymore. internal abstract SafeClientHandle Handle { get; } - internal abstract IMessage CreateResponse(); + internal abstract IRosMessage CreateResponse(); - internal abstract void HandleResponse(long sequenceNumber, IMessage response); + internal abstract void HandleResponse(long sequenceNumber, IRosMessage response); } public sealed class Client : Client where TService : IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new() + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() { // ros2_java uses a WeakReference here. Not sure if its needed or not. private readonly Node _node; @@ -80,10 +80,11 @@ internal Client(SafeClientHandle handle, Node node) public bool ServiceIsReady() { - var ret = (RCLRet)ClientDelegates.native_rcl_service_server_is_available(_node.Handle, Handle, out var serviceIsReady); + RCLRet ret = ClientDelegates.native_rcl_service_server_is_available(_node.Handle, Handle, out var serviceIsReady); if (ret != RCLRet.Ok) { - throw new Exception($"rcl_service_server_is_available() failed: {ret}"); + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_service_server_is_available)}() failed."); + return false; // unrachable } return serviceIsReady; @@ -94,14 +95,38 @@ public Task SendRequestAsync(TRequest request) // TODO: (sh) Add cancellationToken(?), timeout (via cancellationToken?) and cleanup of pending requests. // TODO: (sh) Catch all exceptions and return Task with error? // How should this be done according to best practices. - IntPtr requestHandle = request._CREATE_NATIVE_MESSAGE(); - request._WRITE_HANDLE(requestHandle); + long sequenceNumber; - RCLRet ret = (RCLRet)ClientDelegates.native_rcl_send_request(Handle, requestHandle, out var sequenceNumber); - - // TODO: (sh) don't leak memory on exceptions - request._DESTROY_NATIVE_MESSAGE(requestHandle); + MethodInfo m = typeof(TRequest).GetTypeInfo().GetDeclaredMethod ("__CreateMessageHandle"); + using (var requestHandle = (SafeHandle)m.Invoke (null, new object[] { })) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + requestHandle.DangerousAddRef(ref mustRelease); + request.__WriteToHandle(requestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + requestHandle.DangerousRelease(); + } + } + + RCLRet ret = ClientDelegates.native_rcl_send_request(Handle, requestHandle, out sequenceNumber); + if (ret != RCLRet.Ok) + { + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_send_request)}() failed."); + return null; // unrachable + } + } var taskCompletionSource = new TaskCompletionSource(); var pendingRequest = new PendingRequest(taskCompletionSource); @@ -113,9 +138,9 @@ public Task SendRequestAsync(TRequest request) return taskCompletionSource.Task; } - internal override IMessage CreateResponse() => (IMessage)new TResponse(); + internal override IRosMessage CreateResponse() => (IRosMessage)new TResponse(); - internal override void HandleResponse(long sequenceNumber, IMessage response) + internal override void HandleResponse(long sequenceNumber, IRosMessage response) { if (!_pendingRequests.TryRemove(sequenceNumber, out var pendingRequest)) { diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 11045235..241e0ae9 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -18,7 +18,6 @@ using System.Reflection; using System.Runtime.InteropServices; using ROS2.Common; -using ROS2.Interfaces; using ROS2.Utils; namespace ROS2 { @@ -166,8 +165,8 @@ internal Node (SafeNodeHandle handle) { // Disposed if the node is not live anymore. internal SafeNodeHandle Handle { get; } - public Publisher CreatePublisher (string topic) where T : IMessage { - MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("_GET_TYPE_SUPPORT"); + public Publisher CreatePublisher (string topic) where T : IRosMessage { + MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("__GetTypeSupport"); IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); var publisherHandle = new SafePublisherHandle(); @@ -184,8 +183,8 @@ public Publisher CreatePublisher (string topic) where T : IMessage { return publisher; } - public Subscription CreateSubscription (string topic, Action callback) where T : IMessage, new () { - MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("_GET_TYPE_SUPPORT"); + public Subscription CreateSubscription (string topic, Action callback) where T : IRosMessage, new () { + MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("__GetTypeSupport"); IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); var subscriptionHandle = new SafeSubscriptionHandle(); @@ -205,10 +204,10 @@ public Publisher CreatePublisher (string topic) where T : IMessage { public Service CreateService(string serviceName, Action callback) where TService : IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new() + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() { - MethodInfo m = typeof(TService).GetTypeInfo().GetDeclaredMethod("_GET_TYPE_SUPPORT"); + MethodInfo m = typeof(TService).GetTypeInfo().GetDeclaredMethod("__GetTypeSupport"); IntPtr typesupport = (IntPtr)m.Invoke (null, new object[] { }); var serviceHandle = new SafeServiceHandle(); @@ -228,10 +227,10 @@ public Service CreateService CreateClient(string serviceName) where TService : IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new() + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() { - MethodInfo m = typeof(TService).GetTypeInfo().GetDeclaredMethod("_GET_TYPE_SUPPORT"); + MethodInfo m = typeof(TService).GetTypeInfo().GetDeclaredMethod("__GetTypeSupport"); IntPtr typesupport = (IntPtr)m.Invoke (null, new object[] { }); var clientHandle = new SafeClientHandle(); diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index 1d9a3d0a..9c2bfcaa 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -14,8 +14,9 @@ */ using System; +using System.Reflection; using System.Runtime.InteropServices; -using ROS2.Interfaces; +using ROS2.Common; using ROS2.Utils; namespace ROS2 { @@ -23,14 +24,15 @@ internal static class PublisherDelegates { internal static readonly DllLoadUtils dllLoadUtils; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate void NativeRCLPublishType ( - SafePublisherHandle publisherHandle, IntPtr messageHandle); + internal delegate RCLRet NativeRCLPublishType ( + SafePublisherHandle publisherHandle, SafeHandle messageHandle); internal static NativeRCLPublishType native_rcl_publish = null; static PublisherDelegates () { dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); IntPtr nativelibrary = dllLoadUtils.LoadLibrary ("rcldotnet_publisher"); + IntPtr native_rcl_publish_ptr = dllLoadUtils.GetProcAddress (nativelibrary, "native_rcl_publish"); PublisherDelegates.native_rcl_publish = (NativeRCLPublishType) Marshal.GetDelegateForFunctionPointer ( native_rcl_publish_ptr, typeof (NativeRCLPublishType)); @@ -53,27 +55,47 @@ internal Publisher() // By relying on the GC/Finalizer of SafeHandle the handle only gets // Disposed if the publisher is not live anymore. internal abstract SafePublisherHandle Handle { get; } - } public sealed class Publisher : Publisher - where T : IMessage { - - internal Publisher (SafePublisherHandle handle) { - Handle = handle; - } + where T : IRosMessage { - internal override SafePublisherHandle Handle { get; } - - public void Publish (T msg) { - IntPtr messageHandle = msg._CREATE_NATIVE_MESSAGE (); + internal Publisher (SafePublisherHandle handle) { + Handle = handle; + } - msg._WRITE_HANDLE (messageHandle); + internal override SafePublisherHandle Handle { get; } - // TODO: (sh) Pass and handle return value. - PublisherDelegates.native_rcl_publish (Handle, messageHandle); + public void Publish (T message) { + MethodInfo m = typeof(T).GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); + using (var messageHandle = (SafeHandle)m.Invoke(null, new object[] { })) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__WriteToHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } - msg._DESTROY_NATIVE_MESSAGE (messageHandle); + RCLRet ret = PublisherDelegates.native_rcl_publish(Handle, messageHandle); + if (ret != RCLRet.Ok) + { + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(PublisherDelegates.native_rcl_publish)}() failed."); + return; // unrachable + } } } + } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index ea8a3341..5d0bd4cd 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -14,9 +14,9 @@ */ using System; +using System.Reflection; using System.Runtime.InteropServices; using ROS2.Common; -using ROS2.Interfaces; using ROS2.Utils; namespace ROS2 { @@ -89,7 +89,7 @@ internal delegate RCLRet NativeRCLCreateWaitSetHandleType ( internal static NativeRCLWaitType native_rcl_wait = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeType (SafeSubscriptionHandle subscriptionHandle, IntPtr messageHandle); + internal delegate RCLRet NativeRCLTakeType (SafeSubscriptionHandle subscriptionHandle, SafeHandle messageHandle); internal static NativeRCLTakeType native_rcl_take = null; @@ -110,17 +110,17 @@ internal delegate RCLRet NativeRCLCreateRequestIdHandleType ( internal static NativeRCLRequestIdGetSequenceNumberType native_rcl_request_id_get_sequence_number = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeRequestType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IntPtr requestHandle); + internal delegate RCLRet NativeRCLTakeRequestType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle requestHandle); internal static NativeRCLTakeRequestType native_rcl_take_request = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLSendResponseType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IntPtr responseHandle); + internal delegate RCLRet NativeRCLSendResponseType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle responseHandle); internal static NativeRCLSendResponseType native_rcl_send_response = null; [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeResponseType (SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, IntPtr responseHandle); + internal delegate RCLRet NativeRCLTakeResponseType (SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle responseHandle); internal static NativeRCLTakeResponseType native_rcl_take_response = null; @@ -349,85 +349,158 @@ private static SafeRequestIdHandle CreateRequestId() return requestIdHandle; } - private static bool Take (SafeSubscriptionHandle subscriptionHandle, IMessage message) { - bool status = false; - IntPtr messageHandle = message._CREATE_NATIVE_MESSAGE (); - RCLRet ret = RCLdotnetDelegates.native_rcl_take (subscriptionHandle, messageHandle); - - // TODO: (sh) Handle all return values (exceptions) - switch (ret) { - case RCLRet.Ok: - message._READ_HANDLE (messageHandle); - status = true; - break; - case RCLRet.SubscriptionTakeFailed: - status = false; - break; - default: - break; + private static bool Take (SafeSubscriptionHandle subscriptionHandle, IRosMessage message) { + // TODO: (sh) Move to Subscription to take advantage of generic argument. + MethodInfo m = message.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); + using (var messageHandle = (SafeHandle)m.Invoke(null, new object[] { })) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_take(subscriptionHandle, messageHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__ReadFromHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.SubscriptionTakeFailed: + return false; + + default: + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take)}() failed."); + return false; // unrachable + } } - message._DESTROY_NATIVE_MESSAGE (messageHandle); - return status; } - private static bool TakeRequest(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IMessage request) { - bool status = false; - IntPtr requestHandle = request._CREATE_NATIVE_MESSAGE(); - RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(serviceHandle, requestHeaderHandle, requestHandle); - - // TODO: (sh) Handle all return values (exceptions) - switch (ret) + private static bool TakeRequest(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IRosMessage request) { + // TODO: (sh) Move to Service to take advantage of generic argument. + MethodInfo m = request.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); + using (var requestHandle = (SafeHandle)m.Invoke(null, new object[] { })) { - case RCLRet.Ok: - request._READ_HANDLE(requestHandle); - status = true; - break; - case RCLRet.ServiceTakeFailed: - status = false; - break; - default: - break; + RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(serviceHandle, requestHeaderHandle, requestHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + requestHandle.DangerousAddRef(ref mustRelease); + request.__ReadFromHandle(requestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + requestHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_request)}() failed."); + return false; // unrachable + } } - - // TODO: (sh) don't leak memory on exceptions - request._DESTROY_NATIVE_MESSAGE (requestHandle); - return status; } - private static bool TakeResponse(SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, IMessage response) { - bool status = false; - IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); - RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(clientHandle, requestHeaderHandle, responseHandle); - - // TODO: (sh) Handle all return values (exceptions) - switch (ret) + private static bool TakeResponse(SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { + // TODO: (sh) Move to Client to take advantage of generic argument. + MethodInfo m = response.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); + using (var responseHandle = (SafeHandle)m.Invoke(null, new object[] { })) { - case RCLRet.Ok: - response._READ_HANDLE(responseHandle); - status = true; - break; - case RCLRet.ClientTakeFailed: - status = false; - break; - default: - break; + RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(clientHandle, requestHeaderHandle, responseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + responseHandle.DangerousAddRef(ref mustRelease); + response.__ReadFromHandle(responseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + responseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_response)}() failed."); + return false; // unrachable + } } - - // TODO: (sh) don't leak memory on exceptions - response._DESTROY_NATIVE_MESSAGE (responseHandle); - return status; } - private static void SendResponse(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IMessage response) + private static void SendResponse(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { - IntPtr responseHandle = response._CREATE_NATIVE_MESSAGE(); - response._WRITE_HANDLE (responseHandle); - - // TODO: (sh) check return values - RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(serviceHandle, requestHeaderHandle, responseHandle); - - // TODO: (sh) don't leak memory on exceptions - response._DESTROY_NATIVE_MESSAGE (responseHandle); + // TODO: (sh) Move to Service to take advantage of generic argument. + MethodInfo m = response.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); + using (var responseHandle = (SafeHandle)m.Invoke (null, new object[] { })) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + responseHandle.DangerousAddRef(ref mustRelease); + response.__WriteToHandle(responseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + responseHandle.DangerousRelease(); + } + } + + RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(serviceHandle, requestHeaderHandle, responseHandle); + if (ret != RCLRet.Ok) + { + RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); + return; // unrachable + } + } } public static void SpinOnce (Node node, long timeout) { @@ -484,7 +557,7 @@ public static void SpinOnce (Node node, long timeout) { } foreach (Subscription subscription in node.Subscriptions) { - IMessage message = subscription.CreateMessage (); + IRosMessage message = subscription.CreateMessage(); bool result = Take (subscription.Handle, message); if (result) { subscription.TriggerCallback (message); diff --git a/rcldotnet/Service.cs b/rcldotnet/Service.cs index 6add4c97..ba31f962 100644 --- a/rcldotnet/Service.cs +++ b/rcldotnet/Service.cs @@ -1,5 +1,4 @@ using System; -using ROS2.Interfaces; namespace ROS2 { @@ -20,17 +19,17 @@ internal Service() // Disposed if the service is not live anymore. abstract internal SafeServiceHandle Handle { get; } - abstract internal IMessage CreateRequest(); + abstract internal IRosMessage CreateRequest(); - abstract internal IMessage CreateResponse(); + abstract internal IRosMessage CreateResponse(); - abstract internal void TriggerCallback(IMessage request, IMessage response); + abstract internal void TriggerCallback(IRosMessage request, IRosMessage response); } public sealed class Service : Service where TService : IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new() + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() { private Action _callback; @@ -40,13 +39,13 @@ internal Service(SafeServiceHandle handle, Action callback) _callback = callback; } - internal override SafeServiceHandle Handle { get; } + internal override SafeServiceHandle Handle { get; } - internal override IMessage CreateRequest() => (IMessage)new TRequest(); + internal override IRosMessage CreateRequest() => (IRosMessage)new TRequest(); - internal override IMessage CreateResponse() => (IMessage)new TResponse(); + internal override IRosMessage CreateResponse() => (IRosMessage)new TResponse(); - internal override void TriggerCallback(IMessage request, IMessage response) + internal override void TriggerCallback(IRosMessage request, IRosMessage response) { _callback((TRequest)request, (TResponse)response); } diff --git a/rcldotnet/Subscription.cs b/rcldotnet/Subscription.cs index 1b149e52..9ea06974 100644 --- a/rcldotnet/Subscription.cs +++ b/rcldotnet/Subscription.cs @@ -14,7 +14,6 @@ */ using System; -using ROS2.Interfaces; namespace ROS2 { @@ -35,29 +34,26 @@ internal Subscription() // Disposed if the subscription is not live anymore. internal abstract SafeSubscriptionHandle Handle { get; } - internal abstract IMessage CreateMessage(); + internal abstract IRosMessage CreateMessage(); - internal abstract void TriggerCallback(IMessage message); + internal abstract void TriggerCallback(IRosMessage message); } public class Subscription : Subscription - where T : IMessage, new () { - private Action callback_; + where T : IRosMessage, new() { + private Action callback_; - internal Subscription(SafeSubscriptionHandle handle, Action callback) { - Handle = handle; - callback_ = callback; - } + internal Subscription(SafeSubscriptionHandle handle, Action callback) { + Handle = handle; + callback_ = callback; + } - internal override SafeSubscriptionHandle Handle { get; } + internal override SafeSubscriptionHandle Handle { get; } - internal override IMessage CreateMessage() { - IMessage msg = (IMessage) new T (); - return msg; - } + internal override IRosMessage CreateMessage() => (IRosMessage)new T(); - internal override void TriggerCallback(IMessage message) { - callback_ ((T) message); - } + internal override void TriggerCallback(IRosMessage message) { + callback_((T) message); } + } } diff --git a/rcldotnet/rcldotnet_publisher.c b/rcldotnet/rcldotnet_publisher.c index d8e1b944..1450feb8 100644 --- a/rcldotnet/rcldotnet_publisher.c +++ b/rcldotnet/rcldotnet_publisher.c @@ -25,14 +25,11 @@ #include "rcldotnet_publisher.h" -void native_rcl_publish(void * publisher_handle, void * raw_ros_message) +int32_t native_rcl_publish(void * publisher_handle, void * raw_ros_message) { rcl_publisher_t * publisher = (rcl_publisher_t *)publisher_handle; rcl_ret_t ret = rcl_publish(publisher, raw_ros_message, NULL); - - // TODO(esteve): handle error - if (ret != RCL_RET_OK) { - assert(false); - } + + return ret; } diff --git a/rcldotnet/rcldotnet_publisher.h b/rcldotnet/rcldotnet_publisher.h index 649affc7..5f1a4112 100644 --- a/rcldotnet/rcldotnet_publisher.h +++ b/rcldotnet/rcldotnet_publisher.h @@ -18,6 +18,6 @@ #include "rcldotnet_macros.h" RCLDOTNET_EXPORT -void RCLDOTNET_CDECL native_rcl_publish(void *, void *); +int32_t RCLDOTNET_CDECL native_rcl_publish(void *, void *); #endif // RCLDOTNET_PUBLISHER_H diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index 18057004..578258fb 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -10,7 +10,8 @@ find_package(DotNETExtra REQUIRED) set(CS_SOURCES DllLoadUtils.cs - Interfaces.cs + IRosMessage.cs + IRosServiceDefinition.cs RCLExceptionHelper.cs RCLRet.cs ) diff --git a/rcldotnet_common/IRosMessage.cs b/rcldotnet_common/IRosMessage.cs new file mode 100644 index 00000000..534a3141 --- /dev/null +++ b/rcldotnet_common/IRosMessage.cs @@ -0,0 +1,16 @@ +using System; + +namespace ROS2 +{ + public interface IRosMessage + { + // must be implemented on deriving types, gets called via reflection + // (static abstract interface members are not supported yet.) + // public static abstract IntPtr __GetTypeSupport(); + // public static abstract SafeHandle __CreateMessageHandle(); + + void __ReadFromHandle(IntPtr messageHandle); + + void __WriteToHandle(IntPtr messageHandle); + } +} diff --git a/rcldotnet_common/IRosServiceDefinition.cs b/rcldotnet_common/IRosServiceDefinition.cs new file mode 100644 index 00000000..06b74410 --- /dev/null +++ b/rcldotnet_common/IRosServiceDefinition.cs @@ -0,0 +1,11 @@ +namespace ROS2 +{ + public interface IRosServiceDefinition + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() + { + // must be implemented on deriving types, gets called via reflection + // (static abstract interface members are not supported yet.) + // public static abstract IntPtr __GetTypeSupport(); + } +} diff --git a/rcldotnet_common/Interfaces.cs b/rcldotnet_common/Interfaces.cs deleted file mode 100644 index f3069c9b..00000000 --- a/rcldotnet_common/Interfaces.cs +++ /dev/null @@ -1,40 +0,0 @@ -/* Copyright 2016-2018 Esteve Fernandez - * - * 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; - -namespace ROS2 { - namespace Interfaces { - public interface IMessage { - // must be implemented on deriving types, gets called via reflection - // (static abstract interface members are not supported yet.) - // public static abstract IntPtr _GET_TYPE_SUPPORT(); - - IntPtr _CREATE_NATIVE_MESSAGE (); - void _READ_HANDLE (IntPtr messageHandle); - void _DESTROY_NATIVE_MESSAGE (IntPtr messageHandle); - void _WRITE_HANDLE (IntPtr messageHandle); - } - - public interface IRosServiceDefinition - where TRequest : IMessage, new() - where TResponse : IMessage, new() - { - // must be implemented on deriving types, gets called via reflection - // (static abstract interface members are not supported yet.) - // public static abstract IntPtr _GET_TYPE_SUPPORT(); - } - } -} diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index 5bca49d2..3cfc1ede 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -14,7 +14,6 @@ using System; using System.Runtime.InteropServices; using System.Collections.Generic; -using ROS2.Interfaces; using ROS2.Utils; @####################################################################### diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 61dfb5b7..963387c2 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -19,7 +19,7 @@ msg_typename = '%s__%s' % ('__'.join(message.structure.namespaced_type.namespace namespace @('.'.join(message.structure.namespaced_type.namespaces)) { -public class @(type_name) : IMessage { +public class @(type_name) : global::ROS2.IRosMessage { private static readonly DllLoadUtils dllLoadUtils; @[for member in message.structure.members]@ @@ -142,7 +142,7 @@ public class @(type_name) : IMessage { private static NativeGetTypeSupportType native_get_typesupport = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate IntPtr NativeCreateNativeType(); + private delegate Safe@(type_name)Handle NativeCreateNativeType(); private static NativeCreateNativeType native_create_native_message = null; @@ -217,15 +217,16 @@ public class @(type_name) : IMessage { @[ end if]@ @[end for]@ - public static IntPtr _GET_TYPE_SUPPORT() { - return native_get_typesupport(); - } +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::System.IntPtr __GetTypeSupport() => native_get_typesupport(); - public IntPtr _CREATE_NATIVE_MESSAGE() { - return native_create_native_message(); - } +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::System.Runtime.InteropServices.SafeHandle __CreateMessageHandle() => native_create_native_message(); - public void _READ_HANDLE(IntPtr messageHandle) { + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public void __ReadFromHandle(global::System.IntPtr messageHandle) { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ { @@ -241,7 +242,7 @@ public class @(type_name) : IMessage { // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name))[i__local_variable] = new @(get_dotnet_type(member.type.value_type))(); - @(get_field_name(type_name, member.name))[i__local_variable]._READ_HANDLE(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); + @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if] } } @@ -260,7 +261,7 @@ public class @(type_name) : IMessage { // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name)).Add(new @(get_dotnet_type(member.type.value_type))()); - @(get_field_name(type_name, member.name))[i__local_variable]._READ_HANDLE(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); + @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if] } } @@ -274,12 +275,13 @@ public class @(type_name) : IMessage { @(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(messageHandle); @[ end if]@ @[ else]@ - @(get_field_name(type_name, member.name))._READ_HANDLE(native_get_field_@(member.name)_HANDLE(messageHandle)); + @(get_field_name(type_name, member.name)).__ReadFromHandle(native_get_field_@(member.name)_HANDLE(messageHandle)); @[ end if]@ @[end for]@ } - public void _WRITE_HANDLE(IntPtr messageHandle) { + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public void __WriteToHandle(global::System.IntPtr messageHandle) { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ { @@ -310,7 +312,7 @@ public class @(type_name) : IMessage { @[ elif isinstance(member.type.value_type, AbstractWString)] // TODO: Unicode types are not supported @[ else]@ - value__local_variable._WRITE_HANDLE(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); + value__local_variable.__WriteToHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if]@ } } @@ -320,15 +322,11 @@ public class @(type_name) : IMessage { @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ native_write_field_@(member.name)(messageHandle, @(get_field_name(type_name, member.name))); @[ else]@ - @(get_field_name(type_name, member.name))._WRITE_HANDLE(native_get_field_@(member.name)_HANDLE(messageHandle)); + @(get_field_name(type_name, member.name)).__WriteToHandle(native_get_field_@(member.name)_HANDLE(messageHandle)); @[ end if]@ @[end for]@ } - public void _DESTROY_NATIVE_MESSAGE(IntPtr messageHandle) { - native_destroy_native_message(messageHandle); - } - @[for constant in message.constants]@ public static readonly @(get_dotnet_type(constant.type)) @(constant.name) = @(constant_value_to_dotnet(constant.type, constant.value)); @@ -345,6 +343,19 @@ public class @(type_name) : IMessage { public @(get_dotnet_type(member.type)) @(get_field_name(type_name, member.name)) { get; set; } @[ end if]@ @[end for]@ + + private sealed class Safe@(type_name)Handle : global::System.Runtime.InteropServices.SafeHandle + { + public Safe@(type_name)Handle() : base(global::System.IntPtr.Zero, true) { } + + public override bool IsInvalid => handle == global::System.IntPtr.Zero; + + protected override bool ReleaseHandle() + { + native_destroy_native_message(handle); + return true; + } + } } } diff --git a/rosidl_generator_dotnet/resource/srv.cs.em b/rosidl_generator_dotnet/resource/srv.cs.em index 9ec5ed73..bd541280 100644 --- a/rosidl_generator_dotnet/resource/srv.cs.em +++ b/rosidl_generator_dotnet/resource/srv.cs.em @@ -27,7 +27,7 @@ namespace @('.'.join(service.namespaced_type.namespaces)) @# static abstract interface members are currently in preview, so maybe we could use the feature in the future. @# (if hey add support to derive from static only interfaces in static classes) @# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. - public sealed class @(type_name) : IRosServiceDefinition<@(request_type_name), @(response_type_name)> + public sealed class @(type_name) : global::ROS2.IRosServiceDefinition<@(request_type_name), @(response_type_name)> { private static readonly DllLoadUtils dllLoadUtils; @@ -52,7 +52,8 @@ namespace @('.'.join(service.namespaced_type.namespaces)) private static NativeGetTypeSupportType native_get_typesupport = null; @# This method gets called via reflection as static abstract interface members are not supported yet. - public static IntPtr _GET_TYPE_SUPPORT() { + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static IntPtr __GetTypeSupport() { return native_get_typesupport(); } } From c3d3797fd063e066dff4dbe8f026ef8880fbb13a Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 9 Sep 2021 10:36:50 +0200 Subject: [PATCH 23/73] Add static member caches - Avoids doing reflection over and over. - If we can require static abstract interface members this could go away. --- rcldotnet/CMakeLists.txt | 2 + rcldotnet/Client.cs | 8 +- rcldotnet/MessageStaticMemberCache.cs | 75 +++++++++++++++++++ rcldotnet/Node.cs | 15 ++-- rcldotnet/Publisher.cs | 7 +- rcldotnet/RCLdotnet.cs | 41 ++++------ rcldotnet/Service.cs | 9 +++ .../ServiceDefinitionStaticMemberCache.cs | 46 ++++++++++++ rcldotnet/Subscription.cs | 5 ++ 9 files changed, 166 insertions(+), 42 deletions(-) create mode 100644 rcldotnet/MessageStaticMemberCache.cs create mode 100644 rcldotnet/ServiceDefinitionStaticMemberCache.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 0bd5be57..f62b9dc4 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -29,6 +29,7 @@ set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") set(CS_SOURCES Client.cs + MessageStaticMemberCache.cs Node.cs Publisher.cs RCLdotnet.cs @@ -40,6 +41,7 @@ set(CS_SOURCES SafeSubscriptionHandle.cs SafeWaitSetHandle.cs Service.cs + ServiceDefinitionStaticMemberCache.cs Subscription.cs ) diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index a5e28fa3..62b0b2fc 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -1,6 +1,5 @@ using System; using System.Collections.Concurrent; -using System.Reflection; using System.Runtime.InteropServices; using System.Threading.Tasks; using ROS2.Common; @@ -58,6 +57,8 @@ internal Client() internal abstract IRosMessage CreateResponse(); + internal abstract SafeHandle CreateResponseHandle(); + internal abstract void HandleResponse(long sequenceNumber, IRosMessage response); } @@ -98,8 +99,7 @@ public Task SendRequestAsync(TRequest request) long sequenceNumber; - MethodInfo m = typeof(TRequest).GetTypeInfo().GetDeclaredMethod ("__CreateMessageHandle"); - using (var requestHandle = (SafeHandle)m.Invoke (null, new object[] { })) + using (var requestHandle = MessageStaticMemberCache.CreateMessageHandle()) { bool mustRelease = false; try @@ -140,6 +140,8 @@ public Task SendRequestAsync(TRequest request) internal override IRosMessage CreateResponse() => (IRosMessage)new TResponse(); + internal override SafeHandle CreateResponseHandle() => MessageStaticMemberCache.CreateMessageHandle(); + internal override void HandleResponse(long sequenceNumber, IRosMessage response) { if (!_pendingRequests.TryRemove(sequenceNumber, out var pendingRequest)) diff --git a/rcldotnet/MessageStaticMemberCache.cs b/rcldotnet/MessageStaticMemberCache.cs new file mode 100644 index 00000000..59756c77 --- /dev/null +++ b/rcldotnet/MessageStaticMemberCache.cs @@ -0,0 +1,75 @@ +using System; +using System.Reflection; +using System.Runtime.InteropServices; + +namespace ROS2 +{ + internal static class MessageStaticMemberCache + where T : IRosMessage + { + private static IntPtr s_typeSupport; + private static Func s_createMessageHandle; + + static MessageStaticMemberCache() + { + TypeInfo typeInfo = typeof(T).GetTypeInfo(); + + MethodInfo getTypeSupport = typeInfo.GetDeclaredMethod("__GetTypeSupport"); + if (getTypeSupport != null) + { + try + { + s_typeSupport = (IntPtr)getTypeSupport.Invoke(null, new object[] { }); + } + catch + { + s_typeSupport = IntPtr.Zero; + } + } + else + { + s_typeSupport = IntPtr.Zero; + } + + MethodInfo createMessageHandle = typeInfo.GetDeclaredMethod("__CreateMessageHandle"); + if (createMessageHandle != null) + { + try + { + s_createMessageHandle = (Func)createMessageHandle.CreateDelegate(typeof(Func)); + } + catch + { + s_createMessageHandle = null; + } + } + else + { + s_createMessageHandle = null; + } + } + + public static IntPtr GetTypeSupport() + { + // mehtod because it could throw. + if (s_typeSupport == IntPtr.Zero) + { + throw new InvalidOperationException($"Type '{typeof(T).FullName}' did not define a correct __GetTypeSupport mehtod."); + } + + return s_typeSupport; + } + + public static SafeHandle CreateMessageHandle() + { + if (s_createMessageHandle != null) + { + return s_createMessageHandle(); + } + else + { + throw new InvalidOperationException($"Type '{typeof(T).FullName}' did not define a correct __CreateMessageHandle mehtod."); + } + } + } +} diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 241e0ae9..963ced88 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -15,7 +15,6 @@ using System; using System.Collections.Generic; -using System.Reflection; using System.Runtime.InteropServices; using ROS2.Common; using ROS2.Utils; @@ -166,8 +165,7 @@ internal Node (SafeNodeHandle handle) { internal SafeNodeHandle Handle { get; } public Publisher CreatePublisher (string topic) where T : IRosMessage { - MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("__GetTypeSupport"); - IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); + IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); var publisherHandle = new SafePublisherHandle(); RCLRet ret = NodeDelegates.native_rcl_create_publisher_handle (ref publisherHandle, Handle, topic, typesupport); @@ -184,8 +182,7 @@ public Publisher CreatePublisher (string topic) where T : IRosMessage { } public Subscription CreateSubscription (string topic, Action callback) where T : IRosMessage, new () { - MethodInfo m = typeof (T).GetTypeInfo().GetDeclaredMethod ("__GetTypeSupport"); - IntPtr typesupport = (IntPtr) m.Invoke (null, new object[] { }); + IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); var subscriptionHandle = new SafeSubscriptionHandle(); RCLRet ret = NodeDelegates.native_rcl_create_subscription_handle (ref subscriptionHandle, Handle, topic, typesupport); @@ -207,8 +204,7 @@ public Service CreateService.GetTypeSupport(); var serviceHandle = new SafeServiceHandle(); RCLRet ret = NodeDelegates.native_rcl_create_service_handle(ref serviceHandle, Handle, serviceName, typesupport); @@ -230,8 +226,7 @@ public Client CreateClient.GetTypeSupport(); var clientHandle = new SafeClientHandle(); RCLRet ret = NodeDelegates.native_rcl_create_client_handle(ref clientHandle, Handle, serviceName, typesupport); @@ -239,7 +234,7 @@ public Client CreateClient.CreateMessageHandle()) { bool mustRelease = false; try diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 5d0bd4cd..e6ae98bf 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -14,7 +14,6 @@ */ using System; -using System.Reflection; using System.Runtime.InteropServices; using ROS2.Common; using ROS2.Utils; @@ -349,12 +348,10 @@ private static SafeRequestIdHandle CreateRequestId() return requestIdHandle; } - private static bool Take (SafeSubscriptionHandle subscriptionHandle, IRosMessage message) { - // TODO: (sh) Move to Subscription to take advantage of generic argument. - MethodInfo m = message.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); - using (var messageHandle = (SafeHandle)m.Invoke(null, new object[] { })) + private static bool Take (Subscription subscription, IRosMessage message) { + using (var messageHandle = subscription.CreateMessageHandle()) { - RCLRet ret = RCLdotnetDelegates.native_rcl_take(subscriptionHandle, messageHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_take(subscription.Handle, messageHandle); switch (ret) { case RCLRet.Ok: @@ -389,12 +386,10 @@ private static bool Take (SafeSubscriptionHandle subscriptionHandle, IRosMessage } } - private static bool TakeRequest(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IRosMessage request) { - // TODO: (sh) Move to Service to take advantage of generic argument. - MethodInfo m = request.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); - using (var requestHandle = (SafeHandle)m.Invoke(null, new object[] { })) + private static bool TakeRequest(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage request) { + using (var requestHandle = service.CreateRequestHandle()) { - RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(serviceHandle, requestHeaderHandle, requestHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(service.Handle, requestHeaderHandle, requestHandle); switch (ret) { case RCLRet.Ok: @@ -429,12 +424,10 @@ private static bool TakeRequest(SafeServiceHandle serviceHandle, SafeRequestIdHa } } - private static bool TakeResponse(SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { - // TODO: (sh) Move to Client to take advantage of generic argument. - MethodInfo m = response.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); - using (var responseHandle = (SafeHandle)m.Invoke(null, new object[] { })) + private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { + using (var responseHandle = client.CreateResponseHandle()) { - RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(clientHandle, requestHeaderHandle, responseHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(client.Handle, requestHeaderHandle, responseHandle); switch (ret) { case RCLRet.Ok: @@ -469,11 +462,9 @@ private static bool TakeResponse(SafeClientHandle clientHandle, SafeRequestIdHan } } - private static void SendResponse(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) + private static void SendResponse(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { - // TODO: (sh) Move to Service to take advantage of generic argument. - MethodInfo m = response.GetType().GetTypeInfo().GetDeclaredMethod("__CreateMessageHandle"); - using (var responseHandle = (SafeHandle)m.Invoke (null, new object[] { })) + using (var responseHandle = service.CreateResponseHandle()) { bool mustRelease = false; try @@ -494,7 +485,7 @@ private static void SendResponse(SafeServiceHandle serviceHandle, SafeRequestIdH } } - RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(serviceHandle, requestHeaderHandle, responseHandle); + RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); if (ret != RCLRet.Ok) { RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); @@ -558,7 +549,7 @@ public static void SpinOnce (Node node, long timeout) { foreach (Subscription subscription in node.Subscriptions) { IRosMessage message = subscription.CreateMessage(); - bool result = Take (subscription.Handle, message); + bool result = Take(subscription, message); if (result) { subscription.TriggerCallback (message); } @@ -572,13 +563,13 @@ public static void SpinOnce (Node node, long timeout) { var request = service.CreateRequest(); var response = service.CreateResponse(); - var result = TakeRequest(service.Handle, requestIdHandle, request); + var result = TakeRequest(service, requestIdHandle, request); if (result) { // TODO: (sh) catch exceptions service.TriggerCallback(request, response); - SendResponse(service.Handle, requestIdHandle, response); + SendResponse(service, requestIdHandle, response); } } @@ -586,7 +577,7 @@ public static void SpinOnce (Node node, long timeout) { { var response = client.CreateResponse(); - var result = TakeResponse(client.Handle, requestIdHandle, response); + var result = TakeResponse(client, requestIdHandle, response); if (result) { var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); diff --git a/rcldotnet/Service.cs b/rcldotnet/Service.cs index ba31f962..99e65dbe 100644 --- a/rcldotnet/Service.cs +++ b/rcldotnet/Service.cs @@ -1,4 +1,5 @@ using System; +using System.Runtime.InteropServices; namespace ROS2 { @@ -20,9 +21,13 @@ internal Service() abstract internal SafeServiceHandle Handle { get; } abstract internal IRosMessage CreateRequest(); + + abstract internal SafeHandle CreateRequestHandle(); abstract internal IRosMessage CreateResponse(); + abstract internal SafeHandle CreateResponseHandle(); + abstract internal void TriggerCallback(IRosMessage request, IRosMessage response); } @@ -43,8 +48,12 @@ internal Service(SafeServiceHandle handle, Action callback) internal override IRosMessage CreateRequest() => (IRosMessage)new TRequest(); + internal override SafeHandle CreateRequestHandle() => MessageStaticMemberCache.CreateMessageHandle(); + internal override IRosMessage CreateResponse() => (IRosMessage)new TResponse(); + internal override SafeHandle CreateResponseHandle() => MessageStaticMemberCache.CreateMessageHandle(); + internal override void TriggerCallback(IRosMessage request, IRosMessage response) { _callback((TRequest)request, (TResponse)response); diff --git a/rcldotnet/ServiceDefinitionStaticMemberCache.cs b/rcldotnet/ServiceDefinitionStaticMemberCache.cs new file mode 100644 index 00000000..853fbdcc --- /dev/null +++ b/rcldotnet/ServiceDefinitionStaticMemberCache.cs @@ -0,0 +1,46 @@ +using System; +using System.Reflection; + +namespace ROS2 +{ + internal static class ServiceDefinitionStaticMemberCache + where TService : IRosServiceDefinition + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() + { + private static IntPtr s_typeSupport; + + static ServiceDefinitionStaticMemberCache() + { + TypeInfo typeInfo = typeof(TService).GetTypeInfo(); + + MethodInfo getTypeSupport = typeInfo.GetDeclaredMethod("__GetTypeSupport"); + if (getTypeSupport != null) + { + try + { + s_typeSupport = (IntPtr)getTypeSupport.Invoke(null, new object[] { }); + } + catch + { + s_typeSupport = IntPtr.Zero; + } + } + else + { + s_typeSupport = IntPtr.Zero; + } + } + + public static IntPtr GetTypeSupport() + { + // mehtod because it could throw. + if (s_typeSupport == IntPtr.Zero) + { + throw new InvalidOperationException($"Type '{typeof(TService).FullName}' did not define a correct __GetTypeSupport mehtod."); + } + + return s_typeSupport; + } + } +} diff --git a/rcldotnet/Subscription.cs b/rcldotnet/Subscription.cs index 9ea06974..e5de2084 100644 --- a/rcldotnet/Subscription.cs +++ b/rcldotnet/Subscription.cs @@ -14,6 +14,7 @@ */ using System; +using System.Runtime.InteropServices; namespace ROS2 { @@ -36,6 +37,8 @@ internal Subscription() internal abstract IRosMessage CreateMessage(); + internal abstract SafeHandle CreateMessageHandle(); + internal abstract void TriggerCallback(IRosMessage message); } @@ -52,6 +55,8 @@ internal Subscription(SafeSubscriptionHandle handle, Action callback) { internal override IRosMessage CreateMessage() => (IRosMessage)new T(); + internal override SafeHandle CreateMessageHandle() => MessageStaticMemberCache.CreateMessageHandle(); + internal override void TriggerCallback(IRosMessage message) { callback_((T) message); } From f87e1692360cc47b27a8b8c3633ab173789022df Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 9 Sep 2021 16:21:42 +0200 Subject: [PATCH 24/73] Improve error handling - move RCLExeptionHelper from rcldotnet_common to rcldotnet for acces to rcl_get_error_string() - get error string using rcl_get_error_string() and add this to the exception message - change throw helper to avoid unrachable code - also some other cleanup --- rcldotnet/CMakeLists.txt | 2 + rcldotnet/Client.cs | 16 +------ rcldotnet/Node.cs | 9 ++-- rcldotnet/Publisher.cs | 7 +--- rcldotnet/RCLExceptionHelper.cs | 44 +++++++++++++++++++ rcldotnet/RCLRet.cs | 55 ++++++++++++++++++++++++ rcldotnet/RCLdotnet.cs | 58 ++++++++++++++++---------- rcldotnet/SafeClientHandle.cs | 1 - rcldotnet/SafeNodeHandle.cs | 1 - rcldotnet/SafePublisherHandle.cs | 1 - rcldotnet/SafeRequestIdHandle.cs | 1 - rcldotnet/SafeServiceHandle.cs | 1 - rcldotnet/SafeSubscriptionHandle.cs | 1 - rcldotnet/SafeWaitSetHandle.cs | 1 - rcldotnet/rcldotnet.c | 12 ++++-- rcldotnet/rcldotnet.h | 6 +++ rcldotnet_common/CMakeLists.txt | 2 - rcldotnet_common/RCLExceptionHelper.cs | 36 ---------------- rcldotnet_common/RCLRet.cs | 55 ------------------------ 19 files changed, 160 insertions(+), 149 deletions(-) create mode 100644 rcldotnet/RCLExceptionHelper.cs create mode 100644 rcldotnet/RCLRet.cs delete mode 100644 rcldotnet_common/RCLExceptionHelper.cs delete mode 100644 rcldotnet_common/RCLRet.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index f62b9dc4..aeaa49d2 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -33,6 +33,8 @@ set(CS_SOURCES Node.cs Publisher.cs RCLdotnet.cs + RCLExceptionHelper.cs + RCLRet.cs SafeClientHandle.cs SafeNodeHandle.cs SafePublisherHandle.cs diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 62b0b2fc..bce327a8 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -2,7 +2,6 @@ using System.Collections.Concurrent; using System.Runtime.InteropServices; using System.Threading.Tasks; -using ROS2.Common; using ROS2.Utils; namespace ROS2 @@ -82,11 +81,7 @@ internal Client(SafeClientHandle handle, Node node) public bool ServiceIsReady() { RCLRet ret = ClientDelegates.native_rcl_service_server_is_available(_node.Handle, Handle, out var serviceIsReady); - if (ret != RCLRet.Ok) - { - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_service_server_is_available)}() failed."); - return false; // unrachable - } + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_service_server_is_available)}() failed."); return serviceIsReady; } @@ -94,9 +89,6 @@ public bool ServiceIsReady() public Task SendRequestAsync(TRequest request) { // TODO: (sh) Add cancellationToken(?), timeout (via cancellationToken?) and cleanup of pending requests. - // TODO: (sh) Catch all exceptions and return Task with error? - // How should this be done according to best practices. - long sequenceNumber; using (var requestHandle = MessageStaticMemberCache.CreateMessageHandle()) @@ -121,11 +113,7 @@ public Task SendRequestAsync(TRequest request) } RCLRet ret = ClientDelegates.native_rcl_send_request(Handle, requestHandle, out sequenceNumber); - if (ret != RCLRet.Ok) - { - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_send_request)}() failed."); - return null; // unrachable - } + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_send_request)}() failed."); } var taskCompletionSource = new TaskCompletionSource(); diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 963ced88..9d020b4a 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -16,7 +16,6 @@ using System; using System.Collections.Generic; using System.Runtime.InteropServices; -using ROS2.Common; using ROS2.Utils; namespace ROS2 { @@ -173,7 +172,7 @@ public Publisher CreatePublisher (string topic) where T : IRosMessage { if (ret != RCLRet.Ok) { publisherHandle.Dispose(); - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_publisher_handle)}() failed."); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_publisher_handle)}() failed."); } // TODO: (sh) Add topic as propety to Publisher. @@ -190,7 +189,7 @@ public Publisher CreatePublisher (string topic) where T : IRosMessage { if (ret != RCLRet.Ok) { subscriptionHandle.Dispose(); - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_subscription_handle)}() failed."); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_subscription_handle)}() failed."); } // TODO: (sh) Add topic as propety to Subscription. @@ -212,7 +211,7 @@ public Service CreateService CreateClient + * + * 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. + */ + +namespace ROS2 +{ + internal enum RCLRet + { + Ok = 0, + Error = 1, + Timeout = 2, + Unsupported = 3, + BadAlloc = 10, + InvalidArgument = 11, + AlreadyInit = 100, + NotInit = 101, + MismatchedRmwId = 102, + TopicNameInvalid = 103, + ServiceNameInvalid = 104, + UnknownSubstitution = 105, + AlreadyShutdown = 106, + NodeInvalid = 200, + NodeInvalidName = 201, + NodeInvalidNamespace = 202, + PublisherInvalid = 300, + SubscriptionInvalid = 400, + SubscriptionTakeFailed = 401, + ClientInvalid = 500, + ClientTakeFailed = 501, + ServiceInvalid = 600, + ServiceTakeFailed = 601, + TimerInvalid = 800, + TimerCanceled = 801, + WaitSetInvalid = 900, + WaitSetEmpty = 901, + WaitSetFull = 902, + InvalidRemapRule = 1001, + WrongLexme = 1002, + InvalidParamRule = 1010, + InvalidLogLevelRule = 1020, + EventInvalid = 2000, + EventTakeFailed = 2001 + } +} diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index e6ae98bf..260aec97 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -15,7 +15,6 @@ using System; using System.Runtime.InteropServices; -using ROS2.Common; using ROS2.Utils; namespace ROS2 { @@ -27,6 +26,17 @@ internal static class RCLdotnetDelegates { internal static NativeRCLInitType native_rcl_init = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate void NativeRCLGetErrorStringType( + byte[] buffer, int bufferSize); + + internal static NativeRCLGetErrorStringType native_rcl_get_error_string = null; + + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + internal delegate void NativeRCLResetErrorType (); + + internal static NativeRCLResetErrorType native_rcl_reset_error = null; + [UnmanagedFunctionPointer (CallingConvention.Cdecl)] internal delegate bool NativeRCLOkType (); @@ -134,6 +144,18 @@ static RCLdotnetDelegates () { (NativeRCLInitType) Marshal.GetDelegateForFunctionPointer ( native_rcl_init_ptr, typeof (NativeRCLInitType)); + IntPtr native_rcl_get_error_string_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_get_error_string"); + RCLdotnetDelegates.native_rcl_get_error_string = + (NativeRCLGetErrorStringType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_get_error_string_ptr, typeof (NativeRCLGetErrorStringType)); + + IntPtr native_rcl_reset_error_ptr = + dllLoadUtils.GetProcAddress (pDll, "native_rcl_reset_error"); + RCLdotnetDelegates.native_rcl_reset_error = + (NativeRCLResetErrorType) Marshal.GetDelegateForFunctionPointer ( + native_rcl_reset_error_ptr, typeof (NativeRCLResetErrorType)); + IntPtr native_rcl_get_rmw_identifier_ptr = dllLoadUtils.GetProcAddress (pDll, "native_rcl_get_rmw_identifier"); RCLdotnetDelegates.native_rcl_get_rmw_identifier = @@ -262,7 +284,7 @@ public static Node CreateNode (string nodeName, string nodeNamespace) { if (ret != RCLRet.Ok) { nodeHandle.Dispose(); - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_node_handle)}() failed."); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_node_handle)}() failed."); } Node node = new Node (nodeHandle); @@ -289,7 +311,7 @@ private static SafeWaitSetHandle CreateWaitSet( if (ret != RCLRet.Ok) { waitSetHandle.Dispose(); - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_wait_set_handle)}() failed."); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_wait_set_handle)}() failed."); } return waitSetHandle; @@ -297,22 +319,22 @@ private static SafeWaitSetHandle CreateWaitSet( private static void WaitSetClear(SafeWaitSetHandle waitSetHandle) { RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_clear(waitSetHandle); - RCLExceptionHelper.CheckReturnValue(ret); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_clear)}() failed."); } private static void WaitSetAddSubscription(SafeWaitSetHandle waitSetHandle, SafeSubscriptionHandle subscriptionHandle) { RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_subscription(waitSetHandle, subscriptionHandle); - RCLExceptionHelper.CheckReturnValue(ret); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_subscription)}() failed."); } private static void WaitSetAddService(SafeWaitSetHandle waitSetHandle, SafeServiceHandle serviceHandle) { RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); - RCLExceptionHelper.CheckReturnValue(ret); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_service)}() failed."); } private static void WaitSetAddClient(SafeWaitSetHandle waitSetHandle, SafeClientHandle clientHandle) { RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_client(waitSetHandle, clientHandle); - RCLExceptionHelper.CheckReturnValue(ret); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_client)}() failed."); } /// @@ -330,8 +352,7 @@ private static bool Wait(SafeWaitSetHandle waitSetHandle, long timeout) { } else { - RCLExceptionHelper.ThrowFromReturnValue(ret); - return false; // unreachable + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait)}() failed."); } } @@ -342,7 +363,7 @@ private static SafeRequestIdHandle CreateRequestId() if (ret != RCLRet.Ok) { requestIdHandle.Dispose(); - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_request_id_handle)}() failed."); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_request_id_handle)}() failed."); } return requestIdHandle; @@ -380,8 +401,7 @@ private static bool Take (Subscription subscription, IRosMessage message) { return false; default: - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take)}() failed."); - return false; // unrachable + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take)}() failed."); } } } @@ -418,8 +438,7 @@ private static bool TakeRequest(Service service, SafeRequestIdHandle requestHead return false; default: - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_request)}() failed."); - return false; // unrachable + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_request)}() failed."); } } } @@ -456,8 +475,7 @@ private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeade return false; default: - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_response)}() failed."); - return false; // unrachable + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_response)}() failed."); } } } @@ -488,8 +506,7 @@ private static void SendResponse(Service service, SafeRequestIdHandle requestHea RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); if (ret != RCLRet.Ok) { - RCLExceptionHelper.ThrowFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); - return; // unrachable + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); } } } @@ -566,7 +583,6 @@ public static void SpinOnce (Node node, long timeout) { var result = TakeRequest(service, requestIdHandle, request); if (result) { - // TODO: (sh) catch exceptions service.TriggerCallback(request, response); SendResponse(service, requestIdHandle, response); @@ -590,8 +606,8 @@ public static void SpinOnce (Node node, long timeout) { public static void Init () { lock (syncLock) { if (!initialized) { - // TODO: (sh) Handle return value - RCLRet ret = RCLdotnetDelegates.native_rcl_init (); + RCLRet ret = RCLdotnetDelegates.native_rcl_init(); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_init)}() failed."); initialized = true; } } diff --git a/rcldotnet/SafeClientHandle.cs b/rcldotnet/SafeClientHandle.cs index a59b7acc..c976f0ae 100644 --- a/rcldotnet/SafeClientHandle.cs +++ b/rcldotnet/SafeClientHandle.cs @@ -1,6 +1,5 @@ using System.Diagnostics; using Microsoft.Win32.SafeHandles; -using ROS2.Common; namespace ROS2 { diff --git a/rcldotnet/SafeNodeHandle.cs b/rcldotnet/SafeNodeHandle.cs index 8aabc0b5..cc964287 100644 --- a/rcldotnet/SafeNodeHandle.cs +++ b/rcldotnet/SafeNodeHandle.cs @@ -1,6 +1,5 @@ using System.Diagnostics; using Microsoft.Win32.SafeHandles; -using ROS2.Common; namespace ROS2 { diff --git a/rcldotnet/SafePublisherHandle.cs b/rcldotnet/SafePublisherHandle.cs index 6fe7a132..fed4859d 100644 --- a/rcldotnet/SafePublisherHandle.cs +++ b/rcldotnet/SafePublisherHandle.cs @@ -1,6 +1,5 @@ using System.Diagnostics; using Microsoft.Win32.SafeHandles; -using ROS2.Common; namespace ROS2 { diff --git a/rcldotnet/SafeRequestIdHandle.cs b/rcldotnet/SafeRequestIdHandle.cs index 31acb39c..28b6ba51 100644 --- a/rcldotnet/SafeRequestIdHandle.cs +++ b/rcldotnet/SafeRequestIdHandle.cs @@ -1,6 +1,5 @@ using System.Diagnostics; using Microsoft.Win32.SafeHandles; -using ROS2.Common; namespace ROS2 { diff --git a/rcldotnet/SafeServiceHandle.cs b/rcldotnet/SafeServiceHandle.cs index 94878797..336331e8 100644 --- a/rcldotnet/SafeServiceHandle.cs +++ b/rcldotnet/SafeServiceHandle.cs @@ -1,6 +1,5 @@ using System.Diagnostics; using Microsoft.Win32.SafeHandles; -using ROS2.Common; namespace ROS2 { diff --git a/rcldotnet/SafeSubscriptionHandle.cs b/rcldotnet/SafeSubscriptionHandle.cs index eae81db7..5457574f 100644 --- a/rcldotnet/SafeSubscriptionHandle.cs +++ b/rcldotnet/SafeSubscriptionHandle.cs @@ -1,6 +1,5 @@ using System.Diagnostics; using Microsoft.Win32.SafeHandles; -using ROS2.Common; namespace ROS2 { diff --git a/rcldotnet/SafeWaitSetHandle.cs b/rcldotnet/SafeWaitSetHandle.cs index 87a67b45..e1b9fb93 100644 --- a/rcldotnet/SafeWaitSetHandle.cs +++ b/rcldotnet/SafeWaitSetHandle.cs @@ -1,6 +1,5 @@ using System.Diagnostics; using Microsoft.Win32.SafeHandles; -using ROS2.Common; namespace ROS2 { diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index bb20e8fe..ad955536 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -45,11 +45,17 @@ const char *native_rcl_get_rmw_identifier() { return rmw_get_implementation_identifier(); } -const char *native_rcl_get_error_string() { - return rcl_get_error_string().str; +void native_rcl_get_error_string(char *buffer, int32_t bufferSize) { + size_t minBufferSize = (size_t)bufferSize < (size_t)RCUTILS_ERROR_MESSAGE_MAX_LENGTH + ? (size_t)bufferSize + : (size_t)RCUTILS_ERROR_MESSAGE_MAX_LENGTH; + + strncpy(buffer, rcl_get_error_string().str, minBufferSize); } -void native_rcl_reset_error() { rcl_reset_error(); } +void native_rcl_reset_error(void) { + rcl_reset_error(); +} bool native_rcl_ok() { return rcl_context_is_valid(&context); } diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 4a44df31..c6283a94 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -23,6 +23,12 @@ int32_t RCLDOTNET_CDECL native_rcl_init(); RCLDOTNET_EXPORT const char * RCLDOTNET_CDECL native_rcl_get_rmw_identifier(); +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_get_error_string(char *buffer, int32_t bufferSize); + +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_reset_error(void); + RCLDOTNET_EXPORT bool RCLDOTNET_CDECL native_rcl_ok(); diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index 578258fb..246c87b2 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -12,8 +12,6 @@ set(CS_SOURCES DllLoadUtils.cs IRosMessage.cs IRosServiceDefinition.cs - RCLExceptionHelper.cs - RCLRet.cs ) add_dotnet_library(${PROJECT_NAME} ${CS_SOURCES}) diff --git a/rcldotnet_common/RCLExceptionHelper.cs b/rcldotnet_common/RCLExceptionHelper.cs deleted file mode 100644 index e8df7299..00000000 --- a/rcldotnet_common/RCLExceptionHelper.cs +++ /dev/null @@ -1,36 +0,0 @@ -using System; - -namespace ROS2.Common -{ - // TODO: (sh) Check return values to throw the appropirate Exceptions arcording - // to the Framework Design Guidlines. - // https://docs.microsoft.com/en-us/dotnet/standard/design-guidelines/exceptions - - // TODO: (sh) Use rcl_get_error_state() for additional information. - public static class RCLExceptionHelper - { - public static void ThrowFromReturnValue(RCLRet value, string messagePrefix = null) - { - if (value == RCLRet.Ok) - { - throw new ArgumentException($"The return value must not be Ok.", nameof(value)); - } - - var message = messagePrefix == null - ? value.ToString() - : messagePrefix + " " + value.ToString(); - - throw new Exception(message); - } - - public static void CheckReturnValue(RCLRet value, string messagePrefix = null) - { - if (value == RCLRet.Ok) - { - return; - } - - ThrowFromReturnValue(value, messagePrefix); - } - } -} diff --git a/rcldotnet_common/RCLRet.cs b/rcldotnet_common/RCLRet.cs deleted file mode 100644 index b87fa0e7..00000000 --- a/rcldotnet_common/RCLRet.cs +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright 2016-2018 Esteve Fernandez - * - * 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. - */ - -namespace ROS2 { - namespace Common { - public enum RCLRet { - Ok = 0, - Error = 1, - Timeout = 2, - Unsupported = 3, - BadAlloc = 10, - InvalidArgument = 11, - AlreadyInit = 100, - NotInit = 101, - MismatchedRmwId = 102, - TopicNameInvalid = 103, - ServiceNameInvalid = 104, - UnknownSubstitution = 105, - AlreadyShutdown = 106, - NodeInvalid = 200, - NodeInvalidName = 201, - NodeInvalidNamespace = 202, - PublisherInvalid = 300, - SubscriptionInvalid = 400, - SubscriptionTakeFailed = 401, - ClientInvalid = 500, - ClientTakeFailed = 501, - ServiceInvalid = 600, - ServiceTakeFailed = 601, - TimerInvalid = 800, - TimerCanceled = 801, - WaitSetInvalid = 900, - WaitSetEmpty = 901, - WaitSetFull = 902, - InvalidRemapRule = 1001, - WrongLexme = 1002, - InvalidParamRule = 1010, - InvalidLogLevelRule = 1020, - EventInvalid = 2000, - EventTakeFailed = 2001 - } - } -} From 91f8e018932f5e43a17d895c7e3d3767738d84c0 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 12 Oct 2021 07:46:26 +0000 Subject: [PATCH 25/73] Rename request variable to solve a CS0136 error: error CS0136: A local or parameter named 'request' cannot be declared in this scope because that name is used in an enclosing local scope to define a local or parameter --- rcldotnet/test/test_services.cs | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/rcldotnet/test/test_services.cs b/rcldotnet/test/test_services.cs index e1743193..5b59d71e 100644 --- a/rcldotnet/test/test_services.cs +++ b/rcldotnet/test/test_services.cs @@ -78,24 +78,24 @@ public void TestServiceAndClient() Assert.Equal((ulong)47, clientReceivedResponse.Uint64_value); Assert.Equal("one", clientReceivedResponse.String_value); - void HandleRequest(test_msgs.srv.BasicTypes_Request request, test_msgs.srv.BasicTypes_Response response) + void HandleRequest(test_msgs.srv.BasicTypes_Request req, test_msgs.srv.BasicTypes_Response response) { - serviceReceivedRequest = request; + serviceReceivedRequest = req; - response.Bool_value = request.Bool_value; - response.Byte_value = request.Byte_value; - response.Char_value = request.Char_value; - response.Float32_value = request.Float32_value; - response.Float64_value = request.Float64_value; - response.Int8_value = request.Int8_value; - response.Uint8_value = request.Uint8_value; - response.Int16_value = request.Int16_value; - response.Uint16_value = request.Uint16_value; - response.Int32_value = request.Int32_value + 100; - response.Uint32_value = request.Uint32_value; - response.Int64_value = request.Int64_value; - response.Uint64_value = request.Uint64_value; - response.String_value = request.String_value; + response.Bool_value = req.Bool_value; + response.Byte_value = req.Byte_value; + response.Char_value = req.Char_value; + response.Float32_value = req.Float32_value; + response.Float64_value = req.Float64_value; + response.Int8_value = req.Int8_value; + response.Uint8_value = req.Uint8_value; + response.Int16_value = req.Int16_value; + response.Uint16_value = req.Uint16_value; + response.Int32_value = req.Int32_value + 100; + response.Uint32_value = req.Uint32_value; + response.Int64_value = req.Int64_value; + response.Uint64_value = req.Uint64_value; + response.String_value = req.String_value; } } } From 67a732bdca958ed3ef359b4c79819838158b56ae Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 26 Jan 2022 09:58:32 +0100 Subject: [PATCH 26/73] Use `const` instead of `static readonly` for constants This allows them to be used in switch cases and other places. --- rosidl_generator_dotnet/resource/msg.cs.em | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 963387c2..bb5b336c 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -328,7 +328,7 @@ public class @(type_name) : global::ROS2.IRosMessage { } @[for constant in message.constants]@ - public static readonly @(get_dotnet_type(constant.type)) @(constant.name) = + public const @(get_dotnet_type(constant.type)) @(constant.name) = @(constant_value_to_dotnet(constant.type, constant.value)); @[end for]@ From 849eca52eed117043d0837dca550ca0f0dbde9f2 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Apr 2022 19:05:51 +0200 Subject: [PATCH 27/73] Add licence headers and entry in NOTICE --- NOTICE | 1 + rcldotnet/Client.cs | 15 +++++++++++++++ rcldotnet/MessageStaticMemberCache.cs | 15 +++++++++++++++ rcldotnet/RCLExceptionHelper.cs | 15 +++++++++++++++ rcldotnet/SafeClientHandle.cs | 15 +++++++++++++++ rcldotnet/SafeNodeHandle.cs | 15 +++++++++++++++ rcldotnet/SafePublisherHandle.cs | 15 +++++++++++++++ rcldotnet/SafeRequestIdHandle.cs | 15 +++++++++++++++ rcldotnet/SafeServiceHandle.cs | 15 +++++++++++++++ rcldotnet/SafeSubscriptionHandle.cs | 15 +++++++++++++++ rcldotnet/SafeWaitSetHandle.cs | 15 +++++++++++++++ rcldotnet/Service.cs | 15 +++++++++++++++ rcldotnet/ServiceDefinitionStaticMemberCache.cs | 15 +++++++++++++++ rcldotnet_common/IRosMessage.cs | 15 +++++++++++++++ rcldotnet_common/IRosServiceDefinition.cs | 15 +++++++++++++++ 15 files changed, 211 insertions(+) diff --git a/NOTICE b/NOTICE index 09be901b..4a88c0da 100644 --- a/NOTICE +++ b/NOTICE @@ -3,3 +3,4 @@ Copyright 2016-2018 Esteve Fernandez (esteve@apache.org) This product includes software developed by Esteve Fernandez (esteve@apache.org) +Stefan Hoffmann (stefan.hoffmann@schiller.de) diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index bce327a8..fd7d2a35 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Collections.Concurrent; using System.Runtime.InteropServices; diff --git a/rcldotnet/MessageStaticMemberCache.cs b/rcldotnet/MessageStaticMemberCache.cs index 59756c77..0bade221 100644 --- a/rcldotnet/MessageStaticMemberCache.cs +++ b/rcldotnet/MessageStaticMemberCache.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Reflection; using System.Runtime.InteropServices; diff --git a/rcldotnet/RCLExceptionHelper.cs b/rcldotnet/RCLExceptionHelper.cs index f4b9e04a..fd0684d7 100644 --- a/rcldotnet/RCLExceptionHelper.cs +++ b/rcldotnet/RCLExceptionHelper.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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; namespace ROS2 diff --git a/rcldotnet/SafeClientHandle.cs b/rcldotnet/SafeClientHandle.cs index c976f0ae..c5400eff 100644 --- a/rcldotnet/SafeClientHandle.cs +++ b/rcldotnet/SafeClientHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/SafeNodeHandle.cs b/rcldotnet/SafeNodeHandle.cs index cc964287..2c6ea769 100644 --- a/rcldotnet/SafeNodeHandle.cs +++ b/rcldotnet/SafeNodeHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/SafePublisherHandle.cs b/rcldotnet/SafePublisherHandle.cs index fed4859d..a51d3003 100644 --- a/rcldotnet/SafePublisherHandle.cs +++ b/rcldotnet/SafePublisherHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/SafeRequestIdHandle.cs b/rcldotnet/SafeRequestIdHandle.cs index 28b6ba51..896c1da4 100644 --- a/rcldotnet/SafeRequestIdHandle.cs +++ b/rcldotnet/SafeRequestIdHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/SafeServiceHandle.cs b/rcldotnet/SafeServiceHandle.cs index 336331e8..3307137b 100644 --- a/rcldotnet/SafeServiceHandle.cs +++ b/rcldotnet/SafeServiceHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/SafeSubscriptionHandle.cs b/rcldotnet/SafeSubscriptionHandle.cs index 5457574f..77d0738f 100644 --- a/rcldotnet/SafeSubscriptionHandle.cs +++ b/rcldotnet/SafeSubscriptionHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/SafeWaitSetHandle.cs b/rcldotnet/SafeWaitSetHandle.cs index e1b9fb93..3a6c0945 100644 --- a/rcldotnet/SafeWaitSetHandle.cs +++ b/rcldotnet/SafeWaitSetHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/Service.cs b/rcldotnet/Service.cs index 99e65dbe..dbfebf9d 100644 --- a/rcldotnet/Service.cs +++ b/rcldotnet/Service.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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; diff --git a/rcldotnet/ServiceDefinitionStaticMemberCache.cs b/rcldotnet/ServiceDefinitionStaticMemberCache.cs index 853fbdcc..20cb0abc 100644 --- a/rcldotnet/ServiceDefinitionStaticMemberCache.cs +++ b/rcldotnet/ServiceDefinitionStaticMemberCache.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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.Reflection; diff --git a/rcldotnet_common/IRosMessage.cs b/rcldotnet_common/IRosMessage.cs index 534a3141..87dc0751 100644 --- a/rcldotnet_common/IRosMessage.cs +++ b/rcldotnet_common/IRosMessage.cs @@ -1,3 +1,18 @@ +/* Copyright 2016-2018 Esteve Fernandez + * + * 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; namespace ROS2 diff --git a/rcldotnet_common/IRosServiceDefinition.cs b/rcldotnet_common/IRosServiceDefinition.cs index 06b74410..658c608b 100644 --- a/rcldotnet_common/IRosServiceDefinition.cs +++ b/rcldotnet_common/IRosServiceDefinition.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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. + */ + namespace ROS2 { public interface IRosServiceDefinition From 79a338d8c04e8240e651dd31b274bfa75aa8d15b Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Apr 2022 19:06:23 +0200 Subject: [PATCH 28/73] Update README.md --- README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/README.md b/README.md index 27e9f12a..c7bbc41e 100644 --- a/README.md +++ b/README.md @@ -22,6 +22,7 @@ Features The current set of features include: - Generation of all builtin ROS types - Support for publishers and subscriptions +- Support for clients and services - Cross-platform support (Linux, Windows, Windows IoT Core, UWP) What's missing? @@ -30,9 +31,7 @@ What's missing? Lots of things! - Unicode types - String constants (specifically BoundedString) -- Nested types - Component nodes -- Clients and services - Actions - Tests - Documentation From 06f231e65f80c9e265ac982e8114a02722dc1145 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 14 Jun 2022 16:44:38 +0200 Subject: [PATCH 29/73] Use CamelCase for property names --- rcldotnet/test/test_messages.cs | 1508 ++++++++--------- rcldotnet/test/test_services.cs | 127 +- rosidl_generator_dotnet/resource/msg.cs.em | 4 +- .../rosidl_generator_dotnet/__init__.py | 10 +- 4 files changed, 832 insertions(+), 817 deletions(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index f0668fcc..1270064e 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -48,10 +48,10 @@ public void TestPublishBuiltins() test_msgs.msg.Builtins msg = new test_msgs.msg.Builtins (); test_msgs.msg.Builtins msg2 = new test_msgs.msg.Builtins (); - msg.Duration_value.Sec = 1; - msg.Duration_value.Nanosec = 2u; - msg.Time_value.Sec = 3; - msg.Time_value.Nanosec = 4u; + msg.DurationValue.Sec = 1; + msg.DurationValue.Nanosec = 2u; + msg.TimeValue.Sec = 3; + msg.TimeValue.Nanosec = 4u; bool received=false; Subscription chatter_sub = node_builtins_2.CreateSubscription ( @@ -70,10 +70,10 @@ public void TestPublishBuiltins() RCLdotnet.SpinOnce(node_builtins_2, 500); } - Assert.Equal(1, msg2.Duration_value.Sec); - Assert.Equal(2u, msg2.Duration_value.Nanosec); - Assert.Equal(3, msg2.Time_value.Sec); - Assert.Equal(4u, msg2.Time_value.Nanosec); + Assert.Equal(1, msg2.DurationValue.Sec); + Assert.Equal(2u, msg2.DurationValue.Nanosec); + Assert.Equal(3, msg2.TimeValue.Sec); + Assert.Equal(4u, msg2.TimeValue.Nanosec); } [Fact] @@ -87,125 +87,125 @@ public void TestPublishArrays() test_msgs.msg.Arrays msg = new test_msgs.msg.Arrays (); test_msgs.msg.Arrays msg2 = new test_msgs.msg.Arrays (); - // bool_values - msg.Bool_values[0] = true; - msg.Bool_values[1] = false; - msg.Bool_values[2] = true; - - // byte_values - msg.Byte_values[0] = 0; - msg.Byte_values[1] = 1; - msg.Byte_values[2] = 2; - - // char_values - msg.Char_values[0] = 3; - msg.Char_values[1] = 4; - msg.Char_values[2] = 5; - - // float32_values - msg.Float32_values[0] = 6.1f; - msg.Float32_values[1] = 7.1f; - msg.Float32_values[2] = 8.1f; - - // float64_values - msg.Float64_values[0] = 9.1; - msg.Float64_values[1] = 10.1; - msg.Float64_values[2] = 11.1; - - // int8_values - msg.Int8_values[0] = 12; - msg.Int8_values[1] = 13; - msg.Int8_values[2] = 14; - - // uint8_values - msg.Uint8_values[0] = 15; - msg.Uint8_values[1] = 16; - msg.Uint8_values[2] = 17; - - // int16_values - msg.Int16_values[0] = 18; - msg.Int16_values[1] = 19; - msg.Int16_values[2] = 20; - - // uint16_values - msg.Uint16_values[0] = 21; - msg.Uint16_values[1] = 22; - msg.Uint16_values[2] = 23; - - // int32_values - msg.Int32_values[0] = 24; - msg.Int32_values[1] = 25; - msg.Int32_values[2] = 26; - - // uint32_values - msg.Uint32_values[0] = 27; - msg.Uint32_values[1] = 28; - msg.Uint32_values[2] = 29; - - // int64_values - msg.Int64_values[0] = 30; - msg.Int64_values[1] = 31; - msg.Int64_values[2] = 32; - - // uint64_values - msg.Uint64_values[0] = 33; - msg.Uint64_values[1] = 34; - msg.Uint64_values[2] = 35; - - // string_values - msg.String_values[0] = "one"; - msg.String_values[1] = "two"; - msg.String_values[2] = "three"; - - // basic_types_values + // boolValues + msg.BoolValues[0] = true; + msg.BoolValues[1] = false; + msg.BoolValues[2] = true; + + // byteValues + msg.ByteValues[0] = 0; + msg.ByteValues[1] = 1; + msg.ByteValues[2] = 2; + + // charValues + msg.CharValues[0] = 3; + msg.CharValues[1] = 4; + msg.CharValues[2] = 5; + + // float32Values + msg.Float32Values[0] = 6.1f; + msg.Float32Values[1] = 7.1f; + msg.Float32Values[2] = 8.1f; + + // float64Values + msg.Float64Values[0] = 9.1; + msg.Float64Values[1] = 10.1; + msg.Float64Values[2] = 11.1; + + // int8Values + msg.Int8Values[0] = 12; + msg.Int8Values[1] = 13; + msg.Int8Values[2] = 14; + + // uint8Values + msg.Uint8Values[0] = 15; + msg.Uint8Values[1] = 16; + msg.Uint8Values[2] = 17; + + // int16Values + msg.Int16Values[0] = 18; + msg.Int16Values[1] = 19; + msg.Int16Values[2] = 20; + + // uint16Values + msg.Uint16Values[0] = 21; + msg.Uint16Values[1] = 22; + msg.Uint16Values[2] = 23; + + // int32Values + msg.Int32Values[0] = 24; + msg.Int32Values[1] = 25; + msg.Int32Values[2] = 26; + + // uint32Values + msg.Uint32Values[0] = 27; + msg.Uint32Values[1] = 28; + msg.Uint32Values[2] = 29; + + // int64Values + msg.Int64Values[0] = 30; + msg.Int64Values[1] = 31; + msg.Int64Values[2] = 32; + + // uint64Values + msg.Uint64Values[0] = 33; + msg.Uint64Values[1] = 34; + msg.Uint64Values[2] = 35; + + // stringValues + msg.StringValues[0] = "one"; + msg.StringValues[1] = "two"; + msg.StringValues[2] = "three"; + + // basicTypesValues test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.Bool_value = true; - basic_type_1.Byte_value = 36; - basic_type_1.Char_value = 37; - basic_type_1.Float32_value = 38.1f; - basic_type_1.Float64_value = 39.1; - basic_type_1.Int8_value = 40; - basic_type_1.Uint8_value = 41; - basic_type_1.Int16_value = 42; - basic_type_1.Uint16_value = 43; - basic_type_1.Int32_value = 44; - basic_type_1.Uint32_value = 45; - basic_type_1.Int64_value = 46; - basic_type_1.Uint64_value = 47; + basic_type_1.BoolValue = true; + basic_type_1.ByteValue = 36; + basic_type_1.CharValue = 37; + basic_type_1.Float32Value = 38.1f; + basic_type_1.Float64Value = 39.1; + basic_type_1.Int8Value = 40; + basic_type_1.Uint8Value = 41; + basic_type_1.Int16Value = 42; + basic_type_1.Uint16Value = 43; + basic_type_1.Int32Value = 44; + basic_type_1.Uint32Value = 45; + basic_type_1.Int64Value = 46; + basic_type_1.Uint64Value = 47; test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.Bool_value = false; - basic_type_2.Byte_value = 48; - basic_type_2.Char_value = 49; - basic_type_2.Float32_value = 50.1f; - basic_type_2.Float64_value = 51.1; - basic_type_2.Int8_value = 52; - basic_type_2.Uint8_value = 53; - basic_type_2.Int16_value = 54; - basic_type_2.Uint16_value = 55; - basic_type_2.Int32_value = 56; - basic_type_2.Uint32_value = 57; - basic_type_2.Int64_value = 58; - basic_type_2.Uint64_value = 59; + basic_type_2.BoolValue = false; + basic_type_2.ByteValue = 48; + basic_type_2.CharValue = 49; + basic_type_2.Float32Value = 50.1f; + basic_type_2.Float64Value = 51.1; + basic_type_2.Int8Value = 52; + basic_type_2.Uint8Value = 53; + basic_type_2.Int16Value = 54; + basic_type_2.Uint16Value = 55; + basic_type_2.Int32Value = 56; + basic_type_2.Uint32Value = 57; + basic_type_2.Int64Value = 58; + basic_type_2.Uint64Value = 59; test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.Bool_value = true; - basic_type_3.Byte_value = 60; - basic_type_3.Char_value = 61; - basic_type_3.Float32_value = 62.1f; - basic_type_3.Float64_value = 63.1; - basic_type_3.Int8_value = 64; - basic_type_3.Uint8_value = 65; - basic_type_3.Int16_value = 66; - basic_type_3.Uint16_value = 67; - basic_type_3.Int32_value = 68; - basic_type_3.Uint32_value = 69; - basic_type_3.Int64_value = 70; - basic_type_3.Uint64_value = 71; - - msg.Basic_types_values[0] = basic_type_1; - msg.Basic_types_values[1] = basic_type_2; - msg.Basic_types_values[2] = basic_type_3; + basic_type_3.BoolValue = true; + basic_type_3.ByteValue = 60; + basic_type_3.CharValue = 61; + basic_type_3.Float32Value = 62.1f; + basic_type_3.Float64Value = 63.1; + basic_type_3.Int8Value = 64; + basic_type_3.Uint8Value = 65; + basic_type_3.Int16Value = 66; + basic_type_3.Uint16Value = 67; + basic_type_3.Int32Value = 68; + basic_type_3.Uint32Value = 69; + basic_type_3.Int64Value = 70; + basic_type_3.Uint64Value = 71; + + msg.BasicTypesValues[0] = basic_type_1; + msg.BasicTypesValues[1] = basic_type_2; + msg.BasicTypesValues[2] = basic_type_3; bool received=false; Subscription chatter_sub = node_array_2.CreateSubscription ( @@ -224,149 +224,149 @@ public void TestPublishArrays() RCLdotnet.SpinOnce(node_array_2, 500); } - // bool_values - Assert.Equal(3, test_msgs.msg.Arrays.Bool_values_Length); - Assert.Equal(3, msg2.Bool_values.Length); - Assert.True(msg2.Bool_values[0]); - Assert.False(msg2.Bool_values[1]); - Assert.True(msg2.Bool_values[2]); - - // byte_values - Assert.Equal(3, test_msgs.msg.Arrays.Byte_values_Length); - Assert.Equal(3, msg2.Byte_values.Length); - Assert.Equal(0, msg2.Byte_values[0]); - Assert.Equal(1, msg2.Byte_values[1]); - Assert.Equal(2, msg2.Byte_values[2]); - - // char_values - Assert.Equal(3, test_msgs.msg.Arrays.Char_values_Length); - Assert.Equal(3, msg2.Char_values.Length); - Assert.Equal(3, msg2.Char_values[0]); - Assert.Equal(4, msg2.Char_values[1]); - Assert.Equal(5, msg2.Char_values[2]); - - // float32_values - Assert.Equal(3, test_msgs.msg.Arrays.Float32_values_Length); - Assert.Equal(3, msg2.Float32_values.Length); - Assert.Equal(6.1f, msg2.Float32_values[0]); - Assert.Equal(7.1f, msg2.Float32_values[1]); - Assert.Equal(8.1f, msg2.Float32_values[2]); - - // float64_values - Assert.Equal(3, test_msgs.msg.Arrays.Float64_values_Length); - Assert.Equal(3, msg2.Float64_values.Length); - Assert.Equal(9.1, msg2.Float64_values[0]); - Assert.Equal(10.1, msg2.Float64_values[1]); - Assert.Equal(11.1, msg2.Float64_values[2]); - - // int8_values - Assert.Equal(3, test_msgs.msg.Arrays.Int8_values_Length); - Assert.Equal(3, msg2.Int8_values.Length); - Assert.Equal(12, msg2.Int8_values[0]); - Assert.Equal(13, msg2.Int8_values[1]); - Assert.Equal(14, msg2.Int8_values[2]); - - // uint8_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint8_values_Length); - Assert.Equal(3, msg2.Uint8_values.Length); - Assert.Equal(15, msg2.Uint8_values[0]); - Assert.Equal(16, msg2.Uint8_values[1]); - Assert.Equal(17, msg2.Uint8_values[2]); - - // int16_values - Assert.Equal(3, test_msgs.msg.Arrays.Int16_values_Length); - Assert.Equal(3, msg2.Int16_values.Length); - Assert.Equal(18, msg2.Int16_values[0]); - Assert.Equal(19, msg2.Int16_values[1]); - Assert.Equal(20, msg2.Int16_values[2]); - - // uint16_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint16_values_Length); - Assert.Equal(3, msg2.Uint16_values.Length); - Assert.Equal(21, msg2.Uint16_values[0]); - Assert.Equal(22, msg2.Uint16_values[1]); - Assert.Equal(23, msg2.Uint16_values[2]); - - // int32_values - Assert.Equal(3, test_msgs.msg.Arrays.Int32_values_Length); - Assert.Equal(3, msg2.Int32_values.Length); - Assert.Equal(24, msg2.Int32_values[0]); - Assert.Equal(25, msg2.Int32_values[1]); - Assert.Equal(26, msg2.Int32_values[2]); - - // uint32_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint32_values_Length); - Assert.Equal(3, msg2.Uint32_values.Length); - Assert.Equal((uint)27, msg2.Uint32_values[0]); - Assert.Equal((uint)28, msg2.Uint32_values[1]); - Assert.Equal((uint)29, msg2.Uint32_values[2]); - - // int64_values - Assert.Equal(3, test_msgs.msg.Arrays.Int64_values_Length); - Assert.Equal(3, msg2.Int64_values.Length); - Assert.Equal(30, msg2.Int64_values[0]); - Assert.Equal(31, msg2.Int64_values[1]); - Assert.Equal(32, msg2.Int64_values[2]); - - // uint64_values - Assert.Equal(3, test_msgs.msg.Arrays.Uint64_values_Length); - Assert.Equal(3, msg2.Uint64_values.Length); - Assert.Equal((ulong)33, msg2.Uint64_values[0]); - Assert.Equal((ulong)34, msg2.Uint64_values[1]); - Assert.Equal((ulong)35, msg2.Uint64_values[2]); - - // string_values - Assert.Equal(3, test_msgs.msg.Arrays.String_values_Length); - Assert.Equal(3, msg2.String_values.Length); - Assert.Equal("one", msg2.String_values[0]); - Assert.Equal("two", msg2.String_values[1]); - Assert.Equal("three", msg2.String_values[2]); - - // basic_types_values - Assert.Equal(3, test_msgs.msg.Arrays.Basic_types_values_Length); - Assert.Equal(3, msg2.Basic_types_values.Length); - - Assert.True(msg2.Basic_types_values[0].Bool_value); - Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); - Assert.Equal(37, msg2.Basic_types_values[0].Char_value); - Assert.Equal(38.1f, msg2.Basic_types_values[0].Float32_value); - Assert.Equal(39.1, msg2.Basic_types_values[0].Float64_value); - Assert.Equal(40, msg2.Basic_types_values[0].Int8_value); - Assert.Equal(41, msg2.Basic_types_values[0].Uint8_value); - Assert.Equal(42, msg2.Basic_types_values[0].Int16_value); - Assert.Equal(43, msg2.Basic_types_values[0].Uint16_value); - Assert.Equal(44, msg2.Basic_types_values[0].Int32_value); - Assert.Equal((uint)45, msg2.Basic_types_values[0].Uint32_value); - Assert.Equal(46, msg2.Basic_types_values[0].Int64_value); - Assert.Equal((ulong)47, msg2.Basic_types_values[0].Uint64_value); - - Assert.False(msg2.Basic_types_values[1].Bool_value); - Assert.Equal(48, msg2.Basic_types_values[1].Byte_value); - Assert.Equal(49, msg2.Basic_types_values[1].Char_value); - Assert.Equal(50.1f, msg2.Basic_types_values[1].Float32_value); - Assert.Equal(51.1, msg2.Basic_types_values[1].Float64_value); - Assert.Equal(52, msg2.Basic_types_values[1].Int8_value); - Assert.Equal(53, msg2.Basic_types_values[1].Uint8_value); - Assert.Equal(54, msg2.Basic_types_values[1].Int16_value); - Assert.Equal(55, msg2.Basic_types_values[1].Uint16_value); - Assert.Equal(56, msg2.Basic_types_values[1].Int32_value); - Assert.Equal((uint)57, msg2.Basic_types_values[1].Uint32_value); - Assert.Equal(58, msg2.Basic_types_values[1].Int64_value); - Assert.Equal((ulong)59, msg2.Basic_types_values[1].Uint64_value); - - Assert.True(msg2.Basic_types_values[2].Bool_value); - Assert.Equal(60, msg2.Basic_types_values[2].Byte_value); - Assert.Equal(61, msg2.Basic_types_values[2].Char_value); - Assert.Equal(62.1f, msg2.Basic_types_values[2].Float32_value); - Assert.Equal(63.1, msg2.Basic_types_values[2].Float64_value); - Assert.Equal(64, msg2.Basic_types_values[2].Int8_value); - Assert.Equal(65, msg2.Basic_types_values[2].Uint8_value); - Assert.Equal(66, msg2.Basic_types_values[2].Int16_value); - Assert.Equal(67, msg2.Basic_types_values[2].Uint16_value); - Assert.Equal(68, msg2.Basic_types_values[2].Int32_value); - Assert.Equal((uint)69, msg2.Basic_types_values[2].Uint32_value); - Assert.Equal(70, msg2.Basic_types_values[2].Int64_value); - Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); + // boolValues + Assert.Equal(3, test_msgs.msg.Arrays.BoolValuesLength); + Assert.Equal(3, msg2.BoolValues.Length); + Assert.True(msg2.BoolValues[0]); + Assert.False(msg2.BoolValues[1]); + Assert.True(msg2.BoolValues[2]); + + // byteValues + Assert.Equal(3, test_msgs.msg.Arrays.ByteValuesLength); + Assert.Equal(3, msg2.ByteValues.Length); + Assert.Equal(0, msg2.ByteValues[0]); + Assert.Equal(1, msg2.ByteValues[1]); + Assert.Equal(2, msg2.ByteValues[2]); + + // charValues + Assert.Equal(3, test_msgs.msg.Arrays.CharValuesLength); + Assert.Equal(3, msg2.CharValues.Length); + Assert.Equal(3, msg2.CharValues[0]); + Assert.Equal(4, msg2.CharValues[1]); + Assert.Equal(5, msg2.CharValues[2]); + + // float32Values + Assert.Equal(3, test_msgs.msg.Arrays.Float32ValuesLength); + Assert.Equal(3, msg2.Float32Values.Length); + Assert.Equal(6.1f, msg2.Float32Values[0]); + Assert.Equal(7.1f, msg2.Float32Values[1]); + Assert.Equal(8.1f, msg2.Float32Values[2]); + + // float64Values + Assert.Equal(3, test_msgs.msg.Arrays.Float64ValuesLength); + Assert.Equal(3, msg2.Float64Values.Length); + Assert.Equal(9.1, msg2.Float64Values[0]); + Assert.Equal(10.1, msg2.Float64Values[1]); + Assert.Equal(11.1, msg2.Float64Values[2]); + + // int8Values + Assert.Equal(3, test_msgs.msg.Arrays.Int8ValuesLength); + Assert.Equal(3, msg2.Int8Values.Length); + Assert.Equal(12, msg2.Int8Values[0]); + Assert.Equal(13, msg2.Int8Values[1]); + Assert.Equal(14, msg2.Int8Values[2]); + + // uint8Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint8ValuesLength); + Assert.Equal(3, msg2.Uint8Values.Length); + Assert.Equal(15, msg2.Uint8Values[0]); + Assert.Equal(16, msg2.Uint8Values[1]); + Assert.Equal(17, msg2.Uint8Values[2]); + + // int16Values + Assert.Equal(3, test_msgs.msg.Arrays.Int16ValuesLength); + Assert.Equal(3, msg2.Int16Values.Length); + Assert.Equal(18, msg2.Int16Values[0]); + Assert.Equal(19, msg2.Int16Values[1]); + Assert.Equal(20, msg2.Int16Values[2]); + + // uint16Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint16ValuesLength); + Assert.Equal(3, msg2.Uint16Values.Length); + Assert.Equal(21, msg2.Uint16Values[0]); + Assert.Equal(22, msg2.Uint16Values[1]); + Assert.Equal(23, msg2.Uint16Values[2]); + + // int32Values + Assert.Equal(3, test_msgs.msg.Arrays.Int32ValuesLength); + Assert.Equal(3, msg2.Int32Values.Length); + Assert.Equal(24, msg2.Int32Values[0]); + Assert.Equal(25, msg2.Int32Values[1]); + Assert.Equal(26, msg2.Int32Values[2]); + + // uint32Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint32ValuesLength); + Assert.Equal(3, msg2.Uint32Values.Length); + Assert.Equal((uint)27, msg2.Uint32Values[0]); + Assert.Equal((uint)28, msg2.Uint32Values[1]); + Assert.Equal((uint)29, msg2.Uint32Values[2]); + + // int64Values + Assert.Equal(3, test_msgs.msg.Arrays.Int64ValuesLength); + Assert.Equal(3, msg2.Int64Values.Length); + Assert.Equal(30, msg2.Int64Values[0]); + Assert.Equal(31, msg2.Int64Values[1]); + Assert.Equal(32, msg2.Int64Values[2]); + + // uint64Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint64ValuesLength); + Assert.Equal(3, msg2.Uint64Values.Length); + Assert.Equal((ulong)33, msg2.Uint64Values[0]); + Assert.Equal((ulong)34, msg2.Uint64Values[1]); + Assert.Equal((ulong)35, msg2.Uint64Values[2]); + + // stringValues + Assert.Equal(3, test_msgs.msg.Arrays.StringValuesLength); + Assert.Equal(3, msg2.StringValues.Length); + Assert.Equal("one", msg2.StringValues[0]); + Assert.Equal("two", msg2.StringValues[1]); + Assert.Equal("three", msg2.StringValues[2]); + + // basicTypesValues + Assert.Equal(3, test_msgs.msg.Arrays.BasicTypesValuesLength); + Assert.Equal(3, msg2.BasicTypesValues.Length); + + Assert.True(msg2.BasicTypesValues[0].BoolValue); + Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); + Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); + Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); + Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); + Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); + Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); + Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); + Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); + Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); + Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); + Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); + Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); + + Assert.False(msg2.BasicTypesValues[1].BoolValue); + Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); + Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); + Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); + Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); + Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); + Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); + Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); + Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); + Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); + Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); + Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); + Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); + + Assert.True(msg2.BasicTypesValues[2].BoolValue); + Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); + Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); + Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); + Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); + Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); + Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); + Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); + Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); + Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); + Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); + Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); + Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); } [Fact] @@ -377,14 +377,14 @@ public void TestPublishArraysSizeCheckToLittle() var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_little"); var msg = new test_msgs.msg.Arrays(); - msg.Bool_values = new bool[] + msg.BoolValues = new bool[] { false, true, }; var exception = Assert.Throws(() => chatterPub.Publish(msg)); - Assert.Equal("Invalid size of array 'Bool_values'.", exception.Message); + Assert.Equal("Invalid size of array 'BoolValues'.", exception.Message); } [Fact] @@ -395,7 +395,7 @@ public void TestPublishArraysSizeCheckToMuch() var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_much"); var msg = new test_msgs.msg.Arrays(); - msg.String_values = new string[] + msg.StringValues = new string[] { "0", "1", @@ -404,7 +404,7 @@ public void TestPublishArraysSizeCheckToMuch() }; var exception = Assert.Throws(() => chatterPub.Publish(msg)); - Assert.Equal("Invalid size of array 'String_values'.", exception.Message); + Assert.Equal("Invalid size of array 'StringValues'.", exception.Message); } [Fact] @@ -418,124 +418,124 @@ public void TestPublishUnboundedSequences() test_msgs.msg.UnboundedSequences msg = new test_msgs.msg.UnboundedSequences(); test_msgs.msg.UnboundedSequences msg2 = new test_msgs.msg.UnboundedSequences(); - // bool_values - msg.Bool_values.Add(true); - msg.Bool_values.Add(false); - msg.Bool_values.Add(true); - - // byte_values - msg.Byte_values.Add(0); - msg.Byte_values.Add(1); - msg.Byte_values.Add(2); - - // char_values - msg.Char_values.Add(3); - msg.Char_values.Add(4); - msg.Char_values.Add(5); - - // float32_values - msg.Float32_values.Add(6.1f); - msg.Float32_values.Add(7.1f); - msg.Float32_values.Add(8.1f); - - // float64_values - msg.Float64_values.Add(9.1); - msg.Float64_values.Add(10.1); - msg.Float64_values.Add(11.1); - - // int8_values - msg.Int8_values.Add(12); - msg.Int8_values.Add(13); - msg.Int8_values.Add(14); - - // uint8_values - msg.Uint8_values.Add(15); - msg.Uint8_values.Add(16); - msg.Uint8_values.Add(17); - - // int16_values - msg.Int16_values.Add(18); - msg.Int16_values.Add(19); - msg.Int16_values.Add(20); - - // uint16_values - msg.Uint16_values.Add(21); - msg.Uint16_values.Add(22); - msg.Uint16_values.Add(23); - - // int32_values - msg.Int32_values.Add(24); - msg.Int32_values.Add(25); - msg.Int32_values.Add(26); - - // uint32_values - msg.Uint32_values.Add(27); - msg.Uint32_values.Add(28); - msg.Uint32_values.Add(29); - - // int64_values - msg.Int64_values.Add(30); - msg.Int64_values.Add(31); - msg.Int64_values.Add(32); - - // uint64_values - msg.Uint64_values.Add(33); - msg.Uint64_values.Add(34); - msg.Uint64_values.Add(35); - - // string_values - msg.String_values.Add("one"); - msg.String_values.Add("two"); - msg.String_values.Add("three"); + // boolValues + msg.BoolValues.Add(true); + msg.BoolValues.Add(false); + msg.BoolValues.Add(true); + + // byteValues + msg.ByteValues.Add(0); + msg.ByteValues.Add(1); + msg.ByteValues.Add(2); + + // charValues + msg.CharValues.Add(3); + msg.CharValues.Add(4); + msg.CharValues.Add(5); + + // float32Values + msg.Float32Values.Add(6.1f); + msg.Float32Values.Add(7.1f); + msg.Float32Values.Add(8.1f); + + // float64Values + msg.Float64Values.Add(9.1); + msg.Float64Values.Add(10.1); + msg.Float64Values.Add(11.1); + + // int8Values + msg.Int8Values.Add(12); + msg.Int8Values.Add(13); + msg.Int8Values.Add(14); + + // uint8Values + msg.Uint8Values.Add(15); + msg.Uint8Values.Add(16); + msg.Uint8Values.Add(17); + + // int16Values + msg.Int16Values.Add(18); + msg.Int16Values.Add(19); + msg.Int16Values.Add(20); + + // uint16Values + msg.Uint16Values.Add(21); + msg.Uint16Values.Add(22); + msg.Uint16Values.Add(23); + + // int32Values + msg.Int32Values.Add(24); + msg.Int32Values.Add(25); + msg.Int32Values.Add(26); + + // uint32Values + msg.Uint32Values.Add(27); + msg.Uint32Values.Add(28); + msg.Uint32Values.Add(29); + + // int64Values + msg.Int64Values.Add(30); + msg.Int64Values.Add(31); + msg.Int64Values.Add(32); + + // uint64Values + msg.Uint64Values.Add(33); + msg.Uint64Values.Add(34); + msg.Uint64Values.Add(35); + + // stringValues + msg.StringValues.Add("one"); + msg.StringValues.Add("two"); + msg.StringValues.Add("three"); test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.Bool_value = true; - basic_type_1.Byte_value = 36; - basic_type_1.Char_value = 37; - basic_type_1.Float32_value = 38.1f; - basic_type_1.Float64_value = 39.1; - basic_type_1.Int8_value = 40; - basic_type_1.Uint8_value = 41; - basic_type_1.Int16_value = 42; - basic_type_1.Uint16_value = 43; - basic_type_1.Int32_value = 44; - basic_type_1.Uint32_value = 45; - basic_type_1.Int64_value = 46; - basic_type_1.Uint64_value = 47; + basic_type_1.BoolValue = true; + basic_type_1.ByteValue = 36; + basic_type_1.CharValue = 37; + basic_type_1.Float32Value = 38.1f; + basic_type_1.Float64Value = 39.1; + basic_type_1.Int8Value = 40; + basic_type_1.Uint8Value = 41; + basic_type_1.Int16Value = 42; + basic_type_1.Uint16Value = 43; + basic_type_1.Int32Value = 44; + basic_type_1.Uint32Value = 45; + basic_type_1.Int64Value = 46; + basic_type_1.Uint64Value = 47; test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.Bool_value = false; - basic_type_2.Byte_value = 48; - basic_type_2.Char_value = 49; - basic_type_2.Float32_value = 50.1f; - basic_type_2.Float64_value = 51.1; - basic_type_2.Int8_value = 52; - basic_type_2.Uint8_value = 53; - basic_type_2.Int16_value = 54; - basic_type_2.Uint16_value = 55; - basic_type_2.Int32_value = 56; - basic_type_2.Uint32_value = 57; - basic_type_2.Int64_value = 58; - basic_type_2.Uint64_value = 59; + basic_type_2.BoolValue = false; + basic_type_2.ByteValue = 48; + basic_type_2.CharValue = 49; + basic_type_2.Float32Value = 50.1f; + basic_type_2.Float64Value = 51.1; + basic_type_2.Int8Value = 52; + basic_type_2.Uint8Value = 53; + basic_type_2.Int16Value = 54; + basic_type_2.Uint16Value = 55; + basic_type_2.Int32Value = 56; + basic_type_2.Uint32Value = 57; + basic_type_2.Int64Value = 58; + basic_type_2.Uint64Value = 59; test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.Bool_value = true; - basic_type_3.Byte_value = 60; - basic_type_3.Char_value = 61; - basic_type_3.Float32_value = 62.1f; - basic_type_3.Float64_value = 63.1; - basic_type_3.Int8_value = 64; - basic_type_3.Uint8_value = 65; - basic_type_3.Int16_value = 66; - basic_type_3.Uint16_value = 67; - basic_type_3.Int32_value = 68; - basic_type_3.Uint32_value = 69; - basic_type_3.Int64_value = 70; - basic_type_3.Uint64_value = 71; - - msg.Basic_types_values.Add(basic_type_1); - msg.Basic_types_values.Add(basic_type_2); - msg.Basic_types_values.Add(basic_type_3); + basic_type_3.BoolValue = true; + basic_type_3.ByteValue = 60; + basic_type_3.CharValue = 61; + basic_type_3.Float32Value = 62.1f; + basic_type_3.Float64Value = 63.1; + basic_type_3.Int8Value = 64; + basic_type_3.Uint8Value = 65; + basic_type_3.Int16Value = 66; + basic_type_3.Uint16Value = 67; + basic_type_3.Int32Value = 68; + basic_type_3.Uint32Value = 69; + basic_type_3.Int64Value = 70; + basic_type_3.Uint64Value = 71; + + msg.BasicTypesValues.Add(basic_type_1); + msg.BasicTypesValues.Add(basic_type_2); + msg.BasicTypesValues.Add(basic_type_3); bool received=false; Subscription chatter_sub = node_array_2.CreateSubscription( @@ -554,132 +554,132 @@ public void TestPublishUnboundedSequences() RCLdotnet.SpinOnce(node_array_2, 500); } - // bool_values - Assert.Equal(3, msg2.Bool_values.Count); - Assert.True(msg2.Bool_values[0]); - Assert.False(msg2.Bool_values[1]); - Assert.True(msg2.Bool_values[2]); - - // byte_values - Assert.Equal(3, msg2.Byte_values.Count); - Assert.Equal(0, msg2.Byte_values[0]); - Assert.Equal(1, msg2.Byte_values[1]); - Assert.Equal(2, msg2.Byte_values[2]); - - // char_values - Assert.Equal(3, msg2.Char_values.Count); - Assert.Equal(3, msg2.Char_values[0]); - Assert.Equal(4, msg2.Char_values[1]); - Assert.Equal(5, msg2.Char_values[2]); - - // float32_values - Assert.Equal(3, msg2.Float32_values.Count); - Assert.Equal(6.1f, msg2.Float32_values[0]); - Assert.Equal(7.1f, msg2.Float32_values[1]); - Assert.Equal(8.1f, msg2.Float32_values[2]); - - // float64_values - Assert.Equal(3, msg2.Float64_values.Count); - Assert.Equal(9.1, msg2.Float64_values[0]); - Assert.Equal(10.1, msg2.Float64_values[1]); - Assert.Equal(11.1, msg2.Float64_values[2]); - - // int8_values - Assert.Equal(3, msg2.Int8_values.Count); - Assert.Equal(12, msg2.Int8_values[0]); - Assert.Equal(13, msg2.Int8_values[1]); - Assert.Equal(14, msg2.Int8_values[2]); - - // uint8_values - Assert.Equal(3, msg2.Uint8_values.Count); - Assert.Equal(15, msg2.Uint8_values[0]); - Assert.Equal(16, msg2.Uint8_values[1]); - Assert.Equal(17, msg2.Uint8_values[2]); - - // int16_values - Assert.Equal(3, msg2.Int16_values.Count); - Assert.Equal(18, msg2.Int16_values[0]); - Assert.Equal(19, msg2.Int16_values[1]); - Assert.Equal(20, msg2.Int16_values[2]); - - // uint16_values - Assert.Equal(3, msg2.Uint16_values.Count); - Assert.Equal(21, msg2.Uint16_values[0]); - Assert.Equal(22, msg2.Uint16_values[1]); - Assert.Equal(23, msg2.Uint16_values[2]); - - // int32_values - Assert.Equal(3, msg2.Int32_values.Count); - Assert.Equal(24, msg2.Int32_values[0]); - Assert.Equal(25, msg2.Int32_values[1]); - Assert.Equal(26, msg2.Int32_values[2]); + // boolValues + Assert.Equal(3, msg2.BoolValues.Count); + Assert.True(msg2.BoolValues[0]); + Assert.False(msg2.BoolValues[1]); + Assert.True(msg2.BoolValues[2]); + + // byteValues + Assert.Equal(3, msg2.ByteValues.Count); + Assert.Equal(0, msg2.ByteValues[0]); + Assert.Equal(1, msg2.ByteValues[1]); + Assert.Equal(2, msg2.ByteValues[2]); + + // charValues + Assert.Equal(3, msg2.CharValues.Count); + Assert.Equal(3, msg2.CharValues[0]); + Assert.Equal(4, msg2.CharValues[1]); + Assert.Equal(5, msg2.CharValues[2]); + + // float32Values + Assert.Equal(3, msg2.Float32Values.Count); + Assert.Equal(6.1f, msg2.Float32Values[0]); + Assert.Equal(7.1f, msg2.Float32Values[1]); + Assert.Equal(8.1f, msg2.Float32Values[2]); + + // float64Values + Assert.Equal(3, msg2.Float64Values.Count); + Assert.Equal(9.1, msg2.Float64Values[0]); + Assert.Equal(10.1, msg2.Float64Values[1]); + Assert.Equal(11.1, msg2.Float64Values[2]); + + // int8Values + Assert.Equal(3, msg2.Int8Values.Count); + Assert.Equal(12, msg2.Int8Values[0]); + Assert.Equal(13, msg2.Int8Values[1]); + Assert.Equal(14, msg2.Int8Values[2]); + + // uint8Values + Assert.Equal(3, msg2.Uint8Values.Count); + Assert.Equal(15, msg2.Uint8Values[0]); + Assert.Equal(16, msg2.Uint8Values[1]); + Assert.Equal(17, msg2.Uint8Values[2]); + + // int16Values + Assert.Equal(3, msg2.Int16Values.Count); + Assert.Equal(18, msg2.Int16Values[0]); + Assert.Equal(19, msg2.Int16Values[1]); + Assert.Equal(20, msg2.Int16Values[2]); + + // uint16Values + Assert.Equal(3, msg2.Uint16Values.Count); + Assert.Equal(21, msg2.Uint16Values[0]); + Assert.Equal(22, msg2.Uint16Values[1]); + Assert.Equal(23, msg2.Uint16Values[2]); + + // int32Values + Assert.Equal(3, msg2.Int32Values.Count); + Assert.Equal(24, msg2.Int32Values[0]); + Assert.Equal(25, msg2.Int32Values[1]); + Assert.Equal(26, msg2.Int32Values[2]); - // uint32_values - Assert.Equal(3, msg2.Uint32_values.Count); - Assert.Equal((uint)27, msg2.Uint32_values[0]); - Assert.Equal((uint)28, msg2.Uint32_values[1]); - Assert.Equal((uint)29, msg2.Uint32_values[2]); - - // int64_values - Assert.Equal(3, msg2.Int64_values.Count); - Assert.Equal(30, msg2.Int64_values[0]); - Assert.Equal(31, msg2.Int64_values[1]); - Assert.Equal(32, msg2.Int64_values[2]); + // uint32Values + Assert.Equal(3, msg2.Uint32Values.Count); + Assert.Equal((uint)27, msg2.Uint32Values[0]); + Assert.Equal((uint)28, msg2.Uint32Values[1]); + Assert.Equal((uint)29, msg2.Uint32Values[2]); + + // int64Values + Assert.Equal(3, msg2.Int64Values.Count); + Assert.Equal(30, msg2.Int64Values[0]); + Assert.Equal(31, msg2.Int64Values[1]); + Assert.Equal(32, msg2.Int64Values[2]); - // uint64_values - Assert.Equal(3, msg2.Uint64_values.Count); - Assert.Equal((ulong)33, msg2.Uint64_values[0]); - Assert.Equal((ulong)34, msg2.Uint64_values[1]); - Assert.Equal((ulong)35, msg2.Uint64_values[2]); - - // string_values - Assert.Equal(3, msg2.String_values.Count); - Assert.Equal("one", msg2.String_values[0]); - Assert.Equal("two", msg2.String_values[1]); - Assert.Equal("three", msg2.String_values[2]); + // uint64Values + Assert.Equal(3, msg2.Uint64Values.Count); + Assert.Equal((ulong)33, msg2.Uint64Values[0]); + Assert.Equal((ulong)34, msg2.Uint64Values[1]); + Assert.Equal((ulong)35, msg2.Uint64Values[2]); + + // stringValues + Assert.Equal(3, msg2.StringValues.Count); + Assert.Equal("one", msg2.StringValues[0]); + Assert.Equal("two", msg2.StringValues[1]); + Assert.Equal("three", msg2.StringValues[2]); - Assert.Equal(3, msg2.Basic_types_values.Count); - Assert.True(msg2.Basic_types_values[0].Bool_value); - Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); - Assert.Equal(37, msg2.Basic_types_values[0].Char_value); - Assert.Equal(38.1f, msg2.Basic_types_values[0].Float32_value); - Assert.Equal(39.1, msg2.Basic_types_values[0].Float64_value); - Assert.Equal(40, msg2.Basic_types_values[0].Int8_value); - Assert.Equal(41, msg2.Basic_types_values[0].Uint8_value); - Assert.Equal(42, msg2.Basic_types_values[0].Int16_value); - Assert.Equal(43, msg2.Basic_types_values[0].Uint16_value); - Assert.Equal(44, msg2.Basic_types_values[0].Int32_value); - Assert.Equal((uint)45, msg2.Basic_types_values[0].Uint32_value); - Assert.Equal(46, msg2.Basic_types_values[0].Int64_value); - Assert.Equal((ulong)47, msg2.Basic_types_values[0].Uint64_value); - - Assert.False(msg2.Basic_types_values[1].Bool_value); - Assert.Equal(48, msg2.Basic_types_values[1].Byte_value); - Assert.Equal(49, msg2.Basic_types_values[1].Char_value); - Assert.Equal(50.1f, msg2.Basic_types_values[1].Float32_value); - Assert.Equal(51.1, msg2.Basic_types_values[1].Float64_value); - Assert.Equal(52, msg2.Basic_types_values[1].Int8_value); - Assert.Equal(53, msg2.Basic_types_values[1].Uint8_value); - Assert.Equal(54, msg2.Basic_types_values[1].Int16_value); - Assert.Equal(55, msg2.Basic_types_values[1].Uint16_value); - Assert.Equal(56, msg2.Basic_types_values[1].Int32_value); - Assert.Equal((uint)57, msg2.Basic_types_values[1].Uint32_value); - Assert.Equal(58, msg2.Basic_types_values[1].Int64_value); - Assert.Equal((ulong)59, msg2.Basic_types_values[1].Uint64_value); + Assert.Equal(3, msg2.BasicTypesValues.Count); + Assert.True(msg2.BasicTypesValues[0].BoolValue); + Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); + Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); + Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); + Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); + Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); + Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); + Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); + Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); + Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); + Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); + Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); + Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); + + Assert.False(msg2.BasicTypesValues[1].BoolValue); + Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); + Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); + Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); + Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); + Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); + Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); + Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); + Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); + Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); + Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); + Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); + Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); - Assert.True(msg2.Basic_types_values[2].Bool_value); - Assert.Equal(60, msg2.Basic_types_values[2].Byte_value); - Assert.Equal(61, msg2.Basic_types_values[2].Char_value); - Assert.Equal(62.1f, msg2.Basic_types_values[2].Float32_value); - Assert.Equal(63.1, msg2.Basic_types_values[2].Float64_value); - Assert.Equal(64, msg2.Basic_types_values[2].Int8_value); - Assert.Equal(65, msg2.Basic_types_values[2].Uint8_value); - Assert.Equal(66, msg2.Basic_types_values[2].Int16_value); - Assert.Equal(67, msg2.Basic_types_values[2].Uint16_value); - Assert.Equal(68, msg2.Basic_types_values[2].Int32_value); - Assert.Equal((uint)69, msg2.Basic_types_values[2].Uint32_value); - Assert.Equal(70, msg2.Basic_types_values[2].Int64_value); - Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); + Assert.True(msg2.BasicTypesValues[2].BoolValue); + Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); + Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); + Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); + Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); + Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); + Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); + Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); + Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); + Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); + Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); + Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); + Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); } [Fact] @@ -693,124 +693,124 @@ public void TestPublishBoundedSequences() test_msgs.msg.BoundedSequences msg = new test_msgs.msg.BoundedSequences(); test_msgs.msg.BoundedSequences msg2 = new test_msgs.msg.BoundedSequences(); - // bool_values - msg.Bool_values.Add(true); - msg.Bool_values.Add(false); - msg.Bool_values.Add(true); - - // byte_values - msg.Byte_values.Add(0); - msg.Byte_values.Add(1); - msg.Byte_values.Add(2); - - // char_values - msg.Char_values.Add(3); - msg.Char_values.Add(4); - msg.Char_values.Add(5); - - // float32_values - msg.Float32_values.Add(6.1f); - msg.Float32_values.Add(7.1f); - msg.Float32_values.Add(8.1f); - - // float64_values - msg.Float64_values.Add(9.1); - msg.Float64_values.Add(10.1); - msg.Float64_values.Add(11.1); - - // int8_values - msg.Int8_values.Add(12); - msg.Int8_values.Add(13); - msg.Int8_values.Add(14); - - // uint8_values - msg.Uint8_values.Add(15); - msg.Uint8_values.Add(16); - msg.Uint8_values.Add(17); - - // int16_values - msg.Int16_values.Add(18); - msg.Int16_values.Add(19); - msg.Int16_values.Add(20); - - // uint16_values - msg.Uint16_values.Add(21); - msg.Uint16_values.Add(22); - msg.Uint16_values.Add(23); - - // int32_values - msg.Int32_values.Add(24); - msg.Int32_values.Add(25); - msg.Int32_values.Add(26); - - // uint32_values - msg.Uint32_values.Add(27); - msg.Uint32_values.Add(28); - msg.Uint32_values.Add(29); - - // int64_values - msg.Int64_values.Add(30); - msg.Int64_values.Add(31); - msg.Int64_values.Add(32); - - // uint64_values - msg.Uint64_values.Add(33); - msg.Uint64_values.Add(34); - msg.Uint64_values.Add(35); - - // string_values - msg.String_values.Add("one"); - msg.String_values.Add("two"); - msg.String_values.Add("three"); + // boolValues + msg.BoolValues.Add(true); + msg.BoolValues.Add(false); + msg.BoolValues.Add(true); + + // byteValues + msg.ByteValues.Add(0); + msg.ByteValues.Add(1); + msg.ByteValues.Add(2); + + // charValues + msg.CharValues.Add(3); + msg.CharValues.Add(4); + msg.CharValues.Add(5); + + // float32Values + msg.Float32Values.Add(6.1f); + msg.Float32Values.Add(7.1f); + msg.Float32Values.Add(8.1f); + + // float64Values + msg.Float64Values.Add(9.1); + msg.Float64Values.Add(10.1); + msg.Float64Values.Add(11.1); + + // int8Values + msg.Int8Values.Add(12); + msg.Int8Values.Add(13); + msg.Int8Values.Add(14); + + // uint8Values + msg.Uint8Values.Add(15); + msg.Uint8Values.Add(16); + msg.Uint8Values.Add(17); + + // int16Values + msg.Int16Values.Add(18); + msg.Int16Values.Add(19); + msg.Int16Values.Add(20); + + // uint16Values + msg.Uint16Values.Add(21); + msg.Uint16Values.Add(22); + msg.Uint16Values.Add(23); + + // int32Values + msg.Int32Values.Add(24); + msg.Int32Values.Add(25); + msg.Int32Values.Add(26); + + // uint32Values + msg.Uint32Values.Add(27); + msg.Uint32Values.Add(28); + msg.Uint32Values.Add(29); + + // int64Values + msg.Int64Values.Add(30); + msg.Int64Values.Add(31); + msg.Int64Values.Add(32); + + // uint64Values + msg.Uint64Values.Add(33); + msg.Uint64Values.Add(34); + msg.Uint64Values.Add(35); + + // stringValues + msg.StringValues.Add("one"); + msg.StringValues.Add("two"); + msg.StringValues.Add("three"); test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.Bool_value = true; - basic_type_1.Byte_value = 36; - basic_type_1.Char_value = 37; - basic_type_1.Float32_value = 38.1f; - basic_type_1.Float64_value = 39.1; - basic_type_1.Int8_value = 40; - basic_type_1.Uint8_value = 41; - basic_type_1.Int16_value = 42; - basic_type_1.Uint16_value = 43; - basic_type_1.Int32_value = 44; - basic_type_1.Uint32_value = 45; - basic_type_1.Int64_value = 46; - basic_type_1.Uint64_value = 47; + basic_type_1.BoolValue = true; + basic_type_1.ByteValue = 36; + basic_type_1.CharValue = 37; + basic_type_1.Float32Value = 38.1f; + basic_type_1.Float64Value = 39.1; + basic_type_1.Int8Value = 40; + basic_type_1.Uint8Value = 41; + basic_type_1.Int16Value = 42; + basic_type_1.Uint16Value = 43; + basic_type_1.Int32Value = 44; + basic_type_1.Uint32Value = 45; + basic_type_1.Int64Value = 46; + basic_type_1.Uint64Value = 47; test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.Bool_value = false; - basic_type_2.Byte_value = 48; - basic_type_2.Char_value = 49; - basic_type_2.Float32_value = 50.1f; - basic_type_2.Float64_value = 51.1; - basic_type_2.Int8_value = 52; - basic_type_2.Uint8_value = 53; - basic_type_2.Int16_value = 54; - basic_type_2.Uint16_value = 55; - basic_type_2.Int32_value = 56; - basic_type_2.Uint32_value = 57; - basic_type_2.Int64_value = 58; - basic_type_2.Uint64_value = 59; + basic_type_2.BoolValue = false; + basic_type_2.ByteValue = 48; + basic_type_2.CharValue = 49; + basic_type_2.Float32Value = 50.1f; + basic_type_2.Float64Value = 51.1; + basic_type_2.Int8Value = 52; + basic_type_2.Uint8Value = 53; + basic_type_2.Int16Value = 54; + basic_type_2.Uint16Value = 55; + basic_type_2.Int32Value = 56; + basic_type_2.Uint32Value = 57; + basic_type_2.Int64Value = 58; + basic_type_2.Uint64Value = 59; test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.Bool_value = true; - basic_type_3.Byte_value = 60; - basic_type_3.Char_value = 61; - basic_type_3.Float32_value = 62.1f; - basic_type_3.Float64_value = 63.1; - basic_type_3.Int8_value = 64; - basic_type_3.Uint8_value = 65; - basic_type_3.Int16_value = 66; - basic_type_3.Uint16_value = 67; - basic_type_3.Int32_value = 68; - basic_type_3.Uint32_value = 69; - basic_type_3.Int64_value = 70; - basic_type_3.Uint64_value = 71; - - msg.Basic_types_values.Add(basic_type_1); - msg.Basic_types_values.Add(basic_type_2); - msg.Basic_types_values.Add(basic_type_3); + basic_type_3.BoolValue = true; + basic_type_3.ByteValue = 60; + basic_type_3.CharValue = 61; + basic_type_3.Float32Value = 62.1f; + basic_type_3.Float64Value = 63.1; + basic_type_3.Int8Value = 64; + basic_type_3.Uint8Value = 65; + basic_type_3.Int16Value = 66; + basic_type_3.Uint16Value = 67; + basic_type_3.Int32Value = 68; + basic_type_3.Uint32Value = 69; + basic_type_3.Int64Value = 70; + basic_type_3.Uint64Value = 71; + + msg.BasicTypesValues.Add(basic_type_1); + msg.BasicTypesValues.Add(basic_type_2); + msg.BasicTypesValues.Add(basic_type_3); bool received=false; Subscription chatter_sub = node_array_2.CreateSubscription( @@ -829,147 +829,147 @@ public void TestPublishBoundedSequences() RCLdotnet.SpinOnce(node_array_2, 500); } - // bool_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Bool_values_MaxCount); - Assert.Equal(3, msg2.Bool_values.Count); - Assert.True(msg2.Bool_values[0]); - Assert.False(msg2.Bool_values[1]); - Assert.True(msg2.Bool_values[2]); - - // byte_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Byte_values_MaxCount); - Assert.Equal(3, msg2.Byte_values.Count); - Assert.Equal(0, msg2.Byte_values[0]); - Assert.Equal(1, msg2.Byte_values[1]); - Assert.Equal(2, msg2.Byte_values[2]); - - // char_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Char_values_MaxCount); - Assert.Equal(3, msg2.Char_values.Count); - Assert.Equal(3, msg2.Char_values[0]); - Assert.Equal(4, msg2.Char_values[1]); - Assert.Equal(5, msg2.Char_values[2]); - - // float32_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Float32_values_MaxCount); - Assert.Equal(3, msg2.Float32_values.Count); - Assert.Equal(6.1f, msg2.Float32_values[0]); - Assert.Equal(7.1f, msg2.Float32_values[1]); - Assert.Equal(8.1f, msg2.Float32_values[2]); - - // float64_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Float64_values_MaxCount); - Assert.Equal(3, msg2.Float64_values.Count); - Assert.Equal(9.1, msg2.Float64_values[0]); - Assert.Equal(10.1, msg2.Float64_values[1]); - Assert.Equal(11.1, msg2.Float64_values[2]); - - // int8_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int8_values_MaxCount); - Assert.Equal(3, msg2.Int8_values.Count); - Assert.Equal(12, msg2.Int8_values[0]); - Assert.Equal(13, msg2.Int8_values[1]); - Assert.Equal(14, msg2.Int8_values[2]); - - // uint8_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint8_values_MaxCount); - Assert.Equal(3, msg2.Uint8_values.Count); - Assert.Equal(15, msg2.Uint8_values[0]); - Assert.Equal(16, msg2.Uint8_values[1]); - Assert.Equal(17, msg2.Uint8_values[2]); - - // int16_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int16_values_MaxCount); - Assert.Equal(3, msg2.Int16_values.Count); - Assert.Equal(18, msg2.Int16_values[0]); - Assert.Equal(19, msg2.Int16_values[1]); - Assert.Equal(20, msg2.Int16_values[2]); - - // uint16_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint16_values_MaxCount); - Assert.Equal(3, msg2.Uint16_values.Count); - Assert.Equal(21, msg2.Uint16_values[0]); - Assert.Equal(22, msg2.Uint16_values[1]); - Assert.Equal(23, msg2.Uint16_values[2]); - - // int32_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int32_values_MaxCount); - Assert.Equal(3, msg2.Int32_values.Count); - Assert.Equal(24, msg2.Int32_values[0]); - Assert.Equal(25, msg2.Int32_values[1]); - Assert.Equal(26, msg2.Int32_values[2]); + // boolValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.BoolValuesMaxCount); + Assert.Equal(3, msg2.BoolValues.Count); + Assert.True(msg2.BoolValues[0]); + Assert.False(msg2.BoolValues[1]); + Assert.True(msg2.BoolValues[2]); + + // byteValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.ByteValuesMaxCount); + Assert.Equal(3, msg2.ByteValues.Count); + Assert.Equal(0, msg2.ByteValues[0]); + Assert.Equal(1, msg2.ByteValues[1]); + Assert.Equal(2, msg2.ByteValues[2]); + + // charValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.CharValuesMaxCount); + Assert.Equal(3, msg2.CharValues.Count); + Assert.Equal(3, msg2.CharValues[0]); + Assert.Equal(4, msg2.CharValues[1]); + Assert.Equal(5, msg2.CharValues[2]); + + // float32Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Float32ValuesMaxCount); + Assert.Equal(3, msg2.Float32Values.Count); + Assert.Equal(6.1f, msg2.Float32Values[0]); + Assert.Equal(7.1f, msg2.Float32Values[1]); + Assert.Equal(8.1f, msg2.Float32Values[2]); + + // float64Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Float64ValuesMaxCount); + Assert.Equal(3, msg2.Float64Values.Count); + Assert.Equal(9.1, msg2.Float64Values[0]); + Assert.Equal(10.1, msg2.Float64Values[1]); + Assert.Equal(11.1, msg2.Float64Values[2]); + + // int8Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int8ValuesMaxCount); + Assert.Equal(3, msg2.Int8Values.Count); + Assert.Equal(12, msg2.Int8Values[0]); + Assert.Equal(13, msg2.Int8Values[1]); + Assert.Equal(14, msg2.Int8Values[2]); + + // uint8Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint8ValuesMaxCount); + Assert.Equal(3, msg2.Uint8Values.Count); + Assert.Equal(15, msg2.Uint8Values[0]); + Assert.Equal(16, msg2.Uint8Values[1]); + Assert.Equal(17, msg2.Uint8Values[2]); + + // int16Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int16ValuesMaxCount); + Assert.Equal(3, msg2.Int16Values.Count); + Assert.Equal(18, msg2.Int16Values[0]); + Assert.Equal(19, msg2.Int16Values[1]); + Assert.Equal(20, msg2.Int16Values[2]); + + // uint16Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint16ValuesMaxCount); + Assert.Equal(3, msg2.Uint16Values.Count); + Assert.Equal(21, msg2.Uint16Values[0]); + Assert.Equal(22, msg2.Uint16Values[1]); + Assert.Equal(23, msg2.Uint16Values[2]); + + // int32Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int32ValuesMaxCount); + Assert.Equal(3, msg2.Int32Values.Count); + Assert.Equal(24, msg2.Int32Values[0]); + Assert.Equal(25, msg2.Int32Values[1]); + Assert.Equal(26, msg2.Int32Values[2]); - // uint32_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint32_values_MaxCount); - Assert.Equal(3, msg2.Uint32_values.Count); - Assert.Equal((uint)27, msg2.Uint32_values[0]); - Assert.Equal((uint)28, msg2.Uint32_values[1]); - Assert.Equal((uint)29, msg2.Uint32_values[2]); - - // int64_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int64_values_MaxCount); - Assert.Equal(3, msg2.Int64_values.Count); - Assert.Equal(30, msg2.Int64_values[0]); - Assert.Equal(31, msg2.Int64_values[1]); - Assert.Equal(32, msg2.Int64_values[2]); + // uint32Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint32ValuesMaxCount); + Assert.Equal(3, msg2.Uint32Values.Count); + Assert.Equal((uint)27, msg2.Uint32Values[0]); + Assert.Equal((uint)28, msg2.Uint32Values[1]); + Assert.Equal((uint)29, msg2.Uint32Values[2]); + + // int64Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int64ValuesMaxCount); + Assert.Equal(3, msg2.Int64Values.Count); + Assert.Equal(30, msg2.Int64Values[0]); + Assert.Equal(31, msg2.Int64Values[1]); + Assert.Equal(32, msg2.Int64Values[2]); - // uint64_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint64_values_MaxCount); - Assert.Equal(3, msg2.Uint64_values.Count); - Assert.Equal((ulong)33, msg2.Uint64_values[0]); - Assert.Equal((ulong)34, msg2.Uint64_values[1]); - Assert.Equal((ulong)35, msg2.Uint64_values[2]); - - // string_values - Assert.Equal(3, test_msgs.msg.BoundedSequences.String_values_MaxCount); - Assert.Equal(3, msg2.String_values.Count); - Assert.Equal("one", msg2.String_values[0]); - Assert.Equal("two", msg2.String_values[1]); - Assert.Equal("three", msg2.String_values[2]); + // uint64Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint64ValuesMaxCount); + Assert.Equal(3, msg2.Uint64Values.Count); + Assert.Equal((ulong)33, msg2.Uint64Values[0]); + Assert.Equal((ulong)34, msg2.Uint64Values[1]); + Assert.Equal((ulong)35, msg2.Uint64Values[2]); + + // stringValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.StringValuesMaxCount); + Assert.Equal(3, msg2.StringValues.Count); + Assert.Equal("one", msg2.StringValues[0]); + Assert.Equal("two", msg2.StringValues[1]); + Assert.Equal("three", msg2.StringValues[2]); - Assert.Equal(3, test_msgs.msg.BoundedSequences.Basic_types_values_MaxCount); - Assert.Equal(3, msg2.Basic_types_values.Count); - Assert.True(msg2.Basic_types_values[0].Bool_value); - Assert.Equal(36, msg2.Basic_types_values[0].Byte_value); - Assert.Equal(37, msg2.Basic_types_values[0].Char_value); - Assert.Equal(38.1f, msg2.Basic_types_values[0].Float32_value); - Assert.Equal(39.1, msg2.Basic_types_values[0].Float64_value); - Assert.Equal(40, msg2.Basic_types_values[0].Int8_value); - Assert.Equal(41, msg2.Basic_types_values[0].Uint8_value); - Assert.Equal(42, msg2.Basic_types_values[0].Int16_value); - Assert.Equal(43, msg2.Basic_types_values[0].Uint16_value); - Assert.Equal(44, msg2.Basic_types_values[0].Int32_value); - Assert.Equal((uint)45, msg2.Basic_types_values[0].Uint32_value); - Assert.Equal(46, msg2.Basic_types_values[0].Int64_value); - Assert.Equal((ulong)47, msg2.Basic_types_values[0].Uint64_value); - - Assert.False(msg2.Basic_types_values[1].Bool_value); - Assert.Equal(48, msg2.Basic_types_values[1].Byte_value); - Assert.Equal(49, msg2.Basic_types_values[1].Char_value); - Assert.Equal(50.1f, msg2.Basic_types_values[1].Float32_value); - Assert.Equal(51.1, msg2.Basic_types_values[1].Float64_value); - Assert.Equal(52, msg2.Basic_types_values[1].Int8_value); - Assert.Equal(53, msg2.Basic_types_values[1].Uint8_value); - Assert.Equal(54, msg2.Basic_types_values[1].Int16_value); - Assert.Equal(55, msg2.Basic_types_values[1].Uint16_value); - Assert.Equal(56, msg2.Basic_types_values[1].Int32_value); - Assert.Equal((uint)57, msg2.Basic_types_values[1].Uint32_value); - Assert.Equal(58, msg2.Basic_types_values[1].Int64_value); - Assert.Equal((ulong)59, msg2.Basic_types_values[1].Uint64_value); + Assert.Equal(3, test_msgs.msg.BoundedSequences.BasicTypesValuesMaxCount); + Assert.Equal(3, msg2.BasicTypesValues.Count); + Assert.True(msg2.BasicTypesValues[0].BoolValue); + Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); + Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); + Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); + Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); + Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); + Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); + Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); + Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); + Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); + Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); + Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); + Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); + + Assert.False(msg2.BasicTypesValues[1].BoolValue); + Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); + Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); + Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); + Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); + Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); + Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); + Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); + Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); + Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); + Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); + Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); + Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); - Assert.True(msg2.Basic_types_values[2].Bool_value); - Assert.Equal(60, msg2.Basic_types_values[2].Byte_value); - Assert.Equal(61, msg2.Basic_types_values[2].Char_value); - Assert.Equal(62.1f, msg2.Basic_types_values[2].Float32_value); - Assert.Equal(63.1, msg2.Basic_types_values[2].Float64_value); - Assert.Equal(64, msg2.Basic_types_values[2].Int8_value); - Assert.Equal(65, msg2.Basic_types_values[2].Uint8_value); - Assert.Equal(66, msg2.Basic_types_values[2].Int16_value); - Assert.Equal(67, msg2.Basic_types_values[2].Uint16_value); - Assert.Equal(68, msg2.Basic_types_values[2].Int32_value); - Assert.Equal((uint)69, msg2.Basic_types_values[2].Uint32_value); - Assert.Equal(70, msg2.Basic_types_values[2].Int64_value); - Assert.Equal((ulong)71, msg2.Basic_types_values[2].Uint64_value); + Assert.True(msg2.BasicTypesValues[2].BoolValue); + Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); + Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); + Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); + Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); + Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); + Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); + Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); + Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); + Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); + Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); + Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); + Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); } [Fact] @@ -980,7 +980,7 @@ public void TestPublishBoundedSequencesSizeCheck() var chatterPub = nodeBoundedSequencesSizeCheck.CreatePublisher("topic_bounded_sequences_size_check"); var msg = new test_msgs.msg.BoundedSequences(); - msg.String_values = new List + msg.StringValues = new List { "0", "1", @@ -989,7 +989,7 @@ public void TestPublishBoundedSequencesSizeCheck() }; var exception = Assert.Throws(() => chatterPub.Publish(msg)); - Assert.Equal("Invalid size of bounded sequence 'String_values'.", exception.Message); + Assert.Equal("Invalid size of bounded sequence 'StringValues'.", exception.Message); } } } diff --git a/rcldotnet/test/test_services.cs b/rcldotnet/test/test_services.cs index 5b59d71e..7712d86b 100644 --- a/rcldotnet/test/test_services.cs +++ b/rcldotnet/test/test_services.cs @@ -1,3 +1,18 @@ +/* Copyright 2021-2022 Stefan Hoffmann + * + * 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 ROS2; using Xunit; @@ -20,20 +35,20 @@ public void TestServiceAndClient() var client = clientNode.CreateClient("unittest_dotnet_service"); var request = new test_msgs.srv.BasicTypes_Request(); - request.Bool_value = true; - request.Byte_value = 36; - request.Char_value = 37; - request.Float32_value = 38.1f; - request.Float64_value = 39.1; - request.Int8_value = 40; - request.Uint8_value = 41; - request.Int16_value = 42; - request.Uint16_value = 43; - request.Int32_value = 44; - request.Uint32_value = 45; - request.Int64_value = 46; - request.Uint64_value = 47; - request.String_value = "one"; + request.BoolValue = true; + request.ByteValue = 36; + request.CharValue = 37; + request.Float32Value = 38.1f; + request.Float64Value = 39.1; + request.Int8Value = 40; + request.Uint8Value = 41; + request.Int16Value = 42; + request.Uint16Value = 43; + request.Int32Value = 44; + request.Uint32Value = 45; + request.Int64Value = 46; + request.Uint64Value = 47; + request.StringValue = "one"; var task = client.SendRequestAsync(request); @@ -48,54 +63,54 @@ public void TestServiceAndClient() } } - Assert.True(serviceReceivedRequest.Bool_value); - Assert.Equal(36, serviceReceivedRequest.Byte_value); - Assert.Equal(37, serviceReceivedRequest.Char_value); - Assert.Equal(38.1f, serviceReceivedRequest.Float32_value); - Assert.Equal(39.1, serviceReceivedRequest.Float64_value); - Assert.Equal(40, serviceReceivedRequest.Int8_value); - Assert.Equal(41, serviceReceivedRequest.Uint8_value); - Assert.Equal(42, serviceReceivedRequest.Int16_value); - Assert.Equal(43, serviceReceivedRequest.Uint16_value); - Assert.Equal(44, serviceReceivedRequest.Int32_value); - Assert.Equal((uint)45, serviceReceivedRequest.Uint32_value); - Assert.Equal(46, serviceReceivedRequest.Int64_value); - Assert.Equal((ulong)47, serviceReceivedRequest.Uint64_value); - Assert.Equal("one", serviceReceivedRequest.String_value); + Assert.True(serviceReceivedRequest.BoolValue); + Assert.Equal(36, serviceReceivedRequest.ByteValue); + Assert.Equal(37, serviceReceivedRequest.CharValue); + Assert.Equal(38.1f, serviceReceivedRequest.Float32Value); + Assert.Equal(39.1, serviceReceivedRequest.Float64Value); + Assert.Equal(40, serviceReceivedRequest.Int8Value); + Assert.Equal(41, serviceReceivedRequest.Uint8Value); + Assert.Equal(42, serviceReceivedRequest.Int16Value); + Assert.Equal(43, serviceReceivedRequest.Uint16Value); + Assert.Equal(44, serviceReceivedRequest.Int32Value); + Assert.Equal((uint)45, serviceReceivedRequest.Uint32Value); + Assert.Equal(46, serviceReceivedRequest.Int64Value); + Assert.Equal((ulong)47, serviceReceivedRequest.Uint64Value); + Assert.Equal("one", serviceReceivedRequest.StringValue); - Assert.True(clientReceivedResponse.Bool_value); - Assert.Equal(36, clientReceivedResponse.Byte_value); - Assert.Equal(37, clientReceivedResponse.Char_value); - Assert.Equal(38.1f, clientReceivedResponse.Float32_value); - Assert.Equal(39.1, clientReceivedResponse.Float64_value); - Assert.Equal(40, clientReceivedResponse.Int8_value); - Assert.Equal(41, clientReceivedResponse.Uint8_value); - Assert.Equal(42, clientReceivedResponse.Int16_value); - Assert.Equal(43, clientReceivedResponse.Uint16_value); - Assert.Equal(144, clientReceivedResponse.Int32_value); - Assert.Equal((uint)45, clientReceivedResponse.Uint32_value); - Assert.Equal(46, clientReceivedResponse.Int64_value); - Assert.Equal((ulong)47, clientReceivedResponse.Uint64_value); - Assert.Equal("one", clientReceivedResponse.String_value); + Assert.True(clientReceivedResponse.BoolValue); + Assert.Equal(36, clientReceivedResponse.ByteValue); + Assert.Equal(37, clientReceivedResponse.CharValue); + Assert.Equal(38.1f, clientReceivedResponse.Float32Value); + Assert.Equal(39.1, clientReceivedResponse.Float64Value); + Assert.Equal(40, clientReceivedResponse.Int8Value); + Assert.Equal(41, clientReceivedResponse.Uint8Value); + Assert.Equal(42, clientReceivedResponse.Int16Value); + Assert.Equal(43, clientReceivedResponse.Uint16Value); + Assert.Equal(144, clientReceivedResponse.Int32Value); + Assert.Equal((uint)45, clientReceivedResponse.Uint32Value); + Assert.Equal(46, clientReceivedResponse.Int64Value); + Assert.Equal((ulong)47, clientReceivedResponse.Uint64Value); + Assert.Equal("one", clientReceivedResponse.StringValue); void HandleRequest(test_msgs.srv.BasicTypes_Request req, test_msgs.srv.BasicTypes_Response response) { serviceReceivedRequest = req; - response.Bool_value = req.Bool_value; - response.Byte_value = req.Byte_value; - response.Char_value = req.Char_value; - response.Float32_value = req.Float32_value; - response.Float64_value = req.Float64_value; - response.Int8_value = req.Int8_value; - response.Uint8_value = req.Uint8_value; - response.Int16_value = req.Int16_value; - response.Uint16_value = req.Uint16_value; - response.Int32_value = req.Int32_value + 100; - response.Uint32_value = req.Uint32_value; - response.Int64_value = req.Int64_value; - response.Uint64_value = req.Uint64_value; - response.String_value = req.String_value; + response.BoolValue = req.BoolValue; + response.ByteValue = req.ByteValue; + response.CharValue = req.CharValue; + response.Float32Value = req.Float32Value; + response.Float64Value = req.Float64Value; + response.Int8Value = req.Int8Value; + response.Uint8Value = req.Uint8Value; + response.Int16Value = req.Int16Value; + response.Uint16Value = req.Uint16Value; + response.Int32Value = req.Int32Value + 100; + response.Uint32Value = req.Uint32Value; + response.Int64Value = req.Int64Value; + response.Uint64Value = req.Uint64Value; + response.StringValue = req.StringValue; } } } diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index bb5b336c..750a9c9a 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -24,9 +24,9 @@ public class @(type_name) : global::ROS2.IRosMessage { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ - public const int @(get_field_name(type_name, member.name))_Length = @(member.type.size); + public const int @(get_field_name(type_name, member.name))Length = @(member.type.size); @[ elif isinstance(member.type, AbstractSequence) and member.type.has_maximum_size()]@ - public const int @(get_field_name(type_name, member.name))_MaxCount = @(member.type.maximum_size); + public const int @(get_field_name(type_name, member.name))MaxCount = @(member.type.maximum_size); @[ end if]@ @[end for]@ diff --git a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py index 7528c15d..cf6568fd 100644 --- a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py +++ b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py @@ -149,12 +149,12 @@ def msg_type_to_c(type_): assert isinstance(type_, BasicType) return BASIC_IDL_TYPES_TO_C[type_.typename] -def upperfirst(s): - return s[0].capitalize() + s[1:] - +# Taken from http://stackoverflow.com/a/6425628 +def convert_lower_case_underscore_to_camel_case(word): + return ''.join(x.capitalize() or '_' for x in word.split('_')) def get_field_name(type_name, field_name): - if upperfirst(field_name) == type_name: + if convert_lower_case_underscore_to_camel_case(field_name) == type_name: return "{0}_".format(type_name) else: - return upperfirst(field_name) + return convert_lower_case_underscore_to_camel_case(field_name) From fe31b3fe4dcf5eec92571154427274971c78bddb Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 09:43:18 +0200 Subject: [PATCH 30/73] Add support for field default values for non-collections --- rcldotnet/test/test_messages.cs | 28 +++++++++++++++++++ rosidl_generator_dotnet/resource/msg.cs.em | 7 +++++ .../rosidl_generator_dotnet/__init__.py | 1 + 3 files changed, 36 insertions(+) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index 1270064e..1bc975f3 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -991,5 +991,33 @@ public void TestPublishBoundedSequencesSizeCheck() var exception = Assert.Throws(() => chatterPub.Publish(msg)); Assert.Equal("Invalid size of bounded sequence 'StringValues'.", exception.Message); } + + [Fact] + public void TestDefaults() + { + var defaultsMsg = new test_msgs.msg.Defaults(); + + Assert.Equal(true, defaultsMsg.BoolValue); + Assert.Equal(50, defaultsMsg.ByteValue); + Assert.Equal(100, defaultsMsg.CharValue); + Assert.Equal(1.125f, defaultsMsg.Float32Value); + Assert.Equal(1.125, defaultsMsg.Float64Value); + Assert.Equal(-50, defaultsMsg.Int8Value); + Assert.Equal(200, defaultsMsg.Uint8Value); + Assert.Equal(-1000, defaultsMsg.Int16Value); + Assert.Equal(2000, defaultsMsg.Uint16Value); + Assert.Equal(-30000, defaultsMsg.Int32Value); + Assert.True(60000 == defaultsMsg.Uint32Value); + Assert.Equal(-40000000, defaultsMsg.Int64Value); + Assert.True(50000000 == defaultsMsg.Uint64Value); + + var stringsMsg = new test_msgs.msg.Strings(); + Assert.Equal("", stringsMsg.StringValue); + Assert.Equal("Hello world!", stringsMsg.StringValueDefault1); + Assert.Equal("Hello'world!", stringsMsg.StringValueDefault2); + Assert.Equal("Hello\"world!", stringsMsg.StringValueDefault3); + Assert.Equal("Hello'world!", stringsMsg.StringValueDefault4); + Assert.Equal("Hello\"world!", stringsMsg.StringValueDefault5); + } } } diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 750a9c9a..5a119ec9 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -55,8 +55,15 @@ public class @(type_name) : global::ROS2.IRosMessage { @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType)]@ +@[ if member.has_annotation('default')]@ + @(get_field_name(type_name, member.name)) = @(constant_value_to_dotnet(member.type, member.get_annotation_value('default')['value'])); +@[ end if]@ @[ elif isinstance(member.type, AbstractString)]@ +@[ if member.has_annotation('default')]@ + @(get_field_name(type_name, member.name)) = @(constant_value_to_dotnet(member.type, member.get_annotation_value('default')['value'])); +@[ else]@ @(get_field_name(type_name, member.name)) = ""; +@[ end if]@ @[ else]@ @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type))(); @[ end if]@ diff --git a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py index cf6568fd..95310ecc 100644 --- a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py +++ b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py @@ -53,6 +53,7 @@ def generate_dotnet(generator_arguments_file, typesupport_impls): def escape_string(s): s = s.replace('\\', '\\\\') s = s.replace("'", "\\'") + s = s.replace("\"", "\\\"") return s From 6d3790bf8e170ebe6a23541e1d500d6c47f24ee5 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 10:32:53 +0200 Subject: [PATCH 31/73] Add support for field default values for collections --- rcldotnet/test/test_messages.cs | 393 +++++++++++++++++++++ rosidl_generator_dotnet/resource/msg.cs.em | 33 +- 2 files changed, 425 insertions(+), 1 deletion(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index 1bc975f3..f96e54f2 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Linq; using ROS2; using Xunit; @@ -1019,5 +1020,397 @@ public void TestDefaults() Assert.Equal("Hello'world!", stringsMsg.StringValueDefault4); Assert.Equal("Hello\"world!", stringsMsg.StringValueDefault5); } + + [Fact] + public void TestDefaultsArrays() + { + var msg = new test_msgs.msg.Arrays(); + + Assert.IsType(msg.BoolValues); + Assert.Equal(new bool[3], msg.BoolValues); + + Assert.IsType(msg.ByteValues); + Assert.Equal(new byte[3], msg.ByteValues); + + Assert.IsType(msg.CharValues); + Assert.Equal(new byte[3], msg.CharValues); + + Assert.IsType(msg.Float32Values); + Assert.Equal(new float[3], msg.Float32Values); + + Assert.IsType(msg.Float64Values); + Assert.Equal(new double[3], msg.Float64Values); + + Assert.IsType(msg.Int8Values); + Assert.Equal(new sbyte[3], msg.Int8Values); + + Assert.IsType(msg.Uint8Values); + Assert.Equal(new byte[3], msg.Uint8Values); + + Assert.IsType(msg.Int16Values); + Assert.Equal(new short[3], msg.Int16Values); + + Assert.IsType(msg.Uint16Values); + Assert.Equal(new ushort[3], msg.Uint16Values); + + Assert.IsType(msg.Int32Values); + Assert.Equal(new int[3], msg.Int32Values); + + Assert.IsType(msg.Uint32Values); + Assert.Equal(new uint[3], msg.Uint32Values); + + Assert.IsType(msg.Int64Values); + Assert.Equal(new long[3], msg.Int64Values); + + Assert.IsType(msg.Uint64Values); + Assert.Equal(new ulong[3], msg.Uint64Values); + + Assert.IsType(msg.StringValues); + Assert.Equal( + Enumerable.Repeat("", 3).ToArray(), + msg.StringValues); + + Assert.IsType(msg.BasicTypesValues); + Assert.Equal(3, msg.BasicTypesValues.Length); + Assert.NotNull(msg.BasicTypesValues[0]); + Assert.NotNull(msg.BasicTypesValues[1]); + Assert.NotNull(msg.BasicTypesValues[2]); + + Assert.IsType(msg.ConstantsValues); + Assert.Equal(3, msg.ConstantsValues.Length); + Assert.NotNull(msg.ConstantsValues[0]); + Assert.NotNull(msg.ConstantsValues[1]); + Assert.NotNull(msg.ConstantsValues[2]); + + Assert.IsType(msg.DefaultsValues); + Assert.Equal(3, msg.DefaultsValues.Length); + Assert.NotNull(msg.DefaultsValues[0]); + Assert.NotNull(msg.DefaultsValues[1]); + Assert.NotNull(msg.DefaultsValues[2]); + + Assert.IsType(msg.BoolValuesDefault); + Assert.Equal( + new bool[] { false, true, false }, + msg.BoolValuesDefault); + + Assert.IsType(msg.ByteValuesDefault); + Assert.Equal( + new byte[] { 0, 1, 255 }, + msg.ByteValuesDefault); + + Assert.IsType(msg.CharValuesDefault); + Assert.Equal( + new byte[] { 0, 1, 127 }, + msg.CharValuesDefault); + + Assert.IsType(msg.Float32ValuesDefault); + Assert.Equal( + new float[] { 1.125f, 0.0f, -1.125f}, + msg.Float32ValuesDefault); + + Assert.IsType(msg.Float64ValuesDefault); + Assert.Equal( + new double[] { 3.1415, 0.0, -3.1415 }, + msg.Float64ValuesDefault); + + Assert.IsType(msg.Int8ValuesDefault); + Assert.Equal( + new sbyte[] { 0, 127, -128 }, + msg.Int8ValuesDefault); + + Assert.IsType(msg.Uint8ValuesDefault); + Assert.Equal( + new byte[] { 0, 1, 255 }, + msg.Uint8ValuesDefault); + + Assert.IsType(msg.Int16ValuesDefault); + Assert.Equal( + new short[] { 0, 32767, -32768 }, + msg.Int16ValuesDefault); + + Assert.IsType(msg.Uint16ValuesDefault); + Assert.Equal( + new ushort[] { 0, 1, 65535 }, + msg.Uint16ValuesDefault); + + Assert.IsType(msg.Int32ValuesDefault); + Assert.Equal( + new int[] { 0, 2147483647, -2147483648 }, + msg.Int32ValuesDefault); + + Assert.IsType(msg.Uint32ValuesDefault); + Assert.Equal( + new uint[] { 0, 1, 4294967295 }, + msg.Uint32ValuesDefault); + + Assert.IsType(msg.Int64ValuesDefault); + Assert.Equal( + new long[] { 0, 9223372036854775807, -9223372036854775808 }, + msg.Int64ValuesDefault); + + Assert.IsType(msg.Uint64ValuesDefault); + Assert.Equal( + new ulong[] { 0, 1, 18446744073709551615 }, + msg.Uint64ValuesDefault); + + Assert.IsType(msg.StringValuesDefault); + Assert.Equal( + new string[] { "", "max value", "min value" }, + msg.StringValuesDefault); + } + + [Fact] + public void TestDefaultsUnboundedSequences() + { + var msg = new test_msgs.msg.UnboundedSequences(); + + Assert.IsType>(msg.BoolValues); + Assert.Equal(new List(), msg.BoolValues); + + Assert.IsType>(msg.ByteValues); + Assert.Equal(new List(), msg.ByteValues); + + Assert.IsType>(msg.CharValues); + Assert.Equal(new List(), msg.CharValues); + + Assert.IsType>(msg.Float32Values); + Assert.Equal(new List(), msg.Float32Values); + + Assert.IsType>(msg.Float64Values); + Assert.Equal(new List(), msg.Float64Values); + + Assert.IsType>(msg.Int8Values); + Assert.Equal(new List(), msg.Int8Values); + + Assert.IsType>(msg.Uint8Values); + Assert.Equal(new List(), msg.Uint8Values); + + Assert.IsType>(msg.Int16Values); + Assert.Equal(new List(), msg.Int16Values); + + Assert.IsType>(msg.Uint16Values); + Assert.Equal(new List(), msg.Uint16Values); + + Assert.IsType>(msg.Int32Values); + Assert.Equal(new List(), msg.Int32Values); + + Assert.IsType>(msg.Uint32Values); + Assert.Equal(new List(), msg.Uint32Values); + + Assert.IsType>(msg.Int64Values); + Assert.Equal(new List(), msg.Int64Values); + + Assert.IsType>(msg.Uint64Values); + Assert.Equal(new List(), msg.Uint64Values); + + Assert.IsType>(msg.StringValues); + Assert.Equal(new List(), msg.StringValues); + + Assert.IsType>(msg.BasicTypesValues); + Assert.Equal(new List(), msg.BasicTypesValues); + + Assert.IsType>(msg.ConstantsValues); + Assert.Equal(new List(), msg.ConstantsValues); + + Assert.IsType>(msg.DefaultsValues); + Assert.Equal(new List(), msg.DefaultsValues); + + Assert.IsType>(msg.BoolValuesDefault); + Assert.Equal( + new List { false, true, false }, + msg.BoolValuesDefault); + + Assert.IsType>(msg.ByteValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.ByteValuesDefault); + + Assert.IsType>(msg.CharValuesDefault); + Assert.Equal( + new List { 0, 1, 127 }, + msg.CharValuesDefault); + + Assert.IsType>(msg.Float32ValuesDefault); + Assert.Equal( + new List { 1.125f, 0.0f, -1.125f}, + msg.Float32ValuesDefault); + + Assert.IsType>(msg.Float64ValuesDefault); + Assert.Equal( + new List { 3.1415, 0.0, -3.1415 }, + msg.Float64ValuesDefault); + + Assert.IsType>(msg.Int8ValuesDefault); + Assert.Equal( + new List { 0, 127, -128 }, + msg.Int8ValuesDefault); + + Assert.IsType>(msg.Uint8ValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.Uint8ValuesDefault); + + Assert.IsType>(msg.Int16ValuesDefault); + Assert.Equal( + new List { 0, 32767, -32768 }, + msg.Int16ValuesDefault); + + Assert.IsType>(msg.Uint16ValuesDefault); + Assert.Equal( + new List { 0, 1, 65535 }, + msg.Uint16ValuesDefault); + + Assert.IsType>(msg.Int32ValuesDefault); + Assert.Equal( + new List { 0, 2147483647, -2147483648 }, + msg.Int32ValuesDefault); + + Assert.IsType>(msg.Uint32ValuesDefault); + Assert.Equal( + new List { 0, 1, 4294967295 }, + msg.Uint32ValuesDefault); + + Assert.IsType>(msg.Int64ValuesDefault); + Assert.Equal( + new List { 0, 9223372036854775807, -9223372036854775808 }, + msg.Int64ValuesDefault); + + Assert.IsType>(msg.Uint64ValuesDefault); + Assert.Equal( + new List { 0, 1, 18446744073709551615 }, + msg.Uint64ValuesDefault); + + Assert.IsType>(msg.StringValuesDefault); + Assert.Equal( + new List { "", "max value", "min value" }, + msg.StringValuesDefault); + } + + [Fact] + public void TestDefaultsBoundedSequences() + { + var msg = new test_msgs.msg.BoundedSequences(); + + Assert.IsType>(msg.BoolValues); + Assert.Equal(new List(), msg.BoolValues); + + Assert.IsType>(msg.ByteValues); + Assert.Equal(new List(), msg.ByteValues); + + Assert.IsType>(msg.CharValues); + Assert.Equal(new List(), msg.CharValues); + + Assert.IsType>(msg.Float32Values); + Assert.Equal(new List(), msg.Float32Values); + + Assert.IsType>(msg.Float64Values); + Assert.Equal(new List(), msg.Float64Values); + + Assert.IsType>(msg.Int8Values); + Assert.Equal(new List(), msg.Int8Values); + + Assert.IsType>(msg.Uint8Values); + Assert.Equal(new List(), msg.Uint8Values); + + Assert.IsType>(msg.Int16Values); + Assert.Equal(new List(), msg.Int16Values); + + Assert.IsType>(msg.Uint16Values); + Assert.Equal(new List(), msg.Uint16Values); + + Assert.IsType>(msg.Int32Values); + Assert.Equal(new List(), msg.Int32Values); + + Assert.IsType>(msg.Uint32Values); + Assert.Equal(new List(), msg.Uint32Values); + + Assert.IsType>(msg.Int64Values); + Assert.Equal(new List(), msg.Int64Values); + + Assert.IsType>(msg.Uint64Values); + Assert.Equal(new List(), msg.Uint64Values); + + Assert.IsType>(msg.StringValues); + Assert.Equal(new List(), msg.StringValues); + + Assert.IsType>(msg.BasicTypesValues); + Assert.Equal(new List(), msg.BasicTypesValues); + + Assert.IsType>(msg.ConstantsValues); + Assert.Equal(new List(), msg.ConstantsValues); + + Assert.IsType>(msg.DefaultsValues); + Assert.Equal(new List(), msg.DefaultsValues); + + Assert.IsType>(msg.BoolValuesDefault); + Assert.Equal( + new List { false, true, false }, + msg.BoolValuesDefault); + + Assert.IsType>(msg.ByteValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.ByteValuesDefault); + + Assert.IsType>(msg.CharValuesDefault); + Assert.Equal( + new List { 0, 1, 127 }, + msg.CharValuesDefault); + + Assert.IsType>(msg.Float32ValuesDefault); + Assert.Equal( + new List { 1.125f, 0.0f, -1.125f}, + msg.Float32ValuesDefault); + + Assert.IsType>(msg.Float64ValuesDefault); + Assert.Equal( + new List { 3.1415, 0.0, -3.1415 }, + msg.Float64ValuesDefault); + + Assert.IsType>(msg.Int8ValuesDefault); + Assert.Equal( + new List { 0, 127, -128 }, + msg.Int8ValuesDefault); + + Assert.IsType>(msg.Uint8ValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.Uint8ValuesDefault); + + Assert.IsType>(msg.Int16ValuesDefault); + Assert.Equal( + new List { 0, 32767, -32768 }, + msg.Int16ValuesDefault); + + Assert.IsType>(msg.Uint16ValuesDefault); + Assert.Equal( + new List { 0, 1, 65535 }, + msg.Uint16ValuesDefault); + + Assert.IsType>(msg.Int32ValuesDefault); + Assert.Equal( + new List { 0, 2147483647, -2147483648 }, + msg.Int32ValuesDefault); + + Assert.IsType>(msg.Uint32ValuesDefault); + Assert.Equal( + new List { 0, 1, 4294967295 }, + msg.Uint32ValuesDefault); + + Assert.IsType>(msg.Int64ValuesDefault); + Assert.Equal( + new List { 0, 9223372036854775807, -9223372036854775808 }, + msg.Int64ValuesDefault); + + Assert.IsType>(msg.Uint64ValuesDefault); + Assert.Equal( + new List { 0, 1, 18446744073709551615 }, + msg.Uint64ValuesDefault); + + Assert.IsType>(msg.StringValuesDefault); + Assert.Equal( + new List { "", "max value", "min value" }, + msg.StringValuesDefault); + } } } diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 5a119ec9..69350cde 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -1,4 +1,6 @@ @{ +from ast import literal_eval + from rosidl_generator_dotnet import get_field_name from rosidl_generator_dotnet import get_dotnet_type from rosidl_generator_dotnet import get_builtin_dotnet_type @@ -34,24 +36,53 @@ public class @(type_name) : global::ROS2.IRosMessage { { @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ - @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)]; @[ if isinstance(member.type.value_type, AbstractString)]@ +@[ if member.has_annotation('default')]@ + @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)] + { +@[ for val in literal_eval(member.get_annotation_value('default')['value'])]@ + @(constant_value_to_dotnet(member.type.value_type, val)), +@[ end for]@ + }; +@[ else]@ + @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)]; for (var i__local_variable = 0; i__local_variable < @(member.type.size); i__local_variable++) { @(get_field_name(type_name, member.name))[i__local_variable] = ""; } +@[ end if]@ @[ elif isinstance(member.type.value_type, BasicType)]@ +@[ if member.has_annotation('default')]@ + @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)] + { +@[ for val in literal_eval(member.get_annotation_value('default')['value'])]@ + @(constant_value_to_dotnet(member.type.value_type, val)), +@[ end for]@ + }; +@[ else]@ + @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)]; @# Basic types get initialized by the array constructor. +@[ end if]@ @[ elif isinstance(member.type.value_type, AbstractWString)]@ // TODO: Unicode types are not supported @[ else]@ + @(get_field_name(type_name, member.name)) = new @(get_dotnet_type(member.type.value_type))[@(member.type.size)]; for (var i__local_variable = 0; i__local_variable < @(member.type.size); i__local_variable++) { @(get_field_name(type_name, member.name))[i__local_variable] = new @(get_dotnet_type(member.type.value_type))(); } @[ end if]@ @[ elif isinstance(member.type, AbstractSequence)]@ +@[ if member.has_annotation('default')]@ + @(get_field_name(type_name, member.name)) = new List<@(get_dotnet_type(member.type.value_type))>() + { +@[ for val in literal_eval(member.get_annotation_value('default')['value'])]@ + @(constant_value_to_dotnet(member.type.value_type, val)), +@[ end for]@ + }; +@[ else]@ @(get_field_name(type_name, member.name)) = new List<@(get_dotnet_type(member.type.value_type))>(); +@[ end if]@ @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported @[ elif isinstance(member.type, BasicType)]@ From a337ee55a4391989143341bf37ad911acccb100b Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 12:05:49 +0200 Subject: [PATCH 32/73] reduce warnings: Only use cdecl calling convention on i386 architectures This removes a warning when compiling on x64 plattforms. The C# DllImportAttribute ignores this calling canvention as well, but dosn't give a warning. --- rcldotnet/rcldotnet_macros.h | 12 ++++++++++-- rosidl_generator_dotnet/resource/msg.h.em | 12 ++++++++++-- rosidl_generator_dotnet/resource/srv.h.em | 12 ++++++++++-- 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/rcldotnet/rcldotnet_macros.h b/rcldotnet/rcldotnet_macros.h index d80c676f..2b0800d1 100644 --- a/rcldotnet/rcldotnet_macros.h +++ b/rcldotnet/rcldotnet_macros.h @@ -18,11 +18,19 @@ #if defined(_MSC_VER) #define RCLDOTNET_EXPORT __declspec(dllexport) #define RCLDOTNET_IMPORT __declspec(dllimport) - #define RCLDOTNET_CDECL __cdecl + #if defined(_M_IX86) + #define RCLDOTNET_CDECL __cdecl + #else + #define RCLDOTNET_CDECL + #endif #elif defined(__GNUC__) #define RCLDOTNET_EXPORT __attribute__((visibility("default"))) #define RCLDOTNET_IMPORT - #define RCLDOTNET_CDECL __attribute__((__cdecl__)) + #if defined(__i386__) + #define RCLDOTNET_CDECL __attribute__((__cdecl__)) + #else + #define RCLDOTNET_CDECL + #endif #else #define RCLDOTNET_EXPORT #define RCLDOTNET_IMPORT diff --git a/rosidl_generator_dotnet/resource/msg.h.em b/rosidl_generator_dotnet/resource/msg.h.em index 2d113edb..a81516f3 100644 --- a/rosidl_generator_dotnet/resource/msg.h.em +++ b/rosidl_generator_dotnet/resource/msg.h.em @@ -18,12 +18,20 @@ msg_prefix = "RCLDOTNET_{0}_{1}_{2}".format(package_name, '_'.join(message.struc // Microsoft #define @(msg_prefix)_EXPORT __declspec(dllexport) #define @(msg_prefix)_IMPORT __declspec(dllimport) - #define @(msg_prefix)_CDECL __cdecl + #if defined(_M_IX86) + #define @(msg_prefix)_CDECL __cdecl + #else + #define @(msg_prefix)_CDECL + #endif #elif defined(__GNUC__) // GCC #define @(msg_prefix)_EXPORT __attribute__((visibility("default"))) #define @(msg_prefix)_IMPORT - #define @(msg_prefix)_CDECL __attribute__((__cdecl__)) + #if defined(__i386__) + #define @(msg_prefix)_CDECL __attribute__((__cdecl__)) + #else + #define @(msg_prefix)_CDECL + #endif #else // do nothing and hope for the best? #define @(msg_prefix)_EXPORT diff --git a/rosidl_generator_dotnet/resource/srv.h.em b/rosidl_generator_dotnet/resource/srv.h.em index 46c279ba..047333f7 100644 --- a/rosidl_generator_dotnet/resource/srv.h.em +++ b/rosidl_generator_dotnet/resource/srv.h.em @@ -7,12 +7,20 @@ srv_prefix = "RCLDOTNET_{0}_{1}_{2}".format(package_name, '_'.join(service.names // Microsoft #define @(srv_prefix)_EXPORT __declspec(dllexport) #define @(srv_prefix)_IMPORT __declspec(dllimport) - #define @(srv_prefix)_CDECL __cdecl + #if defined(_M_IX86) + #define @(srv_prefix)_CDECL __cdecl + #else + #define @(srv_prefix)_CDECL + #endif #elif defined(__GNUC__) // GCC #define @(srv_prefix)_EXPORT __attribute__((visibility("default"))) #define @(srv_prefix)_IMPORT - #define @(srv_prefix)_CDECL __attribute__((__cdecl__)) + #if defined(__i386__) + #define @(srv_prefix)_CDECL __attribute__((__cdecl__)) + #else + #define @(srv_prefix)_CDECL + #endif #else // do nothing and hope for the best? #define @(srv_prefix)_EXPORT From 73975d2f1401f337bef64f97694185cbb1dee093 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 12:39:27 +0200 Subject: [PATCH 33/73] reduce warnings: remove debug message in cmake script Removes stderr output on build. --- rcldotnet_examples/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index d2383d34..64d64f89 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -23,8 +23,6 @@ set(_assemblies_dep_dlls ${std_srvs_ASSEMBLIES_DLL} ) -message("Included assemblies: ${_assemblies_dep_dlls}") - add_dotnet_executable(rcldotnet_talker RCLDotnetTalker.cs INCLUDE_DLLS From 6b51d27d63f2c4419e17be5418a1b3eb64c13209 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 13:15:33 +0200 Subject: [PATCH 34/73] codestyle: add .editorconfig --- .editorconfig | 268 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 268 insertions(+) create mode 100644 .editorconfig diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 00000000..a90be369 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,268 @@ +# See http://EditorConfig.org for more information about .editorconfig files. + +# Install VSCode extension for support in all file types. +# The C# extension does the .NET specific stuff. +# https://marketplace.visualstudio.com/items?itemName=EditorConfig.EditorConfig +# https://marketplace.visualstudio.com/items?itemName=ms-dotnettools.csharp + + +# Non C# specific config taken from https://github.com/RehanSaeed/EditorConfig/ + +# This file is the top-most EditorConfig file +root = true + +# All Files +[*] +charset = utf-8 +indent_style = space +indent_size = 4 +insert_final_newline = true +trim_trailing_whitespace = true + +# Visual Studio Solution Files +[*.sln] +indent_style = tab + +# Visual Studio XML Project Files +[*.{csproj,vbproj,vcxproj.filters,proj,projitems,shproj}] +indent_size = 2 + +# XML Configuration Files +[*.{xml,config,props,targets,nuspec,resx,ruleset,vsixmanifest,vsct}] +indent_size = 2 + +# JSON Files +[*.{json,json5,webmanifest}] +indent_size = 2 + +# YAML Files +[*.{yml,yaml}] +indent_size = 2 + +# Markdown Files +[*.{md,mdx}] +indent_size = 2 +trim_trailing_whitespace = false + +# Web Files +[*.{htm,html,js,jsm,ts,tsx,cjs,cts,ctsx,mjs,mts,mtsx,css,sass,scss,less,pcss,svg,vue}] +indent_size = 2 + +# Batch Files +[*.{cmd,bat}] +end_of_line = crlf + +# Bash Files +[*.sh] +end_of_line = lf + +# Makefiles +[Makefile] +indent_style = tab + +# Generic stuff not taken from the above repo + +# CMake +[{CMakeLists.txt,*.cmake,*.rst}] +indent_size = 2 +indent_style = space + + +# The next section is based on https://github.com/terrajobst/apireview.net/blob/main/.editorconfig +# with some modifications to not be so strict or more in sync with the +# Coding Style Conventions from the book "Framework Design Guidlines". +# The Coding Style Conventions the book are derived from this file https://github.com/dotnet/runtime/blob/main/docs/coding-guidelines/coding-style.md + +# see https://docs.microsoft.com/en-us/dotnet/fundamentals/code-analysis/style-rules/ + +# Dotnet code style settings: +[*.{cs,vb}] +# Sort using and Import directives with System.* appearing first +dotnet_sort_system_directives_first = true +# Avoid "this." and "Me." if not necessary +dotnet_style_qualification_for_field = false:error +dotnet_style_qualification_for_property = false:error +dotnet_style_qualification_for_method = false:error +dotnet_style_qualification_for_event = false:error + +# Use language keywords instead of framework type names for type references +dotnet_style_predefined_type_for_locals_parameters_members = true:error +dotnet_style_predefined_type_for_member_access = true:error + +# Suggest more modern language features when available +dotnet_style_object_initializer = true:suggestion +dotnet_style_collection_initializer = true:suggestion +dotnet_style_coalesce_expression = true:suggestion +dotnet_style_null_propagation = true:suggestion +dotnet_style_explicit_tuple_names = true:suggestion + +# Non-private static fields are PascalCase +dotnet_naming_rule.non_private_static_fields_should_be_pascal_case.severity = suggestion +dotnet_naming_rule.non_private_static_fields_should_be_pascal_case.symbols = non_private_static_fields +dotnet_naming_rule.non_private_static_fields_should_be_pascal_case.style = non_private_static_field_style + +dotnet_naming_symbols.non_private_static_fields.applicable_kinds = field +dotnet_naming_symbols.non_private_static_fields.applicable_accessibilities = public, protected, internal, protected internal, private protected +dotnet_naming_symbols.non_private_static_fields.required_modifiers = static + +dotnet_naming_style.non_private_static_field_style.capitalization = pascal_case + +# Constants are PascalCase +dotnet_naming_rule.constants_should_be_pascal_case.severity = suggestion +dotnet_naming_rule.constants_should_be_pascal_case.symbols = constants +dotnet_naming_rule.constants_should_be_pascal_case.style = constant_style + +dotnet_naming_symbols.constants.applicable_kinds = field, local +dotnet_naming_symbols.constants.required_modifiers = const + +dotnet_naming_style.constant_style.capitalization = pascal_case + +# Instance fields are camelCase and start with _ +dotnet_naming_rule.instance_fields_should_be_camel_case.severity = suggestion +dotnet_naming_rule.instance_fields_should_be_camel_case.symbols = instance_fields +dotnet_naming_rule.instance_fields_should_be_camel_case.style = instance_field_style + +dotnet_naming_symbols.instance_fields.applicable_kinds = field + +dotnet_naming_style.instance_field_style.capitalization = camel_case +dotnet_naming_style.instance_field_style.required_prefix = _ + +# Locals and parameters are camelCase +dotnet_naming_rule.locals_should_be_camel_case.severity = suggestion +dotnet_naming_rule.locals_should_be_camel_case.symbols = locals_and_parameters +dotnet_naming_rule.locals_should_be_camel_case.style = camel_case_style + +dotnet_naming_symbols.locals_and_parameters.applicable_kinds = parameter, local + +dotnet_naming_style.camel_case_style.capitalization = camel_case + +# Local functions are PascalCase +dotnet_naming_rule.local_functions_should_be_pascal_case.severity = suggestion +dotnet_naming_rule.local_functions_should_be_pascal_case.symbols = local_functions +dotnet_naming_rule.local_functions_should_be_pascal_case.style = local_function_style + +dotnet_naming_symbols.local_functions.applicable_kinds = local_function + +dotnet_naming_style.local_function_style.capitalization = pascal_case + +# By default, name items with PascalCase +dotnet_naming_rule.members_should_be_pascal_case.severity = suggestion +dotnet_naming_rule.members_should_be_pascal_case.symbols = all_members +dotnet_naming_rule.members_should_be_pascal_case.style = pascal_case_style + +dotnet_naming_symbols.all_members.applicable_kinds = * + +dotnet_naming_style.pascal_case_style.capitalization = pascal_case + +# CSharp code style settings: +[*.cs] +# Indentation preferences +csharp_indent_block_contents = true +csharp_indent_braces = false +csharp_indent_case_contents = true +csharp_indent_case_contents_when_block = false +csharp_indent_switch_labels = true +csharp_indent_labels = flush_left + +# Prefer "var" everywhere +# Deactivated to not prefere "var", as the recomendation from "Framework Design Guidelines" is to be explicit +# unless the type is apparent from the initializer. +# Main reason is so this is clearer to the reader of the code which types are involved. +# I wouldn't go the other extreme to require explicit types everywhere (except where apparent) as well. +# Maybe we can agree without tooling to not use var when not apparent but don't call it "wrong" either if we do. +# +# dotnet_diagnostic.IDE0007.severity = error +# csharp_style_var_for_built_in_types = false:error +# csharp_style_var_when_type_is_apparent = true:error +# csharp_style_var_elsewhere = true:error + +# Prefer method-like constructs to have a block body +csharp_style_expression_bodied_methods = false:none +csharp_style_expression_bodied_constructors = false:none +csharp_style_expression_bodied_operators = false:none + +# Prefer property-like constructs to have an expression-body +csharp_style_expression_bodied_properties = true:error +csharp_style_expression_bodied_indexers = true:error +csharp_style_expression_bodied_accessors = true:error + +# Suggest more modern language features when available +csharp_style_pattern_matching_over_is_with_cast_check = true:error +csharp_style_pattern_matching_over_as_with_null_check = true:error +csharp_style_inlined_variable_declaration = true:suggestion +csharp_style_throw_expression = true:error +csharp_style_conditional_delegate_call = true:suggestion + +# Newline settings +csharp_new_line_before_open_brace = all +csharp_new_line_before_else = true +csharp_new_line_before_catch = true +csharp_new_line_before_finally = true +csharp_new_line_before_members_in_object_initializers = true +csharp_new_line_before_members_in_anonymous_types = true +csharp_new_line_between_query_expression_clauses = true + +# Spacing +csharp_space_after_cast = false +csharp_space_after_colon_in_inheritance_clause = true +csharp_space_after_keywords_in_control_flow_statements = true +csharp_space_around_binary_operators = before_and_after +csharp_space_before_colon_in_inheritance_clause = true +csharp_space_between_method_call_empty_parameter_list_parentheses = false +csharp_space_between_method_call_name_and_opening_parenthesis = false +csharp_space_between_method_call_parameter_list_parentheses = false +csharp_space_between_method_declaration_empty_parameter_list_parentheses = false +csharp_space_between_method_declaration_parameter_list_parentheses = false +csharp_space_between_parentheses = false + +# Labels (should someone need them...) +csharp_indent_labels = one_less_than_current + +# Modifier preferences +dotnet_style_require_accessibility_modifiers = always:warning +csharp_preferred_modifier_order = public,private,protected,internal,static,extern,new,virtual,abstract,sealed,override,readonly,unsafe,volatile,async:warning + +# Blocks are allowed +# AVOID omitting braces, except for argument validation at the start of the method. +# Not enforced though via tooling. +csharp_prefer_braces = false:silent +csharp_preserve_single_line_blocks = true +csharp_preserve_single_line_statements = true + +# DO NOT use the bracless variant of the "using" statement +csharp_prefer_simple_using_statement = false:warning + +# IDE0035: Remove unreachable code +dotnet_diagnostic.IDE0035.severity = warning + +# IDE0036: Order modifiers +dotnet_diagnostic.IDE0036.severity = warning + +# IDE0040: Add accessibility modifiers +dotnet_diagnostic.IDE0040.severity = warning + +# IDE0043: Format string contains invalid placeholder +dotnet_diagnostic.IDE0043.severity = warning + +# IDE0044: Make field readonly +dotnet_diagnostic.IDE0044.severity = warning + +# CONSIDER: Are IDE0051 and IDE0052 too noisy to be warnings for IDE editing scenarios? Should they be made build-only warnings? +# IDE0051: Remove unused private member +dotnet_diagnostic.IDE0051.severity = warning + +# IDE0052: Remove unread private member +dotnet_diagnostic.IDE0052.severity = warning + +# IDE0055: Fix formating +# see https://docs.microsoft.com/de-de/dotnet/fundamentals/code-analysis/style-rules/formatting-rules#rule-id-ide0055-fix-formatting +dotnet_diagnostic.IDE0055.severity = warning + +# IDE0059: Unnecessary assignment to a value +dotnet_diagnostic.IDE0059.severity = warning + +# IDE0060: Remove unused parameter +dotnet_diagnostic.IDE0060.severity = warning + +# IDE0005: Using directive is unnecessary +dotnet_diagnostic.IDE0005.severity = warning From 534d2a3ea124163b72d2688f04d96df594cfbc4e Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 15:28:05 +0200 Subject: [PATCH 35/73] codestyle: Format code using `dotnet format` This was done with an temporary *.csproj file in the root of the repository: ```xml netstandard2.0 ``` Then run command `dotnet format` (from .NET6) in the root of the repository. Did some manual whitespace changes as well, mainly indented wrapped parameters. --- rcldotnet/Client.cs | 6 +- rcldotnet/MessageStaticMemberCache.cs | 6 +- rcldotnet/Node.cs | 379 +-- rcldotnet/Publisher.cs | 119 +- rcldotnet/RCLExceptionHelper.cs | 2 +- rcldotnet/RCLdotnet.cs | 1041 +++---- rcldotnet/SafeClientHandle.cs | 6 +- rcldotnet/SafeNodeHandle.cs | 2 +- rcldotnet/SafePublisherHandle.cs | 6 +- rcldotnet/SafeServiceHandle.cs | 6 +- rcldotnet/SafeSubscriptionHandle.cs | 6 +- rcldotnet/SafeWaitSetHandle.cs | 2 +- rcldotnet/Service.cs | 18 +- .../ServiceDefinitionStaticMemberCache.cs | 4 +- rcldotnet/Subscription.cs | 68 +- rcldotnet/test/test_messages.cs | 2640 ++++++++--------- rcldotnet/test/test_services.cs | 4 +- rcldotnet_common/DllLoadUtils.cs | 562 ++-- rcldotnet_common/IRosMessage.cs | 2 +- rcldotnet_examples/RCLDotnetClient.cs | 10 +- rcldotnet_examples/RCLDotnetListener.cs | 25 +- rcldotnet_examples/RCLDotnetService.cs | 4 - rcldotnet_examples/RCLDotnetTalker.cs | 42 +- 23 files changed, 2525 insertions(+), 2435 deletions(-) diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index fd7d2a35..695c8ec4 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -25,13 +25,13 @@ internal static class ClientDelegates { internal static readonly DllLoadUtils dllLoadUtils; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLSendRequestType( SafeClientHandle clientHandle, SafeHandle requestHandle, out long seqneceNumber); internal static NativeRCLSendRequestType native_rcl_send_request = null; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLServiceServerIsAvailableType( SafeNodeHandle nodeHandle, SafeClientHandle clientHandle, out bool isAvailable); @@ -84,7 +84,7 @@ public sealed class Client : Client // ros2_java uses a WeakReference here. Not sure if its needed or not. private readonly Node _node; private readonly ConcurrentDictionary _pendingRequests = new ConcurrentDictionary(); - + internal Client(SafeClientHandle handle, Node node) { Handle = handle; diff --git a/rcldotnet/MessageStaticMemberCache.cs b/rcldotnet/MessageStaticMemberCache.cs index 0bade221..2567cb5f 100644 --- a/rcldotnet/MessageStaticMemberCache.cs +++ b/rcldotnet/MessageStaticMemberCache.cs @@ -22,8 +22,8 @@ namespace ROS2 internal static class MessageStaticMemberCache where T : IRosMessage { - private static IntPtr s_typeSupport; - private static Func s_createMessageHandle; + private static readonly IntPtr s_typeSupport; + private static readonly Func s_createMessageHandle; static MessageStaticMemberCache() { @@ -71,7 +71,7 @@ public static IntPtr GetTypeSupport() { throw new InvalidOperationException($"Type '{typeof(T).FullName}' did not define a correct __GetTypeSupport mehtod."); } - + return s_typeSupport; } diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 9d020b4a..342dbb6b 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -18,228 +18,235 @@ using System.Runtime.InteropServices; using ROS2.Utils; -namespace ROS2 { - internal static class NodeDelegates { - private static readonly DllLoadUtils dllLoadUtils; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLCreatePublisherHandleType ( - ref SafePublisherHandle publisherHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); - - internal static NativeRCLCreatePublisherHandleType native_rcl_create_publisher_handle = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLDestroyPublisherHandleType ( - IntPtr publisherHandle, SafeNodeHandle nodeHandle); - - internal static NativeRCLDestroyPublisherHandleType native_rcl_destroy_publisher_handle = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLCreateSubscriptionHandleType ( - ref SafeSubscriptionHandle subscriptionHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); - - internal static NativeRCLCreateSubscriptionHandleType native_rcl_create_subscription_handle = null; +namespace ROS2 +{ + internal static class NodeDelegates + { + private static readonly DllLoadUtils dllLoadUtils; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLDestroySubscriptionHandleType ( - IntPtr subscriptionHandle, SafeNodeHandle nodeHandle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreatePublisherHandleType( + ref SafePublisherHandle publisherHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); - internal static NativeRCLDestroySubscriptionHandleType native_rcl_destroy_subscription_handle = null; + internal static NativeRCLCreatePublisherHandleType native_rcl_create_publisher_handle = null; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLCreateServiceHandleType ( - ref SafeServiceHandle serviceHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroyPublisherHandleType( + IntPtr publisherHandle, SafeNodeHandle nodeHandle); - internal static NativeRCLCreateServiceHandleType native_rcl_create_service_handle = null; + internal static NativeRCLDestroyPublisherHandleType native_rcl_destroy_publisher_handle = null; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLDestroyServiceHandleType ( - IntPtr serviceHandle, SafeNodeHandle nodeHandle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateSubscriptionHandleType( + ref SafeSubscriptionHandle subscriptionHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, IntPtr typesupportHandle); - internal static NativeRCLDestroyServiceHandleType native_rcl_destroy_service_handle = null; + internal static NativeRCLCreateSubscriptionHandleType native_rcl_create_subscription_handle = null; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLCreateClientHandleType ( - ref SafeClientHandle clientHandle, SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroySubscriptionHandleType( + IntPtr subscriptionHandle, SafeNodeHandle nodeHandle); - internal static NativeRCLCreateClientHandleType native_rcl_create_client_handle = null; + internal static NativeRCLDestroySubscriptionHandleType native_rcl_destroy_subscription_handle = null; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLDestroyClientHandleType ( - IntPtr clientHandle, SafeNodeHandle nodeHandle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateServiceHandleType( + ref SafeServiceHandle serviceHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); - internal static NativeRCLDestroyClientHandleType native_rcl_destroy_client_handle = null; + internal static NativeRCLCreateServiceHandleType native_rcl_create_service_handle = null; - static NodeDelegates () { - dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); - IntPtr nativelibrary = dllLoadUtils.LoadLibrary ("rcldotnet_node"); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroyServiceHandleType( + IntPtr serviceHandle, SafeNodeHandle nodeHandle); - IntPtr native_rcl_create_publisher_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_create_publisher_handle"); + internal static NativeRCLDestroyServiceHandleType native_rcl_destroy_service_handle = null; - NodeDelegates.native_rcl_create_publisher_handle = - (NativeRCLCreatePublisherHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_publisher_handle_ptr, typeof (NativeRCLCreatePublisherHandleType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateClientHandleType( + ref SafeClientHandle clientHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string serviceName, IntPtr typesupportHandle); - IntPtr native_rcl_destroy_publisher_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_destroy_publisher_handle"); + internal static NativeRCLCreateClientHandleType native_rcl_create_client_handle = null; - NodeDelegates.native_rcl_destroy_publisher_handle = - (NativeRCLDestroyPublisherHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_publisher_handle_ptr, typeof (NativeRCLDestroyPublisherHandleType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroyClientHandleType( + IntPtr clientHandle, SafeNodeHandle nodeHandle); - IntPtr native_rcl_create_subscription_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_create_subscription_handle"); + internal static NativeRCLDestroyClientHandleType native_rcl_destroy_client_handle = null; - NodeDelegates.native_rcl_create_subscription_handle = - (NativeRCLCreateSubscriptionHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_subscription_handle_ptr, typeof (NativeRCLCreateSubscriptionHandleType)); + static NodeDelegates() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativelibrary = dllLoadUtils.LoadLibrary("rcldotnet_node"); - IntPtr native_rcl_destroy_subscription_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_destroy_subscription_handle"); + IntPtr native_rcl_create_publisher_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_create_publisher_handle"); - NodeDelegates.native_rcl_destroy_subscription_handle = - (NativeRCLDestroySubscriptionHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_subscription_handle_ptr, typeof (NativeRCLDestroySubscriptionHandleType)); - - IntPtr native_rcl_create_service_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_create_service_handle"); + NodeDelegates.native_rcl_create_publisher_handle = + (NativeRCLCreatePublisherHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_publisher_handle_ptr, typeof(NativeRCLCreatePublisherHandleType)); - NodeDelegates.native_rcl_create_service_handle = - (NativeRCLCreateServiceHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_service_handle_ptr, typeof (NativeRCLCreateServiceHandleType)); + IntPtr native_rcl_destroy_publisher_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_destroy_publisher_handle"); - IntPtr native_rcl_destroy_service_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_destroy_service_handle"); + NodeDelegates.native_rcl_destroy_publisher_handle = + (NativeRCLDestroyPublisherHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_publisher_handle_ptr, typeof(NativeRCLDestroyPublisherHandleType)); - NodeDelegates.native_rcl_destroy_service_handle = - (NativeRCLDestroyServiceHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_service_handle_ptr, typeof (NativeRCLDestroyServiceHandleType)); + IntPtr native_rcl_create_subscription_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_create_subscription_handle"); - IntPtr native_rcl_create_client_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_create_client_handle"); + NodeDelegates.native_rcl_create_subscription_handle = + (NativeRCLCreateSubscriptionHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_subscription_handle_ptr, typeof(NativeRCLCreateSubscriptionHandleType)); - NodeDelegates.native_rcl_create_client_handle = - (NativeRCLCreateClientHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_client_handle_ptr, typeof (NativeRCLCreateClientHandleType)); + IntPtr native_rcl_destroy_subscription_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_destroy_subscription_handle"); - IntPtr native_rcl_destroy_client_handle_ptr = dllLoadUtils.GetProcAddress ( - nativelibrary, "native_rcl_destroy_client_handle"); + NodeDelegates.native_rcl_destroy_subscription_handle = + (NativeRCLDestroySubscriptionHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_subscription_handle_ptr, typeof(NativeRCLDestroySubscriptionHandleType)); - NodeDelegates.native_rcl_destroy_client_handle = - (NativeRCLDestroyClientHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_client_handle_ptr, typeof (NativeRCLDestroyClientHandleType)); - } - } + IntPtr native_rcl_create_service_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_create_service_handle"); - public sealed class Node { + NodeDelegates.native_rcl_create_service_handle = + (NativeRCLCreateServiceHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_service_handle_ptr, typeof(NativeRCLCreateServiceHandleType)); - private IList subscriptions_; + IntPtr native_rcl_destroy_service_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_destroy_service_handle"); - private IList services_; + NodeDelegates.native_rcl_destroy_service_handle = + (NativeRCLDestroyServiceHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_service_handle_ptr, typeof(NativeRCLDestroyServiceHandleType)); - private IList clients_; + IntPtr native_rcl_create_client_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_create_client_handle"); - internal Node (SafeNodeHandle handle) { - Handle = handle; - subscriptions_ = new List(); - services_ = new List(); - clients_ = new List(); - } + NodeDelegates.native_rcl_create_client_handle = + (NativeRCLCreateClientHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_client_handle_ptr, typeof(NativeRCLCreateClientHandleType)); - public IList Subscriptions => subscriptions_; - - // TODO: (sh) wrap in readonly collection - // TODO: (sh) Add posibility to remove Subscriptions/Services/Clients from Node (if this is a thing in ROS) - // Now there is no Api to remove these Objects, and there is a strong reference from these fileds to them so they dont't get collected. - public IList Services => services_; - - public IList Clients => clients_; - - // Node does intentionaly (for now) not implement IDisposable as this - // needs some extra consideration how the type works after its - // internal handle is disposed. - // By relying on the GC/Finalizer of SafeHandle the handle only gets - // Disposed if the node is not live anymore. - internal SafeNodeHandle Handle { get; } - - public Publisher CreatePublisher (string topic) where T : IRosMessage { - IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); - - var publisherHandle = new SafePublisherHandle(); - RCLRet ret = NodeDelegates.native_rcl_create_publisher_handle (ref publisherHandle, Handle, topic, typesupport); - publisherHandle.SetParent(Handle); - if (ret != RCLRet.Ok) - { - publisherHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_publisher_handle)}() failed."); - } - - // TODO: (sh) Add topic as propety to Publisher. - Publisher publisher = new Publisher (publisherHandle); - return publisher; - } + IntPtr native_rcl_destroy_client_handle_ptr = dllLoadUtils.GetProcAddress( + nativelibrary, "native_rcl_destroy_client_handle"); - public Subscription CreateSubscription (string topic, Action callback) where T : IRosMessage, new () { - IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); - - var subscriptionHandle = new SafeSubscriptionHandle(); - RCLRet ret = NodeDelegates.native_rcl_create_subscription_handle (ref subscriptionHandle, Handle, topic, typesupport); - subscriptionHandle.SetParent(Handle); - if (ret != RCLRet.Ok) - { - subscriptionHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_subscription_handle)}() failed."); - } - - // TODO: (sh) Add topic as propety to Subscription. - Subscription subscription = new Subscription (subscriptionHandle, callback); - this.subscriptions_.Add(subscription); - return subscription; + NodeDelegates.native_rcl_destroy_client_handle = + (NativeRCLDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_client_handle_ptr, typeof(NativeRCLDestroyClientHandleType)); + } } - public Service CreateService(string serviceName, Action callback) - where TService : IRosServiceDefinition - where TRequest : IRosMessage, new() - where TResponse : IRosMessage, new() + public sealed class Node { - IntPtr typesupport = ServiceDefinitionStaticMemberCache.GetTypeSupport(); - - var serviceHandle = new SafeServiceHandle(); - RCLRet ret = NodeDelegates.native_rcl_create_service_handle(ref serviceHandle, Handle, serviceName, typesupport); - serviceHandle.SetParent(Handle); - if (ret != RCLRet.Ok) - { - serviceHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_service_handle)}() failed."); - } - - // TODO: (sh) Add serviceName to Service. - var service = new Service(serviceHandle, callback); - this.services_.Add(service); - return service; - } - public Client CreateClient(string serviceName) - where TService : IRosServiceDefinition - where TRequest : IRosMessage, new() - where TResponse : IRosMessage, new() - { - IntPtr typesupport = ServiceDefinitionStaticMemberCache.GetTypeSupport(); - - var clientHandle = new SafeClientHandle(); - RCLRet ret = NodeDelegates.native_rcl_create_client_handle(ref clientHandle, Handle, serviceName, typesupport); - clientHandle.SetParent(Handle); - if (ret != RCLRet.Ok) - { - clientHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_client_handle)}() failed."); - } - - // TODO: (sh) Add serviceName to Client. - var client = new Client(clientHandle, this); - this.clients_.Add(client); - return client; + private readonly IList subscriptions_; + + private readonly IList services_; + + private readonly IList clients_; + + internal Node(SafeNodeHandle handle) + { + Handle = handle; + subscriptions_ = new List(); + services_ = new List(); + clients_ = new List(); + } + + public IList Subscriptions => subscriptions_; + + // TODO: (sh) wrap in readonly collection + // TODO: (sh) Add posibility to remove Subscriptions/Services/Clients from Node (if this is a thing in ROS) + // Now there is no Api to remove these Objects, and there is a strong reference from these fileds to them so they dont't get collected. + public IList Services => services_; + + public IList Clients => clients_; + + // Node does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the node is not live anymore. + internal SafeNodeHandle Handle { get; } + + public Publisher CreatePublisher(string topic) where T : IRosMessage + { + IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); + + var publisherHandle = new SafePublisherHandle(); + RCLRet ret = NodeDelegates.native_rcl_create_publisher_handle(ref publisherHandle, Handle, topic, typesupport); + publisherHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + publisherHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_publisher_handle)}() failed."); + } + + // TODO: (sh) Add topic as propety to Publisher. + Publisher publisher = new Publisher(publisherHandle); + return publisher; + } + + public Subscription CreateSubscription(string topic, Action callback) where T : IRosMessage, new() + { + IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); + + var subscriptionHandle = new SafeSubscriptionHandle(); + RCLRet ret = NodeDelegates.native_rcl_create_subscription_handle(ref subscriptionHandle, Handle, topic, typesupport); + subscriptionHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + subscriptionHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_subscription_handle)}() failed."); + } + + // TODO: (sh) Add topic as propety to Subscription. + Subscription subscription = new Subscription(subscriptionHandle, callback); + subscriptions_.Add(subscription); + return subscription; + } + + public Service CreateService(string serviceName, Action callback) + where TService : IRosServiceDefinition + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() + { + IntPtr typesupport = ServiceDefinitionStaticMemberCache.GetTypeSupport(); + + var serviceHandle = new SafeServiceHandle(); + RCLRet ret = NodeDelegates.native_rcl_create_service_handle(ref serviceHandle, Handle, serviceName, typesupport); + serviceHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + serviceHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_service_handle)}() failed."); + } + + // TODO: (sh) Add serviceName to Service. + var service = new Service(serviceHandle, callback); + services_.Add(service); + return service; + } + + public Client CreateClient(string serviceName) + where TService : IRosServiceDefinition + where TRequest : IRosMessage, new() + where TResponse : IRosMessage, new() + { + IntPtr typesupport = ServiceDefinitionStaticMemberCache.GetTypeSupport(); + + var clientHandle = new SafeClientHandle(); + RCLRet ret = NodeDelegates.native_rcl_create_client_handle(ref clientHandle, Handle, serviceName, typesupport); + clientHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + clientHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_create_client_handle)}() failed."); + } + + // TODO: (sh) Add serviceName to Client. + var client = new Client(clientHandle, this); + clients_.Add(client); + return client; + } } - } } diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index 6d828e5c..6edaa285 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -19,77 +19,82 @@ namespace ROS2 { - internal static class PublisherDelegates { - internal static readonly DllLoadUtils dllLoadUtils; + internal static class PublisherDelegates + { + internal static readonly DllLoadUtils dllLoadUtils; - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLPublishType ( - SafePublisherHandle publisherHandle, SafeHandle messageHandle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLPublishType( + SafePublisherHandle publisherHandle, SafeHandle messageHandle); - internal static NativeRCLPublishType native_rcl_publish = null; + internal static NativeRCLPublishType native_rcl_publish = null; - static PublisherDelegates () { - dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); - IntPtr nativelibrary = dllLoadUtils.LoadLibrary ("rcldotnet_publisher"); + static PublisherDelegates() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativelibrary = dllLoadUtils.LoadLibrary("rcldotnet_publisher"); - 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_publish_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "native_rcl_publish"); + PublisherDelegates.native_rcl_publish = (NativeRCLPublishType)Marshal.GetDelegateForFunctionPointer( + native_rcl_publish_ptr, typeof(NativeRCLPublishType)); + } } - } - /// - /// Base class of a Publisher without generic type arguments for use in collections or so. - /// - public abstract class Publisher - { - // Only allow internal subclasses. - internal Publisher() + /// + /// Base class of a Publisher without generic type arguments for use in collections or so. + /// + public abstract class Publisher { - } - - // Publisher does intentionaly (for now) not implement IDisposable as this - // needs some extra consideration how the type works after its - // internal handle is disposed. - // By relying on the GC/Finalizer of SafeHandle the handle only gets - // Disposed if the publisher is not live anymore. - internal abstract SafePublisherHandle Handle { get; } - } - - public sealed class Publisher : Publisher - where T : IRosMessage { + // Only allow internal subclasses. + internal Publisher() + { + } - internal Publisher (SafePublisherHandle handle) { - Handle = handle; + // Publisher does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the publisher is not live anymore. + internal abstract SafePublisherHandle Handle { get; } } - internal override SafePublisherHandle Handle { get; } + public sealed class Publisher : Publisher + where T : IRosMessage + { - public void Publish (T message) { - using (var messageHandle = MessageStaticMemberCache.CreateMessageHandle()) - { - bool mustRelease = false; - try + internal Publisher(SafePublisherHandle handle) { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__WriteToHandle(messageHandle.DangerousGetHandle()); + Handle = handle; } - finally + + internal override SafePublisherHandle Handle { get; } + + public void Publish(T message) { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } + using (var messageHandle = MessageStaticMemberCache.CreateMessageHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__WriteToHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } - RCLRet ret = PublisherDelegates.native_rcl_publish(Handle, messageHandle); - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(PublisherDelegates.native_rcl_publish)}() failed."); - } + RCLRet ret = PublisherDelegates.native_rcl_publish(Handle, messageHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(PublisherDelegates.native_rcl_publish)}() failed."); + } + } } - } } diff --git a/rcldotnet/RCLExceptionHelper.cs b/rcldotnet/RCLExceptionHelper.cs index fd0684d7..1f8d8b29 100644 --- a/rcldotnet/RCLExceptionHelper.cs +++ b/rcldotnet/RCLExceptionHelper.cs @@ -34,7 +34,7 @@ public static Exception CreateFromReturnValue(RCLRet value, string messagePrefix // #define RCUTILS_ERROR_MESSAGE_MAX_LENGTH 1024 var errorStringBuffer = new byte[1024]; RCLdotnetDelegates.native_rcl_get_error_string(errorStringBuffer, errorStringBuffer.Length); - + string errorString = System.Text.Encoding.UTF8.GetString(errorStringBuffer).TrimEnd('\0'); RCLdotnetDelegates.native_rcl_reset_error(); diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 260aec97..585b1ab1 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -17,606 +17,633 @@ using System.Runtime.InteropServices; using ROS2.Utils; -namespace ROS2 { - internal static class RCLdotnetDelegates { - internal static readonly DllLoadUtils dllLoadUtils; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLInitType (); - - internal static NativeRCLInitType native_rcl_init = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate void NativeRCLGetErrorStringType( - byte[] buffer, int bufferSize); - - internal static NativeRCLGetErrorStringType native_rcl_get_error_string = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate void NativeRCLResetErrorType (); - - internal static NativeRCLResetErrorType native_rcl_reset_error = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate bool NativeRCLOkType (); - - internal static NativeRCLOkType native_rcl_ok = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - internal delegate RCLRet NativeRCLCreateNodeHandleType ( - ref SafeNodeHandle nodeHandle, [MarshalAs (UnmanagedType.LPStr)] string nodeName, [MarshalAs (UnmanagedType.LPStr)] string nodeNamespace); - - internal static NativeRCLCreateNodeHandleType native_rcl_create_node_handle = null; - - [UnmanagedFunctionPointer (CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - internal delegate RCLRet NativeRCLDestroyNodeHandleType ( - IntPtr nodeHandle); +namespace ROS2 +{ + internal static class RCLdotnetDelegates + { + internal static readonly DllLoadUtils dllLoadUtils; - internal static NativeRCLDestroyNodeHandleType native_rcl_destroy_node_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLInitType(); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate IntPtr NativeRCLGetRMWIdentifierType (); + internal static NativeRCLInitType native_rcl_init = null; - internal static NativeRCLGetRMWIdentifierType native_rcl_get_rmw_identifier = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void NativeRCLGetErrorStringType( + byte[] buffer, int bufferSize); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLCreateWaitSetHandleType ( - ref SafeWaitSetHandle waitSetHandle, int numberOfSubscriptions, - int numberOfGuardConditions, int numberOfTimers, - int numberOfClients, int numberOfServices, int numberOfEvents); + internal static NativeRCLGetErrorStringType native_rcl_get_error_string = null; - internal static NativeRCLCreateWaitSetHandleType native_rcl_create_wait_set_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void NativeRCLResetErrorType(); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLDestroyWaitSetType (IntPtr waitSetHandle); + internal static NativeRCLResetErrorType native_rcl_reset_error = null; - internal static NativeRCLDestroyWaitSetType native_rcl_destroy_wait_set_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate bool NativeRCLOkType(); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetClearType (SafeWaitSetHandle waitSetHandle); + internal static NativeRCLOkType native_rcl_ok = null; - internal static NativeRCLWaitSetClearType native_rcl_wait_set_clear = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + internal delegate RCLRet NativeRCLCreateNodeHandleType( + ref SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, [MarshalAs(UnmanagedType.LPStr)] string nodeNamespace); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetAddSubscriptionType (SafeWaitSetHandle waitSetHandle, SafeSubscriptionHandle subscriptionHandle); + internal static NativeRCLCreateNodeHandleType native_rcl_create_node_handle = null; - internal static NativeRCLWaitSetAddSubscriptionType native_rcl_wait_set_add_subscription = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + internal delegate RCLRet NativeRCLDestroyNodeHandleType( + IntPtr nodeHandle); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetAddServiceType (SafeWaitSetHandle waitSetHandle, SafeServiceHandle serviceHandle); + internal static NativeRCLDestroyNodeHandleType native_rcl_destroy_node_handle = null; - internal static NativeRCLWaitSetAddServiceType native_rcl_wait_set_add_service = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr NativeRCLGetRMWIdentifierType(); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitSetAddClientType (SafeWaitSetHandle waitSetHandle, SafeClientHandle clientHandle); + internal static NativeRCLGetRMWIdentifierType native_rcl_get_rmw_identifier = null; - internal static NativeRCLWaitSetAddClientType native_rcl_wait_set_add_client = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateWaitSetHandleType( + ref SafeWaitSetHandle waitSetHandle, int numberOfSubscriptions, + int numberOfGuardConditions, int numberOfTimers, + int numberOfClients, int numberOfServices, int numberOfEvents); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLWaitType (SafeWaitSetHandle waitSetHandle, long timeout); + internal static NativeRCLCreateWaitSetHandleType native_rcl_create_wait_set_handle = null; - internal static NativeRCLWaitType native_rcl_wait = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroyWaitSetType(IntPtr waitSetHandle); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeType (SafeSubscriptionHandle subscriptionHandle, SafeHandle messageHandle); + internal static NativeRCLDestroyWaitSetType native_rcl_destroy_wait_set_handle = null; - internal static NativeRCLTakeType native_rcl_take = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLWaitSetClearType(SafeWaitSetHandle waitSetHandle); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLCreateRequestIdHandleType ( - ref SafeRequestIdHandle requestIdHandle); + internal static NativeRCLWaitSetClearType native_rcl_wait_set_clear = null; - internal static NativeRCLCreateRequestIdHandleType native_rcl_create_request_id_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLWaitSetAddSubscriptionType(SafeWaitSetHandle waitSetHandle, SafeSubscriptionHandle subscriptionHandle); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLDestroyRequestIdHandleType (IntPtr requestIdHandle); + internal static NativeRCLWaitSetAddSubscriptionType native_rcl_wait_set_add_subscription = null; - internal static NativeRCLDestroyRequestIdHandleType native_rcl_destroy_request_id_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLWaitSetAddServiceType(SafeWaitSetHandle waitSetHandle, SafeServiceHandle serviceHandle); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate long NativeRCLRequestIdGetSequenceNumberType (SafeRequestIdHandle requestIdHandle); + internal static NativeRCLWaitSetAddServiceType native_rcl_wait_set_add_service = null; - internal static NativeRCLRequestIdGetSequenceNumberType native_rcl_request_id_get_sequence_number = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLWaitSetAddClientType(SafeWaitSetHandle waitSetHandle, SafeClientHandle clientHandle); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeRequestType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle requestHandle); + internal static NativeRCLWaitSetAddClientType native_rcl_wait_set_add_client = null; - internal static NativeRCLTakeRequestType native_rcl_take_request = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLWaitType(SafeWaitSetHandle waitSetHandle, long timeout); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLSendResponseType (SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle responseHandle); + internal static NativeRCLWaitType native_rcl_wait = null; - internal static NativeRCLSendResponseType native_rcl_send_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLTakeType(SafeSubscriptionHandle subscriptionHandle, SafeHandle messageHandle); - [UnmanagedFunctionPointer (CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLTakeResponseType (SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle responseHandle); + internal static NativeRCLTakeType native_rcl_take = null; - internal static NativeRCLTakeResponseType native_rcl_take_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateRequestIdHandleType( + ref SafeRequestIdHandle requestIdHandle); - static RCLdotnetDelegates () { - dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils (); - string library_name = "rcldotnet"; - IntPtr pDll = dllLoadUtils.LoadLibrary (library_name); + internal static NativeRCLCreateRequestIdHandleType native_rcl_create_request_id_handle = null; - IntPtr native_rcl_init_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_init"); - RCLdotnetDelegates.native_rcl_init = - (NativeRCLInitType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_init_ptr, typeof (NativeRCLInitType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroyRequestIdHandleType(IntPtr requestIdHandle); - IntPtr native_rcl_get_error_string_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_get_error_string"); - RCLdotnetDelegates.native_rcl_get_error_string = - (NativeRCLGetErrorStringType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_get_error_string_ptr, typeof (NativeRCLGetErrorStringType)); - - IntPtr native_rcl_reset_error_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_reset_error"); - RCLdotnetDelegates.native_rcl_reset_error = - (NativeRCLResetErrorType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_reset_error_ptr, typeof (NativeRCLResetErrorType)); - - IntPtr native_rcl_get_rmw_identifier_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_get_rmw_identifier"); - RCLdotnetDelegates.native_rcl_get_rmw_identifier = - (NativeRCLGetRMWIdentifierType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_get_rmw_identifier_ptr, typeof (NativeRCLGetRMWIdentifierType)); - - IntPtr native_rcl_ok_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_ok"); - RCLdotnetDelegates.native_rcl_ok = - (NativeRCLOkType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_ok_ptr, typeof (NativeRCLOkType)); - - IntPtr native_rcl_create_node_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_create_node_handle"); - RCLdotnetDelegates.native_rcl_create_node_handle = - (NativeRCLCreateNodeHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_node_handle_ptr, typeof (NativeRCLCreateNodeHandleType)); - - IntPtr native_rcl_destroy_node_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_node_handle"); - RCLdotnetDelegates.native_rcl_destroy_node_handle = - (NativeRCLDestroyNodeHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_node_handle_ptr, typeof (NativeRCLDestroyNodeHandleType)); - - IntPtr native_rcl_create_wait_set_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_create_wait_set_handle"); - RCLdotnetDelegates.native_rcl_create_wait_set_handle = - (NativeRCLCreateWaitSetHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_wait_set_handle_ptr, typeof (NativeRCLCreateWaitSetHandleType)); - - IntPtr native_rcl_destroy_wait_set_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_wait_set_handle"); - RCLdotnetDelegates.native_rcl_destroy_wait_set_handle = - (NativeRCLDestroyWaitSetType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_wait_set_handle_ptr, typeof (NativeRCLDestroyWaitSetType)); - - IntPtr native_rcl_wait_set_clear_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_clear"); - RCLdotnetDelegates.native_rcl_wait_set_clear = - (NativeRCLWaitSetClearType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_wait_set_clear_ptr, typeof (NativeRCLWaitSetClearType)); - - IntPtr native_rcl_wait_set_add_subscription_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_add_subscription"); - RCLdotnetDelegates.native_rcl_wait_set_add_subscription = - (NativeRCLWaitSetAddSubscriptionType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_wait_set_add_subscription_ptr, typeof (NativeRCLWaitSetAddSubscriptionType)); - - IntPtr native_rcl_wait_set_add_service_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_add_service"); - RCLdotnetDelegates.native_rcl_wait_set_add_service = - (NativeRCLWaitSetAddServiceType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_wait_set_add_service_ptr, typeof (NativeRCLWaitSetAddServiceType)); - - IntPtr native_rcl_wait_set_add_client_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait_set_add_client"); - RCLdotnetDelegates.native_rcl_wait_set_add_client = - (NativeRCLWaitSetAddClientType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_wait_set_add_client_ptr, typeof (NativeRCLWaitSetAddClientType)); - - IntPtr native_rcl_wait_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_wait"); - RCLdotnetDelegates.native_rcl_wait = - (NativeRCLWaitType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_wait_ptr, typeof (NativeRCLWaitType)); - - IntPtr native_rcl_take_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_take"); - RCLdotnetDelegates.native_rcl_take = - (NativeRCLTakeType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_take_ptr, typeof (NativeRCLTakeType)); - - IntPtr native_rcl_create_request_id_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_create_request_id_handle"); - RCLdotnetDelegates.native_rcl_create_request_id_handle = - (NativeRCLCreateRequestIdHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_create_request_id_handle_ptr, typeof (NativeRCLCreateRequestIdHandleType)); - - IntPtr native_rcl_destroy_request_id_handle_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_destroy_request_id_handle"); - RCLdotnetDelegates.native_rcl_destroy_request_id_handle = - (NativeRCLDestroyRequestIdHandleType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_destroy_request_id_handle_ptr, typeof (NativeRCLDestroyRequestIdHandleType)); - - IntPtr native_rcl_request_id_get_sequence_number_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_request_id_get_sequence_number"); - RCLdotnetDelegates.native_rcl_request_id_get_sequence_number = - (NativeRCLRequestIdGetSequenceNumberType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_request_id_get_sequence_number_ptr, typeof (NativeRCLRequestIdGetSequenceNumberType)); - - IntPtr native_rcl_take_request_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_take_request"); - RCLdotnetDelegates.native_rcl_take_request = - (NativeRCLTakeRequestType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_take_request_ptr, typeof (NativeRCLTakeRequestType)); - - IntPtr native_rcl_send_response_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_send_response"); - RCLdotnetDelegates.native_rcl_send_response = - (NativeRCLSendResponseType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_send_response_ptr, typeof (NativeRCLSendResponseType)); - - IntPtr native_rcl_take_response_ptr = - dllLoadUtils.GetProcAddress (pDll, "native_rcl_take_response"); - RCLdotnetDelegates.native_rcl_take_response = - (NativeRCLTakeResponseType) Marshal.GetDelegateForFunctionPointer ( - native_rcl_take_response_ptr, typeof (NativeRCLTakeResponseType)); - } - } + internal static NativeRCLDestroyRequestIdHandleType native_rcl_destroy_request_id_handle = null; - public static class RCLdotnet { - private static bool initialized = false; - private static readonly object syncLock = new object (); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate long NativeRCLRequestIdGetSequenceNumberType(SafeRequestIdHandle requestIdHandle); - public static bool Ok () { - return RCLdotnetDelegates.native_rcl_ok (); - } + internal static NativeRCLRequestIdGetSequenceNumberType native_rcl_request_id_get_sequence_number = null; - public static Node CreateNode (string nodeName) { - return CreateNode (nodeName, ""); - } + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLTakeRequestType(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle requestHandle); - public static Node CreateNode (string nodeName, string nodeNamespace) { - var nodeHandle = new SafeNodeHandle(); - RCLRet ret = RCLdotnetDelegates.native_rcl_create_node_handle (ref nodeHandle, nodeName, nodeNamespace); - if (ret != RCLRet.Ok) - { - nodeHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_node_handle)}() failed."); - } - - Node node = new Node (nodeHandle); - return node; - } + internal static NativeRCLTakeRequestType native_rcl_take_request = null; - public static void Spin (Node node) { - while (Ok ()) { - SpinOnce (node, 500); - } - } + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLSendResponseType(SafeServiceHandle serviceHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle responseHandle); - private static SafeWaitSetHandle CreateWaitSet( - int numberOfSubscriptions, - int numberOfGuardConditions, - int numberOfTimers, - int numberOfClients, - int numberOfServices, - int numberOfEvents) { - var waitSetHandle = new SafeWaitSetHandle(); - RCLRet ret = RCLdotnetDelegates.native_rcl_create_wait_set_handle( - ref waitSetHandle, numberOfSubscriptions, numberOfGuardConditions, - numberOfTimers, numberOfClients, numberOfServices, numberOfEvents); - if (ret != RCLRet.Ok) - { - waitSetHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_wait_set_handle)}() failed."); - } - - return waitSetHandle; - } - - private static void WaitSetClear(SafeWaitSetHandle waitSetHandle) { - RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_clear(waitSetHandle); - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_clear)}() failed."); - } - - private static void WaitSetAddSubscription(SafeWaitSetHandle waitSetHandle, SafeSubscriptionHandle subscriptionHandle) { - RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_subscription(waitSetHandle, subscriptionHandle); - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_subscription)}() failed."); - } + internal static NativeRCLSendResponseType native_rcl_send_response = null; - private static void WaitSetAddService(SafeWaitSetHandle waitSetHandle, SafeServiceHandle serviceHandle) { - RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_service)}() failed."); - } + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLTakeResponseType(SafeClientHandle clientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle responseHandle); - private static void WaitSetAddClient(SafeWaitSetHandle waitSetHandle, SafeClientHandle clientHandle) { - RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_client(waitSetHandle, clientHandle); - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_client)}() failed."); - } + internal static NativeRCLTakeResponseType native_rcl_take_response = null; - /// - /// Block until the wait set is ready or until the timeout has been exceeded. - /// - /// The wait set. - /// Timeout in ms. - /// True if wait set is ready, False on timeout. - private static bool Wait(SafeWaitSetHandle waitSetHandle, long timeout) { - long ns_timeout = timeout * 1000000; - RCLRet ret = RCLdotnetDelegates.native_rcl_wait(waitSetHandle, ns_timeout); - if (ret == RCLRet.Ok || ret == RCLRet.Timeout) - { - return ret == RCLRet.Ok; - } - else - { - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait)}() failed."); - } + static RCLdotnetDelegates() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + string library_name = "rcldotnet"; + IntPtr pDll = dllLoadUtils.LoadLibrary(library_name); + + IntPtr native_rcl_init_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_init"); + RCLdotnetDelegates.native_rcl_init = + (NativeRCLInitType)Marshal.GetDelegateForFunctionPointer( + native_rcl_init_ptr, typeof(NativeRCLInitType)); + + IntPtr native_rcl_get_error_string_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_get_error_string"); + RCLdotnetDelegates.native_rcl_get_error_string = + (NativeRCLGetErrorStringType)Marshal.GetDelegateForFunctionPointer( + native_rcl_get_error_string_ptr, typeof(NativeRCLGetErrorStringType)); + + IntPtr native_rcl_reset_error_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_reset_error"); + RCLdotnetDelegates.native_rcl_reset_error = + (NativeRCLResetErrorType)Marshal.GetDelegateForFunctionPointer( + native_rcl_reset_error_ptr, typeof(NativeRCLResetErrorType)); + + IntPtr native_rcl_get_rmw_identifier_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_get_rmw_identifier"); + RCLdotnetDelegates.native_rcl_get_rmw_identifier = + (NativeRCLGetRMWIdentifierType)Marshal.GetDelegateForFunctionPointer( + native_rcl_get_rmw_identifier_ptr, typeof(NativeRCLGetRMWIdentifierType)); + + IntPtr native_rcl_ok_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_ok"); + RCLdotnetDelegates.native_rcl_ok = + (NativeRCLOkType)Marshal.GetDelegateForFunctionPointer( + native_rcl_ok_ptr, typeof(NativeRCLOkType)); + + IntPtr native_rcl_create_node_handle_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_create_node_handle"); + RCLdotnetDelegates.native_rcl_create_node_handle = + (NativeRCLCreateNodeHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_node_handle_ptr, typeof(NativeRCLCreateNodeHandleType)); + + IntPtr native_rcl_destroy_node_handle_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_destroy_node_handle"); + RCLdotnetDelegates.native_rcl_destroy_node_handle = + (NativeRCLDestroyNodeHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_node_handle_ptr, typeof(NativeRCLDestroyNodeHandleType)); + + IntPtr native_rcl_create_wait_set_handle_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_create_wait_set_handle"); + RCLdotnetDelegates.native_rcl_create_wait_set_handle = + (NativeRCLCreateWaitSetHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_wait_set_handle_ptr, typeof(NativeRCLCreateWaitSetHandleType)); + + IntPtr native_rcl_destroy_wait_set_handle_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_destroy_wait_set_handle"); + RCLdotnetDelegates.native_rcl_destroy_wait_set_handle = + (NativeRCLDestroyWaitSetType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_wait_set_handle_ptr, typeof(NativeRCLDestroyWaitSetType)); + + IntPtr native_rcl_wait_set_clear_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_clear"); + RCLdotnetDelegates.native_rcl_wait_set_clear = + (NativeRCLWaitSetClearType)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_clear_ptr, typeof(NativeRCLWaitSetClearType)); + + IntPtr native_rcl_wait_set_add_subscription_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_add_subscription"); + RCLdotnetDelegates.native_rcl_wait_set_add_subscription = + (NativeRCLWaitSetAddSubscriptionType)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_add_subscription_ptr, typeof(NativeRCLWaitSetAddSubscriptionType)); + + IntPtr native_rcl_wait_set_add_service_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_add_service"); + RCLdotnetDelegates.native_rcl_wait_set_add_service = + (NativeRCLWaitSetAddServiceType)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_add_service_ptr, typeof(NativeRCLWaitSetAddServiceType)); + + IntPtr native_rcl_wait_set_add_client_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_add_client"); + RCLdotnetDelegates.native_rcl_wait_set_add_client = + (NativeRCLWaitSetAddClientType)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_add_client_ptr, typeof(NativeRCLWaitSetAddClientType)); + + IntPtr native_rcl_wait_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait"); + RCLdotnetDelegates.native_rcl_wait = + (NativeRCLWaitType)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_ptr, typeof(NativeRCLWaitType)); + + IntPtr native_rcl_take_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_take"); + RCLdotnetDelegates.native_rcl_take = + (NativeRCLTakeType)Marshal.GetDelegateForFunctionPointer( + native_rcl_take_ptr, typeof(NativeRCLTakeType)); + + IntPtr native_rcl_create_request_id_handle_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_create_request_id_handle"); + RCLdotnetDelegates.native_rcl_create_request_id_handle = + (NativeRCLCreateRequestIdHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_request_id_handle_ptr, typeof(NativeRCLCreateRequestIdHandleType)); + + IntPtr native_rcl_destroy_request_id_handle_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_destroy_request_id_handle"); + RCLdotnetDelegates.native_rcl_destroy_request_id_handle = + (NativeRCLDestroyRequestIdHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_request_id_handle_ptr, typeof(NativeRCLDestroyRequestIdHandleType)); + + IntPtr native_rcl_request_id_get_sequence_number_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_request_id_get_sequence_number"); + RCLdotnetDelegates.native_rcl_request_id_get_sequence_number = + (NativeRCLRequestIdGetSequenceNumberType)Marshal.GetDelegateForFunctionPointer( + native_rcl_request_id_get_sequence_number_ptr, typeof(NativeRCLRequestIdGetSequenceNumberType)); + + IntPtr native_rcl_take_request_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_take_request"); + RCLdotnetDelegates.native_rcl_take_request = + (NativeRCLTakeRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_take_request_ptr, typeof(NativeRCLTakeRequestType)); + + IntPtr native_rcl_send_response_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_send_response"); + RCLdotnetDelegates.native_rcl_send_response = + (NativeRCLSendResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_send_response_ptr, typeof(NativeRCLSendResponseType)); + + IntPtr native_rcl_take_response_ptr = + dllLoadUtils.GetProcAddress(pDll, "native_rcl_take_response"); + RCLdotnetDelegates.native_rcl_take_response = + (NativeRCLTakeResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_take_response_ptr, typeof(NativeRCLTakeResponseType)); + } } - private static SafeRequestIdHandle CreateRequestId() + public static class RCLdotnet { - var requestIdHandle = new SafeRequestIdHandle(); - RCLRet ret = RCLdotnetDelegates.native_rcl_create_request_id_handle(ref requestIdHandle); - if (ret != RCLRet.Ok) - { - requestIdHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_request_id_handle)}() failed."); - } - - return requestIdHandle; - } + private static bool initialized = false; + private static readonly object syncLock = new object(); - private static bool Take (Subscription subscription, IRosMessage message) { - using (var messageHandle = subscription.CreateMessageHandle()) - { - RCLRet ret = RCLdotnetDelegates.native_rcl_take(subscription.Handle, messageHandle); - switch (ret) + public static bool Ok() { - case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__ReadFromHandle(messageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } - - return true; - - case RCLRet.SubscriptionTakeFailed: - return false; + return RCLdotnetDelegates.native_rcl_ok(); + } - default: - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take)}() failed."); + public static Node CreateNode(string nodeName) + { + return CreateNode(nodeName, ""); } - } - } - private static bool TakeRequest(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage request) { - using (var requestHandle = service.CreateRequestHandle()) - { - RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(service.Handle, requestHeaderHandle, requestHandle); - switch (ret) + public static Node CreateNode(string nodeName, string nodeNamespace) { - case RCLRet.Ok: - bool mustRelease = false; - try + var nodeHandle = new SafeNodeHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_node_handle(ref nodeHandle, nodeName, nodeNamespace); + if (ret != RCLRet.Ok) { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - requestHandle.DangerousAddRef(ref mustRelease); - request.__ReadFromHandle(requestHandle.DangerousGetHandle()); + nodeHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_node_handle)}() failed."); } - finally - { - if (mustRelease) - { - requestHandle.DangerousRelease(); - } - } - - return true; - case RCLRet.ServiceTakeFailed: - return false; - - default: - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_request)}() failed."); + Node node = new Node(nodeHandle); + return node; } - } - } - private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { - using (var responseHandle = client.CreateResponseHandle()) - { - RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(client.Handle, requestHeaderHandle, responseHandle); - switch (ret) + public static void Spin(Node node) { - case RCLRet.Ok: - bool mustRelease = false; - try + while (Ok()) { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__ReadFromHandle(responseHandle.DangerousGetHandle()); + SpinOnce(node, 500); } - finally + } + + private static SafeWaitSetHandle CreateWaitSet( + int numberOfSubscriptions, + int numberOfGuardConditions, + int numberOfTimers, + int numberOfClients, + int numberOfServices, + int numberOfEvents) + { + var waitSetHandle = new SafeWaitSetHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_wait_set_handle( + ref waitSetHandle, numberOfSubscriptions, numberOfGuardConditions, + numberOfTimers, numberOfClients, numberOfServices, numberOfEvents); + if (ret != RCLRet.Ok) { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } + waitSetHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_wait_set_handle)}() failed."); } - return true; + return waitSetHandle; + } - case RCLRet.ClientTakeFailed: - return false; - - default: - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_response)}() failed."); + private static void WaitSetClear(SafeWaitSetHandle waitSetHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_clear(waitSetHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_clear)}() failed."); } - } - } - private static void SendResponse(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) - { - using (var responseHandle = service.CreateResponseHandle()) - { - bool mustRelease = false; - try + private static void WaitSetAddSubscription(SafeWaitSetHandle waitSetHandle, SafeSubscriptionHandle subscriptionHandle) { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__WriteToHandle(responseHandle.DangerousGetHandle()); + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_subscription(waitSetHandle, subscriptionHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_subscription)}() failed."); } - finally + + private static void WaitSetAddService(SafeWaitSetHandle waitSetHandle, SafeServiceHandle serviceHandle) { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_service(waitSetHandle, serviceHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_service)}() failed."); } - - RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); - if (ret != RCLRet.Ok) + + private static void WaitSetAddClient(SafeWaitSetHandle waitSetHandle, SafeClientHandle clientHandle) { - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_client(waitSetHandle, clientHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_client)}() failed."); } - } - } - public static void SpinOnce (Node node, long timeout) { - int numberOfSubscriptions = node.Subscriptions.Count; - int numberOfGuardConditions = 0; - int numberOfTimers = 0; - int numberOfClients = node.Clients.Count; - int numberOfServices = node.Services.Count; - int numberOfEvents = 0; - - bool waitSetEmpty = numberOfSubscriptions == 0 - && numberOfGuardConditions == 0 - && numberOfTimers == 0 - && numberOfClients == 0 - && numberOfServices == 0 - && numberOfEvents == 0; - - if (waitSetEmpty) - { - // TODO: (sh) Should we sleep here for the timeout to avoid loops without sleep? - // Avoid WaitSetEmpty return value from rcl_wait. - return; - } - - // TODO: (sh) Reuse wait set (see executer in rclc, rclcpp and rclpy) - using (SafeWaitSetHandle waitSetHandle = CreateWaitSet(numberOfSubscriptions, - numberOfGuardConditions, - numberOfTimers, - numberOfClients, - numberOfServices, - numberOfEvents)) - { - WaitSetClear(waitSetHandle); - - foreach (Subscription subscription in node.Subscriptions) { - WaitSetAddSubscription(waitSetHandle, subscription.Handle); + /// + /// Block until the wait set is ready or until the timeout has been exceeded. + /// + /// The wait set. + /// Timeout in ms. + /// True if wait set is ready, False on timeout. + private static bool Wait(SafeWaitSetHandle waitSetHandle, long timeout) + { + long ns_timeout = timeout * 1000000; + RCLRet ret = RCLdotnetDelegates.native_rcl_wait(waitSetHandle, ns_timeout); + if (ret == RCLRet.Ok || ret == RCLRet.Timeout) + { + return ret == RCLRet.Ok; + } + else + { + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait)}() failed."); + } } - foreach (var service in node.Services) + private static SafeRequestIdHandle CreateRequestId() { - WaitSetAddService(waitSetHandle, service.Handle); + var requestIdHandle = new SafeRequestIdHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_request_id_handle(ref requestIdHandle); + if (ret != RCLRet.Ok) + { + requestIdHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_request_id_handle)}() failed."); + } + + return requestIdHandle; } - foreach (var client in node.Clients) + private static bool Take(Subscription subscription, IRosMessage message) { - WaitSetAddClient(waitSetHandle, client.Handle); + using (var messageHandle = subscription.CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_take(subscription.Handle, messageHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__ReadFromHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.SubscriptionTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take)}() failed."); + } + } } - bool ready = Wait(waitSetHandle, timeout); - if (!ready) + private static bool TakeRequest(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage request) { - return; // timeout + using (var requestHandle = service.CreateRequestHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_take_request(service.Handle, requestHeaderHandle, requestHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + requestHandle.DangerousAddRef(ref mustRelease); + request.__ReadFromHandle(requestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + requestHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_request)}() failed."); + } + } } - } - foreach (Subscription subscription in node.Subscriptions) { - IRosMessage message = subscription.CreateMessage(); - bool result = Take(subscription, message); - if (result) { - subscription.TriggerCallback (message); + private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) + { + using (var responseHandle = client.CreateResponseHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_take_response(client.Handle, requestHeaderHandle, responseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + responseHandle.DangerousAddRef(ref mustRelease); + response.__ReadFromHandle(responseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + responseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_take_response)}() failed."); + } + } } - } - - // requestIdHandle gets reused for each element in the loop. - using (SafeRequestIdHandle requestIdHandle = CreateRequestId()) - { - foreach (var service in node.Services) + + private static void SendResponse(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { - var request = service.CreateRequest(); - var response = service.CreateResponse(); + using (var responseHandle = service.CreateResponseHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywere. + // It's not worth it, especialy considering the extra allocations for SafeHandles in + // arrays or collections that don't realy represent their own native recource. + responseHandle.DangerousAddRef(ref mustRelease); + response.__WriteToHandle(responseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + responseHandle.DangerousRelease(); + } + } + + RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); + if (ret != RCLRet.Ok) + { + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); + } + } + } - var result = TakeRequest(service, requestIdHandle, request); - if (result) - { - service.TriggerCallback(request, response); + public static void SpinOnce(Node node, long timeout) + { + int numberOfSubscriptions = node.Subscriptions.Count; + int numberOfGuardConditions = 0; + int numberOfTimers = 0; + int numberOfClients = node.Clients.Count; + int numberOfServices = node.Services.Count; + int numberOfEvents = 0; + + bool waitSetEmpty = numberOfSubscriptions == 0 + && numberOfGuardConditions == 0 + && numberOfTimers == 0 + && numberOfClients == 0 + && numberOfServices == 0 + && numberOfEvents == 0; + + if (waitSetEmpty) + { + // TODO: (sh) Should we sleep here for the timeout to avoid loops without sleep? + // Avoid WaitSetEmpty return value from rcl_wait. + return; + } - SendResponse(service, requestIdHandle, response); - } + // TODO: (sh) Reuse wait set (see executer in rclc, rclcpp and rclpy) + using (SafeWaitSetHandle waitSetHandle = CreateWaitSet( + numberOfSubscriptions, + numberOfGuardConditions, + numberOfTimers, + numberOfClients, + numberOfServices, + numberOfEvents)) + { + WaitSetClear(waitSetHandle); + + foreach (Subscription subscription in node.Subscriptions) + { + WaitSetAddSubscription(waitSetHandle, subscription.Handle); + } + + foreach (var service in node.Services) + { + WaitSetAddService(waitSetHandle, service.Handle); + } + + foreach (var client in node.Clients) + { + WaitSetAddClient(waitSetHandle, client.Handle); + } + + bool ready = Wait(waitSetHandle, timeout); + if (!ready) + { + return; // timeout + } + } + + foreach (Subscription subscription in node.Subscriptions) + { + IRosMessage message = subscription.CreateMessage(); + bool result = Take(subscription, message); + if (result) + { + subscription.TriggerCallback(message); + } + } + + // requestIdHandle gets reused for each element in the loop. + using (SafeRequestIdHandle requestIdHandle = CreateRequestId()) + { + foreach (var service in node.Services) + { + var request = service.CreateRequest(); + var response = service.CreateResponse(); + + var result = TakeRequest(service, requestIdHandle, request); + if (result) + { + service.TriggerCallback(request, response); + + SendResponse(service, requestIdHandle, response); + } + } + + foreach (var client in node.Clients) + { + var response = client.CreateResponse(); + + var result = TakeResponse(client, requestIdHandle, response); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + client.HandleResponse(sequenceNumber, response); + } + } + } } - foreach (var client in node.Clients) + public static void Init() { - var response = client.CreateResponse(); - - var result = TakeResponse(client, requestIdHandle, response); - if (result) - { - var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); - client.HandleResponse(sequenceNumber, response); - } + lock (syncLock) + { + if (!initialized) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_init(); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_init)}() failed."); + initialized = true; + } + } } - } - } - public static void Init () { - lock (syncLock) { - if (!initialized) { - RCLRet ret = RCLdotnetDelegates.native_rcl_init(); - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_init)}() failed."); - initialized = true; + public static string GetRMWIdentifier() + { + IntPtr ptr = RCLdotnetDelegates.native_rcl_get_rmw_identifier(); + string rmw_identifier = Marshal.PtrToStringAnsi(ptr); + return rmw_identifier; } - } - } - - public static string GetRMWIdentifier () { - IntPtr ptr = RCLdotnetDelegates.native_rcl_get_rmw_identifier (); - string rmw_identifier = Marshal.PtrToStringAnsi (ptr); - return rmw_identifier; } - } } diff --git a/rcldotnet/SafeClientHandle.cs b/rcldotnet/SafeClientHandle.cs index c5400eff..98fb121f 100644 --- a/rcldotnet/SafeClientHandle.cs +++ b/rcldotnet/SafeClientHandle.cs @@ -29,7 +29,7 @@ namespace ROS2 /// which point to a rcl_publisher_t, rcl_subscription_t or rcl_service_t. /// internal sealed class SafeClientHandle : SafeHandleZeroOrMinusOneIsInvalid - { + { // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 // Commit from early 2016, but still in current .NET as of september 2021: // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs @@ -53,7 +53,7 @@ protected override bool ReleaseHandle() { var parent = _parent; _parent = null; - + bool successfullyFreed = false; if (parent != null) { @@ -62,7 +62,7 @@ protected override bool ReleaseHandle() RCLRet ret = NodeDelegates.native_rcl_destroy_client_handle(handle, parent); successfullyFreed = ret == RCLRet.Ok; - + parent.DangerousRelease(); } diff --git a/rcldotnet/SafeNodeHandle.cs b/rcldotnet/SafeNodeHandle.cs index 2c6ea769..b9c1f857 100644 --- a/rcldotnet/SafeNodeHandle.cs +++ b/rcldotnet/SafeNodeHandle.cs @@ -12,7 +12,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - + using System.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/SafePublisherHandle.cs b/rcldotnet/SafePublisherHandle.cs index a51d3003..649cf4d2 100644 --- a/rcldotnet/SafePublisherHandle.cs +++ b/rcldotnet/SafePublisherHandle.cs @@ -29,7 +29,7 @@ namespace ROS2 /// which point to a rcl_subscription_t, rcl_service_t or rcl_client_t. /// internal sealed class SafePublisherHandle : SafeHandleZeroOrMinusOneIsInvalid - { + { // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 // Commit from early 2016, but still in current .NET as of september 2021: // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs @@ -53,7 +53,7 @@ protected override bool ReleaseHandle() { var parent = _parent; _parent = null; - + bool successfullyFreed = false; if (parent != null) { @@ -62,7 +62,7 @@ protected override bool ReleaseHandle() RCLRet ret = NodeDelegates.native_rcl_destroy_publisher_handle(handle, parent); successfullyFreed = ret == RCLRet.Ok; - + parent.DangerousRelease(); } diff --git a/rcldotnet/SafeServiceHandle.cs b/rcldotnet/SafeServiceHandle.cs index 3307137b..23b5ff33 100644 --- a/rcldotnet/SafeServiceHandle.cs +++ b/rcldotnet/SafeServiceHandle.cs @@ -29,7 +29,7 @@ namespace ROS2 /// which point to a rcl_publisher_t, rcl_subscription_t or rcl_client_t. /// internal sealed class SafeServiceHandle : SafeHandleZeroOrMinusOneIsInvalid - { + { // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 // Commit from early 2016, but still in current .NET as of september 2021: // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs @@ -53,7 +53,7 @@ protected override bool ReleaseHandle() { var parent = _parent; _parent = null; - + bool successfullyFreed = false; if (parent != null) { @@ -62,7 +62,7 @@ protected override bool ReleaseHandle() RCLRet ret = NodeDelegates.native_rcl_destroy_service_handle(handle, parent); successfullyFreed = ret == RCLRet.Ok; - + parent.DangerousRelease(); } diff --git a/rcldotnet/SafeSubscriptionHandle.cs b/rcldotnet/SafeSubscriptionHandle.cs index 77d0738f..a7075035 100644 --- a/rcldotnet/SafeSubscriptionHandle.cs +++ b/rcldotnet/SafeSubscriptionHandle.cs @@ -29,7 +29,7 @@ namespace ROS2 /// which point to a rcl_publisher_t, rcl_service_t or rcl_client_t. /// internal sealed class SafeSubscriptionHandle : SafeHandleZeroOrMinusOneIsInvalid - { + { // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 // Commit from early 2016, but still in current .NET as of september 2021: // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs @@ -53,7 +53,7 @@ protected override bool ReleaseHandle() { var parent = _parent; _parent = null; - + bool successfullyFreed = false; if (parent != null) { @@ -62,7 +62,7 @@ protected override bool ReleaseHandle() RCLRet ret = NodeDelegates.native_rcl_destroy_subscription_handle(handle, parent); successfullyFreed = ret == RCLRet.Ok; - + parent.DangerousRelease(); } diff --git a/rcldotnet/SafeWaitSetHandle.cs b/rcldotnet/SafeWaitSetHandle.cs index 3a6c0945..2d5faf7c 100644 --- a/rcldotnet/SafeWaitSetHandle.cs +++ b/rcldotnet/SafeWaitSetHandle.cs @@ -12,7 +12,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ - + using System.Diagnostics; using Microsoft.Win32.SafeHandles; diff --git a/rcldotnet/Service.cs b/rcldotnet/Service.cs index dbfebf9d..d5a2f86c 100644 --- a/rcldotnet/Service.cs +++ b/rcldotnet/Service.cs @@ -33,17 +33,17 @@ internal Service() // internal handle is disposed. // By relying on the GC/Finalizer of SafeHandle the handle only gets // Disposed if the service is not live anymore. - abstract internal SafeServiceHandle Handle { get; } + internal abstract SafeServiceHandle Handle { get; } - abstract internal IRosMessage CreateRequest(); - - abstract internal SafeHandle CreateRequestHandle(); + internal abstract IRosMessage CreateRequest(); - abstract internal IRosMessage CreateResponse(); + internal abstract SafeHandle CreateRequestHandle(); - abstract internal SafeHandle CreateResponseHandle(); + internal abstract IRosMessage CreateResponse(); - abstract internal void TriggerCallback(IRosMessage request, IRosMessage response); + internal abstract SafeHandle CreateResponseHandle(); + + internal abstract void TriggerCallback(IRosMessage request, IRosMessage response); } public sealed class Service : Service @@ -51,7 +51,7 @@ public sealed class Service : Service where TRequest : IRosMessage, new() where TResponse : IRosMessage, new() { - private Action _callback; + private readonly Action _callback; internal Service(SafeServiceHandle handle, Action callback) { @@ -59,7 +59,7 @@ internal Service(SafeServiceHandle handle, Action callback) _callback = callback; } - internal override SafeServiceHandle Handle { get; } + internal override SafeServiceHandle Handle { get; } internal override IRosMessage CreateRequest() => (IRosMessage)new TRequest(); diff --git a/rcldotnet/ServiceDefinitionStaticMemberCache.cs b/rcldotnet/ServiceDefinitionStaticMemberCache.cs index 20cb0abc..c222f9c9 100644 --- a/rcldotnet/ServiceDefinitionStaticMemberCache.cs +++ b/rcldotnet/ServiceDefinitionStaticMemberCache.cs @@ -23,7 +23,7 @@ internal static class ServiceDefinitionStaticMemberCache - /// Base class of a Subscription without generic type arguments for use in collections or so. - /// - public abstract class Subscription - { - // Only allow internal subclasses. - internal Subscription() + /// + /// Base class of a Subscription without generic type arguments for use in collections or so. + /// + public abstract class Subscription { - } + // Only allow internal subclasses. + internal Subscription() + { + } - // Subscription does intentionaly (for now) not implement IDisposable as this - // needs some extra consideration how the type works after its - // internal handle is disposed. - // By relying on the GC/Finalizer of SafeHandle the handle only gets - // Disposed if the subscription is not live anymore. - internal abstract SafeSubscriptionHandle Handle { get; } + // Subscription does intentionaly (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the subscription is not live anymore. + internal abstract SafeSubscriptionHandle Handle { get; } - internal abstract IRosMessage CreateMessage(); + internal abstract IRosMessage CreateMessage(); - internal abstract SafeHandle CreateMessageHandle(); + internal abstract SafeHandle CreateMessageHandle(); - internal abstract void TriggerCallback(IRosMessage message); - } + internal abstract void TriggerCallback(IRosMessage message); + } - public class Subscription : Subscription - where T : IRosMessage, new() { - private Action callback_; + public class Subscription : Subscription + where T : IRosMessage, new() + { + private readonly Action callback_; - internal Subscription(SafeSubscriptionHandle handle, Action callback) { - Handle = handle; - callback_ = callback; - } + internal Subscription(SafeSubscriptionHandle handle, Action callback) + { + Handle = handle; + callback_ = callback; + } - internal override SafeSubscriptionHandle Handle { get; } + internal override SafeSubscriptionHandle Handle { get; } - internal override IRosMessage CreateMessage() => (IRosMessage)new T(); + internal override IRosMessage CreateMessage() => (IRosMessage)new T(); - internal override SafeHandle CreateMessageHandle() => MessageStaticMemberCache.CreateMessageHandle(); + internal override SafeHandle CreateMessageHandle() => MessageStaticMemberCache.CreateMessageHandle(); - internal override void TriggerCallback(IRosMessage message) { - callback_((T) message); + internal override void TriggerCallback(IRosMessage message) + { + callback_((T)message); + } } - } } diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index f96e54f2..a344152f 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -6,1411 +6,1411 @@ namespace RCLdotnetTests { - public class TestMessages - { - [Fact] - public void TestPublishString() + public class TestMessages { - RCLdotnet.Init (); - Node node_string_1 = RCLdotnet.CreateNode ("test_string_1"); - Node node_string_2 = RCLdotnet.CreateNode ("test_string_2"); - Publisher chatter_pub = node_string_1.CreatePublisher ("topic_string"); - - std_msgs.msg.String msg = new std_msgs.msg.String (); - std_msgs.msg.String msg2 = new std_msgs.msg.String (); - msg.Data = "Hello"; - - bool received=false; - Subscription chatter_sub = node_string_2.CreateSubscription ( - "topic_string", rcv_msg => + [Fact] + public void TestPublishString() { - received=true; - msg2 = rcv_msg; + RCLdotnet.Init(); + Node node_string_1 = RCLdotnet.CreateNode("test_string_1"); + Node node_string_2 = RCLdotnet.CreateNode("test_string_2"); + Publisher chatter_pub = node_string_1.CreatePublisher("topic_string"); + + std_msgs.msg.String msg = new std_msgs.msg.String(); + std_msgs.msg.String msg2 = new std_msgs.msg.String(); + msg.Data = "Hello"; + + bool received = false; + Subscription chatter_sub = node_string_2.CreateSubscription( + "topic_string", rcv_msg => + { + received = true; + msg2 = rcv_msg; + } + ); + + while (!received) + { + chatter_pub.Publish(msg); + + RCLdotnet.SpinOnce(node_string_1, 500); + RCLdotnet.SpinOnce(node_string_2, 500); + } + Assert.Equal("Hello", msg2.Data); } - ); - while(!received) - { - chatter_pub.Publish (msg); - - RCLdotnet.SpinOnce(node_string_1, 500); - RCLdotnet.SpinOnce(node_string_2, 500); - } - Assert.Equal("Hello", msg2.Data); - } - - [Fact] - public void TestPublishBuiltins() - { - RCLdotnet.Init (); - Node node_builtins_1 = RCLdotnet.CreateNode ("test_builtins_1"); - Node node_builtins_2 = RCLdotnet.CreateNode ("test_builtins_2"); - Publisher chatter_pub = node_builtins_1.CreatePublisher ("topic_builtins"); - - test_msgs.msg.Builtins msg = new test_msgs.msg.Builtins (); - test_msgs.msg.Builtins msg2 = new test_msgs.msg.Builtins (); - msg.DurationValue.Sec = 1; - msg.DurationValue.Nanosec = 2u; - msg.TimeValue.Sec = 3; - msg.TimeValue.Nanosec = 4u; - - bool received=false; - Subscription chatter_sub = node_builtins_2.CreateSubscription ( - "topic_builtins", rcv_msg => + [Fact] + public void TestPublishBuiltins() { - received=true; - msg2 = rcv_msg; + RCLdotnet.Init(); + Node node_builtins_1 = RCLdotnet.CreateNode("test_builtins_1"); + Node node_builtins_2 = RCLdotnet.CreateNode("test_builtins_2"); + Publisher chatter_pub = node_builtins_1.CreatePublisher("topic_builtins"); + + test_msgs.msg.Builtins msg = new test_msgs.msg.Builtins(); + test_msgs.msg.Builtins msg2 = new test_msgs.msg.Builtins(); + msg.DurationValue.Sec = 1; + msg.DurationValue.Nanosec = 2u; + msg.TimeValue.Sec = 3; + msg.TimeValue.Nanosec = 4u; + + bool received = false; + Subscription chatter_sub = node_builtins_2.CreateSubscription( + "topic_builtins", rcv_msg => + { + received = true; + msg2 = rcv_msg; + } + ); + + while (!received) + { + chatter_pub.Publish(msg); + + RCLdotnet.SpinOnce(node_builtins_1, 500); + RCLdotnet.SpinOnce(node_builtins_2, 500); + } + + Assert.Equal(1, msg2.DurationValue.Sec); + Assert.Equal(2u, msg2.DurationValue.Nanosec); + Assert.Equal(3, msg2.TimeValue.Sec); + Assert.Equal(4u, msg2.TimeValue.Nanosec); } - ); - - while(!received) - { - chatter_pub.Publish (msg); - - RCLdotnet.SpinOnce(node_builtins_1, 500); - RCLdotnet.SpinOnce(node_builtins_2, 500); - } - Assert.Equal(1, msg2.DurationValue.Sec); - Assert.Equal(2u, msg2.DurationValue.Nanosec); - Assert.Equal(3, msg2.TimeValue.Sec); - Assert.Equal(4u, msg2.TimeValue.Nanosec); - } - - [Fact] - public void TestPublishArrays() - { - RCLdotnet.Init (); - Node node_array_1 = RCLdotnet.CreateNode ("test_arrays_1"); - Node node_array_2 = RCLdotnet.CreateNode ("test_arrays_2"); - Publisher chatter_pub = node_array_1.CreatePublisher ("topic_array"); - - test_msgs.msg.Arrays msg = new test_msgs.msg.Arrays (); - test_msgs.msg.Arrays msg2 = new test_msgs.msg.Arrays (); - - // boolValues - msg.BoolValues[0] = true; - msg.BoolValues[1] = false; - msg.BoolValues[2] = true; - - // byteValues - msg.ByteValues[0] = 0; - msg.ByteValues[1] = 1; - msg.ByteValues[2] = 2; - - // charValues - msg.CharValues[0] = 3; - msg.CharValues[1] = 4; - msg.CharValues[2] = 5; - - // float32Values - msg.Float32Values[0] = 6.1f; - msg.Float32Values[1] = 7.1f; - msg.Float32Values[2] = 8.1f; - - // float64Values - msg.Float64Values[0] = 9.1; - msg.Float64Values[1] = 10.1; - msg.Float64Values[2] = 11.1; - - // int8Values - msg.Int8Values[0] = 12; - msg.Int8Values[1] = 13; - msg.Int8Values[2] = 14; - - // uint8Values - msg.Uint8Values[0] = 15; - msg.Uint8Values[1] = 16; - msg.Uint8Values[2] = 17; - - // int16Values - msg.Int16Values[0] = 18; - msg.Int16Values[1] = 19; - msg.Int16Values[2] = 20; - - // uint16Values - msg.Uint16Values[0] = 21; - msg.Uint16Values[1] = 22; - msg.Uint16Values[2] = 23; - - // int32Values - msg.Int32Values[0] = 24; - msg.Int32Values[1] = 25; - msg.Int32Values[2] = 26; - - // uint32Values - msg.Uint32Values[0] = 27; - msg.Uint32Values[1] = 28; - msg.Uint32Values[2] = 29; - - // int64Values - msg.Int64Values[0] = 30; - msg.Int64Values[1] = 31; - msg.Int64Values[2] = 32; - - // uint64Values - msg.Uint64Values[0] = 33; - msg.Uint64Values[1] = 34; - msg.Uint64Values[2] = 35; - - // stringValues - msg.StringValues[0] = "one"; - msg.StringValues[1] = "two"; - msg.StringValues[2] = "three"; - - // basicTypesValues - test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.BoolValue = true; - basic_type_1.ByteValue = 36; - basic_type_1.CharValue = 37; - basic_type_1.Float32Value = 38.1f; - basic_type_1.Float64Value = 39.1; - basic_type_1.Int8Value = 40; - basic_type_1.Uint8Value = 41; - basic_type_1.Int16Value = 42; - basic_type_1.Uint16Value = 43; - basic_type_1.Int32Value = 44; - basic_type_1.Uint32Value = 45; - basic_type_1.Int64Value = 46; - basic_type_1.Uint64Value = 47; - - test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.BoolValue = false; - basic_type_2.ByteValue = 48; - basic_type_2.CharValue = 49; - basic_type_2.Float32Value = 50.1f; - basic_type_2.Float64Value = 51.1; - basic_type_2.Int8Value = 52; - basic_type_2.Uint8Value = 53; - basic_type_2.Int16Value = 54; - basic_type_2.Uint16Value = 55; - basic_type_2.Int32Value = 56; - basic_type_2.Uint32Value = 57; - basic_type_2.Int64Value = 58; - basic_type_2.Uint64Value = 59; - - test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.BoolValue = true; - basic_type_3.ByteValue = 60; - basic_type_3.CharValue = 61; - basic_type_3.Float32Value = 62.1f; - basic_type_3.Float64Value = 63.1; - basic_type_3.Int8Value = 64; - basic_type_3.Uint8Value = 65; - basic_type_3.Int16Value = 66; - basic_type_3.Uint16Value = 67; - basic_type_3.Int32Value = 68; - basic_type_3.Uint32Value = 69; - basic_type_3.Int64Value = 70; - basic_type_3.Uint64Value = 71; - - msg.BasicTypesValues[0] = basic_type_1; - msg.BasicTypesValues[1] = basic_type_2; - msg.BasicTypesValues[2] = basic_type_3; - - bool received=false; - Subscription chatter_sub = node_array_2.CreateSubscription ( - "topic_array", rcv_msg => + [Fact] + public void TestPublishArrays() { - received = true; - msg2 = rcv_msg; + RCLdotnet.Init(); + Node node_array_1 = RCLdotnet.CreateNode("test_arrays_1"); + Node node_array_2 = RCLdotnet.CreateNode("test_arrays_2"); + Publisher chatter_pub = node_array_1.CreatePublisher("topic_array"); + + test_msgs.msg.Arrays msg = new test_msgs.msg.Arrays(); + test_msgs.msg.Arrays msg2 = new test_msgs.msg.Arrays(); + + // boolValues + msg.BoolValues[0] = true; + msg.BoolValues[1] = false; + msg.BoolValues[2] = true; + + // byteValues + msg.ByteValues[0] = 0; + msg.ByteValues[1] = 1; + msg.ByteValues[2] = 2; + + // charValues + msg.CharValues[0] = 3; + msg.CharValues[1] = 4; + msg.CharValues[2] = 5; + + // float32Values + msg.Float32Values[0] = 6.1f; + msg.Float32Values[1] = 7.1f; + msg.Float32Values[2] = 8.1f; + + // float64Values + msg.Float64Values[0] = 9.1; + msg.Float64Values[1] = 10.1; + msg.Float64Values[2] = 11.1; + + // int8Values + msg.Int8Values[0] = 12; + msg.Int8Values[1] = 13; + msg.Int8Values[2] = 14; + + // uint8Values + msg.Uint8Values[0] = 15; + msg.Uint8Values[1] = 16; + msg.Uint8Values[2] = 17; + + // int16Values + msg.Int16Values[0] = 18; + msg.Int16Values[1] = 19; + msg.Int16Values[2] = 20; + + // uint16Values + msg.Uint16Values[0] = 21; + msg.Uint16Values[1] = 22; + msg.Uint16Values[2] = 23; + + // int32Values + msg.Int32Values[0] = 24; + msg.Int32Values[1] = 25; + msg.Int32Values[2] = 26; + + // uint32Values + msg.Uint32Values[0] = 27; + msg.Uint32Values[1] = 28; + msg.Uint32Values[2] = 29; + + // int64Values + msg.Int64Values[0] = 30; + msg.Int64Values[1] = 31; + msg.Int64Values[2] = 32; + + // uint64Values + msg.Uint64Values[0] = 33; + msg.Uint64Values[1] = 34; + msg.Uint64Values[2] = 35; + + // stringValues + msg.StringValues[0] = "one"; + msg.StringValues[1] = "two"; + msg.StringValues[2] = "three"; + + // basicTypesValues + test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); + basic_type_1.BoolValue = true; + basic_type_1.ByteValue = 36; + basic_type_1.CharValue = 37; + basic_type_1.Float32Value = 38.1f; + basic_type_1.Float64Value = 39.1; + basic_type_1.Int8Value = 40; + basic_type_1.Uint8Value = 41; + basic_type_1.Int16Value = 42; + basic_type_1.Uint16Value = 43; + basic_type_1.Int32Value = 44; + basic_type_1.Uint32Value = 45; + basic_type_1.Int64Value = 46; + basic_type_1.Uint64Value = 47; + + test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); + basic_type_2.BoolValue = false; + basic_type_2.ByteValue = 48; + basic_type_2.CharValue = 49; + basic_type_2.Float32Value = 50.1f; + basic_type_2.Float64Value = 51.1; + basic_type_2.Int8Value = 52; + basic_type_2.Uint8Value = 53; + basic_type_2.Int16Value = 54; + basic_type_2.Uint16Value = 55; + basic_type_2.Int32Value = 56; + basic_type_2.Uint32Value = 57; + basic_type_2.Int64Value = 58; + basic_type_2.Uint64Value = 59; + + test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); + basic_type_3.BoolValue = true; + basic_type_3.ByteValue = 60; + basic_type_3.CharValue = 61; + basic_type_3.Float32Value = 62.1f; + basic_type_3.Float64Value = 63.1; + basic_type_3.Int8Value = 64; + basic_type_3.Uint8Value = 65; + basic_type_3.Int16Value = 66; + basic_type_3.Uint16Value = 67; + basic_type_3.Int32Value = 68; + basic_type_3.Uint32Value = 69; + basic_type_3.Int64Value = 70; + basic_type_3.Uint64Value = 71; + + msg.BasicTypesValues[0] = basic_type_1; + msg.BasicTypesValues[1] = basic_type_2; + msg.BasicTypesValues[2] = basic_type_3; + + bool received = false; + Subscription chatter_sub = node_array_2.CreateSubscription( + "topic_array", rcv_msg => + { + received = true; + msg2 = rcv_msg; + } + ); + + while (!received) + { + chatter_pub.Publish(msg); + + RCLdotnet.SpinOnce(node_array_1, 500); + RCLdotnet.SpinOnce(node_array_2, 500); + } + + // boolValues + Assert.Equal(3, test_msgs.msg.Arrays.BoolValuesLength); + Assert.Equal(3, msg2.BoolValues.Length); + Assert.True(msg2.BoolValues[0]); + Assert.False(msg2.BoolValues[1]); + Assert.True(msg2.BoolValues[2]); + + // byteValues + Assert.Equal(3, test_msgs.msg.Arrays.ByteValuesLength); + Assert.Equal(3, msg2.ByteValues.Length); + Assert.Equal(0, msg2.ByteValues[0]); + Assert.Equal(1, msg2.ByteValues[1]); + Assert.Equal(2, msg2.ByteValues[2]); + + // charValues + Assert.Equal(3, test_msgs.msg.Arrays.CharValuesLength); + Assert.Equal(3, msg2.CharValues.Length); + Assert.Equal(3, msg2.CharValues[0]); + Assert.Equal(4, msg2.CharValues[1]); + Assert.Equal(5, msg2.CharValues[2]); + + // float32Values + Assert.Equal(3, test_msgs.msg.Arrays.Float32ValuesLength); + Assert.Equal(3, msg2.Float32Values.Length); + Assert.Equal(6.1f, msg2.Float32Values[0]); + Assert.Equal(7.1f, msg2.Float32Values[1]); + Assert.Equal(8.1f, msg2.Float32Values[2]); + + // float64Values + Assert.Equal(3, test_msgs.msg.Arrays.Float64ValuesLength); + Assert.Equal(3, msg2.Float64Values.Length); + Assert.Equal(9.1, msg2.Float64Values[0]); + Assert.Equal(10.1, msg2.Float64Values[1]); + Assert.Equal(11.1, msg2.Float64Values[2]); + + // int8Values + Assert.Equal(3, test_msgs.msg.Arrays.Int8ValuesLength); + Assert.Equal(3, msg2.Int8Values.Length); + Assert.Equal(12, msg2.Int8Values[0]); + Assert.Equal(13, msg2.Int8Values[1]); + Assert.Equal(14, msg2.Int8Values[2]); + + // uint8Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint8ValuesLength); + Assert.Equal(3, msg2.Uint8Values.Length); + Assert.Equal(15, msg2.Uint8Values[0]); + Assert.Equal(16, msg2.Uint8Values[1]); + Assert.Equal(17, msg2.Uint8Values[2]); + + // int16Values + Assert.Equal(3, test_msgs.msg.Arrays.Int16ValuesLength); + Assert.Equal(3, msg2.Int16Values.Length); + Assert.Equal(18, msg2.Int16Values[0]); + Assert.Equal(19, msg2.Int16Values[1]); + Assert.Equal(20, msg2.Int16Values[2]); + + // uint16Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint16ValuesLength); + Assert.Equal(3, msg2.Uint16Values.Length); + Assert.Equal(21, msg2.Uint16Values[0]); + Assert.Equal(22, msg2.Uint16Values[1]); + Assert.Equal(23, msg2.Uint16Values[2]); + + // int32Values + Assert.Equal(3, test_msgs.msg.Arrays.Int32ValuesLength); + Assert.Equal(3, msg2.Int32Values.Length); + Assert.Equal(24, msg2.Int32Values[0]); + Assert.Equal(25, msg2.Int32Values[1]); + Assert.Equal(26, msg2.Int32Values[2]); + + // uint32Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint32ValuesLength); + Assert.Equal(3, msg2.Uint32Values.Length); + Assert.Equal((uint)27, msg2.Uint32Values[0]); + Assert.Equal((uint)28, msg2.Uint32Values[1]); + Assert.Equal((uint)29, msg2.Uint32Values[2]); + + // int64Values + Assert.Equal(3, test_msgs.msg.Arrays.Int64ValuesLength); + Assert.Equal(3, msg2.Int64Values.Length); + Assert.Equal(30, msg2.Int64Values[0]); + Assert.Equal(31, msg2.Int64Values[1]); + Assert.Equal(32, msg2.Int64Values[2]); + + // uint64Values + Assert.Equal(3, test_msgs.msg.Arrays.Uint64ValuesLength); + Assert.Equal(3, msg2.Uint64Values.Length); + Assert.Equal((ulong)33, msg2.Uint64Values[0]); + Assert.Equal((ulong)34, msg2.Uint64Values[1]); + Assert.Equal((ulong)35, msg2.Uint64Values[2]); + + // stringValues + Assert.Equal(3, test_msgs.msg.Arrays.StringValuesLength); + Assert.Equal(3, msg2.StringValues.Length); + Assert.Equal("one", msg2.StringValues[0]); + Assert.Equal("two", msg2.StringValues[1]); + Assert.Equal("three", msg2.StringValues[2]); + + // basicTypesValues + Assert.Equal(3, test_msgs.msg.Arrays.BasicTypesValuesLength); + Assert.Equal(3, msg2.BasicTypesValues.Length); + + Assert.True(msg2.BasicTypesValues[0].BoolValue); + Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); + Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); + Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); + Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); + Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); + Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); + Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); + Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); + Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); + Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); + Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); + Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); + + Assert.False(msg2.BasicTypesValues[1].BoolValue); + Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); + Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); + Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); + Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); + Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); + Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); + Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); + Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); + Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); + Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); + Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); + Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); + + Assert.True(msg2.BasicTypesValues[2].BoolValue); + Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); + Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); + Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); + Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); + Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); + Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); + Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); + Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); + Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); + Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); + Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); + Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); } - ); - - while (!received) - { - chatter_pub.Publish(msg); - - RCLdotnet.SpinOnce(node_array_1, 500); - RCLdotnet.SpinOnce(node_array_2, 500); - } - - // boolValues - Assert.Equal(3, test_msgs.msg.Arrays.BoolValuesLength); - Assert.Equal(3, msg2.BoolValues.Length); - Assert.True(msg2.BoolValues[0]); - Assert.False(msg2.BoolValues[1]); - Assert.True(msg2.BoolValues[2]); - - // byteValues - Assert.Equal(3, test_msgs.msg.Arrays.ByteValuesLength); - Assert.Equal(3, msg2.ByteValues.Length); - Assert.Equal(0, msg2.ByteValues[0]); - Assert.Equal(1, msg2.ByteValues[1]); - Assert.Equal(2, msg2.ByteValues[2]); - - // charValues - Assert.Equal(3, test_msgs.msg.Arrays.CharValuesLength); - Assert.Equal(3, msg2.CharValues.Length); - Assert.Equal(3, msg2.CharValues[0]); - Assert.Equal(4, msg2.CharValues[1]); - Assert.Equal(5, msg2.CharValues[2]); - - // float32Values - Assert.Equal(3, test_msgs.msg.Arrays.Float32ValuesLength); - Assert.Equal(3, msg2.Float32Values.Length); - Assert.Equal(6.1f, msg2.Float32Values[0]); - Assert.Equal(7.1f, msg2.Float32Values[1]); - Assert.Equal(8.1f, msg2.Float32Values[2]); - - // float64Values - Assert.Equal(3, test_msgs.msg.Arrays.Float64ValuesLength); - Assert.Equal(3, msg2.Float64Values.Length); - Assert.Equal(9.1, msg2.Float64Values[0]); - Assert.Equal(10.1, msg2.Float64Values[1]); - Assert.Equal(11.1, msg2.Float64Values[2]); - - // int8Values - Assert.Equal(3, test_msgs.msg.Arrays.Int8ValuesLength); - Assert.Equal(3, msg2.Int8Values.Length); - Assert.Equal(12, msg2.Int8Values[0]); - Assert.Equal(13, msg2.Int8Values[1]); - Assert.Equal(14, msg2.Int8Values[2]); - - // uint8Values - Assert.Equal(3, test_msgs.msg.Arrays.Uint8ValuesLength); - Assert.Equal(3, msg2.Uint8Values.Length); - Assert.Equal(15, msg2.Uint8Values[0]); - Assert.Equal(16, msg2.Uint8Values[1]); - Assert.Equal(17, msg2.Uint8Values[2]); - - // int16Values - Assert.Equal(3, test_msgs.msg.Arrays.Int16ValuesLength); - Assert.Equal(3, msg2.Int16Values.Length); - Assert.Equal(18, msg2.Int16Values[0]); - Assert.Equal(19, msg2.Int16Values[1]); - Assert.Equal(20, msg2.Int16Values[2]); - - // uint16Values - Assert.Equal(3, test_msgs.msg.Arrays.Uint16ValuesLength); - Assert.Equal(3, msg2.Uint16Values.Length); - Assert.Equal(21, msg2.Uint16Values[0]); - Assert.Equal(22, msg2.Uint16Values[1]); - Assert.Equal(23, msg2.Uint16Values[2]); - - // int32Values - Assert.Equal(3, test_msgs.msg.Arrays.Int32ValuesLength); - Assert.Equal(3, msg2.Int32Values.Length); - Assert.Equal(24, msg2.Int32Values[0]); - Assert.Equal(25, msg2.Int32Values[1]); - Assert.Equal(26, msg2.Int32Values[2]); - - // uint32Values - Assert.Equal(3, test_msgs.msg.Arrays.Uint32ValuesLength); - Assert.Equal(3, msg2.Uint32Values.Length); - Assert.Equal((uint)27, msg2.Uint32Values[0]); - Assert.Equal((uint)28, msg2.Uint32Values[1]); - Assert.Equal((uint)29, msg2.Uint32Values[2]); - - // int64Values - Assert.Equal(3, test_msgs.msg.Arrays.Int64ValuesLength); - Assert.Equal(3, msg2.Int64Values.Length); - Assert.Equal(30, msg2.Int64Values[0]); - Assert.Equal(31, msg2.Int64Values[1]); - Assert.Equal(32, msg2.Int64Values[2]); - - // uint64Values - Assert.Equal(3, test_msgs.msg.Arrays.Uint64ValuesLength); - Assert.Equal(3, msg2.Uint64Values.Length); - Assert.Equal((ulong)33, msg2.Uint64Values[0]); - Assert.Equal((ulong)34, msg2.Uint64Values[1]); - Assert.Equal((ulong)35, msg2.Uint64Values[2]); - - // stringValues - Assert.Equal(3, test_msgs.msg.Arrays.StringValuesLength); - Assert.Equal(3, msg2.StringValues.Length); - Assert.Equal("one", msg2.StringValues[0]); - Assert.Equal("two", msg2.StringValues[1]); - Assert.Equal("three", msg2.StringValues[2]); - - // basicTypesValues - Assert.Equal(3, test_msgs.msg.Arrays.BasicTypesValuesLength); - Assert.Equal(3, msg2.BasicTypesValues.Length); - - Assert.True(msg2.BasicTypesValues[0].BoolValue); - Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); - Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); - Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); - Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); - Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); - Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); - Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); - Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); - Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); - Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); - Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); - Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); - - Assert.False(msg2.BasicTypesValues[1].BoolValue); - Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); - Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); - Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); - Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); - Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); - Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); - Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); - Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); - Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); - Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); - Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); - Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); - - Assert.True(msg2.BasicTypesValues[2].BoolValue); - Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); - Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); - Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); - Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); - Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); - Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); - Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); - Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); - Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); - Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); - Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); - Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); - } - [Fact] - public void TestPublishArraysSizeCheckToLittle() - { - RCLdotnet.Init(); - var nodeArraysSizeCheck = RCLdotnet.CreateNode("test_arrays_size_check_to_little"); - var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_little"); - - var msg = new test_msgs.msg.Arrays(); - msg.BoolValues = new bool[] - { - false, - true, - }; - - var exception = Assert.Throws(() => chatterPub.Publish(msg)); - Assert.Equal("Invalid size of array 'BoolValues'.", exception.Message); - } + [Fact] + public void TestPublishArraysSizeCheckToLittle() + { + RCLdotnet.Init(); + var nodeArraysSizeCheck = RCLdotnet.CreateNode("test_arrays_size_check_to_little"); + var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_little"); + + var msg = new test_msgs.msg.Arrays(); + msg.BoolValues = new bool[] + { + false, + true, + }; + + var exception = Assert.Throws(() => chatterPub.Publish(msg)); + Assert.Equal("Invalid size of array 'BoolValues'.", exception.Message); + } - [Fact] - public void TestPublishArraysSizeCheckToMuch() - { - RCLdotnet.Init(); - var nodeArraysSizeCheck = RCLdotnet.CreateNode("test_arrays_size_check_to_much"); - var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_much"); - - var msg = new test_msgs.msg.Arrays(); - msg.StringValues = new string[] - { - "0", - "1", - "2", - "3", - }; - - var exception = Assert.Throws(() => chatterPub.Publish(msg)); - Assert.Equal("Invalid size of array 'StringValues'.", exception.Message); - } + [Fact] + public void TestPublishArraysSizeCheckToMuch() + { + RCLdotnet.Init(); + var nodeArraysSizeCheck = RCLdotnet.CreateNode("test_arrays_size_check_to_much"); + var chatterPub = nodeArraysSizeCheck.CreatePublisher("topic_array_size_check_to_much"); + + var msg = new test_msgs.msg.Arrays(); + msg.StringValues = new string[] + { + "0", + "1", + "2", + "3", + }; + + var exception = Assert.Throws(() => chatterPub.Publish(msg)); + Assert.Equal("Invalid size of array 'StringValues'.", exception.Message); + } - [Fact] - public void TestPublishUnboundedSequences() - { - RCLdotnet.Init(); - Node node_array_1 = RCLdotnet.CreateNode("test_unbounded_sequences_1"); - Node node_array_2 = RCLdotnet.CreateNode("test_unbounded_sequences_2"); - Publisher chatter_pub = node_array_1.CreatePublisher("topic_unbounded_sequences"); - - test_msgs.msg.UnboundedSequences msg = new test_msgs.msg.UnboundedSequences(); - test_msgs.msg.UnboundedSequences msg2 = new test_msgs.msg.UnboundedSequences(); - - // boolValues - msg.BoolValues.Add(true); - msg.BoolValues.Add(false); - msg.BoolValues.Add(true); - - // byteValues - msg.ByteValues.Add(0); - msg.ByteValues.Add(1); - msg.ByteValues.Add(2); - - // charValues - msg.CharValues.Add(3); - msg.CharValues.Add(4); - msg.CharValues.Add(5); - - // float32Values - msg.Float32Values.Add(6.1f); - msg.Float32Values.Add(7.1f); - msg.Float32Values.Add(8.1f); - - // float64Values - msg.Float64Values.Add(9.1); - msg.Float64Values.Add(10.1); - msg.Float64Values.Add(11.1); - - // int8Values - msg.Int8Values.Add(12); - msg.Int8Values.Add(13); - msg.Int8Values.Add(14); - - // uint8Values - msg.Uint8Values.Add(15); - msg.Uint8Values.Add(16); - msg.Uint8Values.Add(17); - - // int16Values - msg.Int16Values.Add(18); - msg.Int16Values.Add(19); - msg.Int16Values.Add(20); - - // uint16Values - msg.Uint16Values.Add(21); - msg.Uint16Values.Add(22); - msg.Uint16Values.Add(23); - - // int32Values - msg.Int32Values.Add(24); - msg.Int32Values.Add(25); - msg.Int32Values.Add(26); - - // uint32Values - msg.Uint32Values.Add(27); - msg.Uint32Values.Add(28); - msg.Uint32Values.Add(29); - - // int64Values - msg.Int64Values.Add(30); - msg.Int64Values.Add(31); - msg.Int64Values.Add(32); - - // uint64Values - msg.Uint64Values.Add(33); - msg.Uint64Values.Add(34); - msg.Uint64Values.Add(35); - - // stringValues - msg.StringValues.Add("one"); - msg.StringValues.Add("two"); - msg.StringValues.Add("three"); - - test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.BoolValue = true; - basic_type_1.ByteValue = 36; - basic_type_1.CharValue = 37; - basic_type_1.Float32Value = 38.1f; - basic_type_1.Float64Value = 39.1; - basic_type_1.Int8Value = 40; - basic_type_1.Uint8Value = 41; - basic_type_1.Int16Value = 42; - basic_type_1.Uint16Value = 43; - basic_type_1.Int32Value = 44; - basic_type_1.Uint32Value = 45; - basic_type_1.Int64Value = 46; - basic_type_1.Uint64Value = 47; - - test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.BoolValue = false; - basic_type_2.ByteValue = 48; - basic_type_2.CharValue = 49; - basic_type_2.Float32Value = 50.1f; - basic_type_2.Float64Value = 51.1; - basic_type_2.Int8Value = 52; - basic_type_2.Uint8Value = 53; - basic_type_2.Int16Value = 54; - basic_type_2.Uint16Value = 55; - basic_type_2.Int32Value = 56; - basic_type_2.Uint32Value = 57; - basic_type_2.Int64Value = 58; - basic_type_2.Uint64Value = 59; - - test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.BoolValue = true; - basic_type_3.ByteValue = 60; - basic_type_3.CharValue = 61; - basic_type_3.Float32Value = 62.1f; - basic_type_3.Float64Value = 63.1; - basic_type_3.Int8Value = 64; - basic_type_3.Uint8Value = 65; - basic_type_3.Int16Value = 66; - basic_type_3.Uint16Value = 67; - basic_type_3.Int32Value = 68; - basic_type_3.Uint32Value = 69; - basic_type_3.Int64Value = 70; - basic_type_3.Uint64Value = 71; - - msg.BasicTypesValues.Add(basic_type_1); - msg.BasicTypesValues.Add(basic_type_2); - msg.BasicTypesValues.Add(basic_type_3); - - bool received=false; - Subscription chatter_sub = node_array_2.CreateSubscription( - "topic_unbounded_sequences", rcv_msg => + [Fact] + public void TestPublishUnboundedSequences() { - received=true; - msg2 = rcv_msg; + RCLdotnet.Init(); + Node node_array_1 = RCLdotnet.CreateNode("test_unbounded_sequences_1"); + Node node_array_2 = RCLdotnet.CreateNode("test_unbounded_sequences_2"); + Publisher chatter_pub = node_array_1.CreatePublisher("topic_unbounded_sequences"); + + test_msgs.msg.UnboundedSequences msg = new test_msgs.msg.UnboundedSequences(); + test_msgs.msg.UnboundedSequences msg2 = new test_msgs.msg.UnboundedSequences(); + + // boolValues + msg.BoolValues.Add(true); + msg.BoolValues.Add(false); + msg.BoolValues.Add(true); + + // byteValues + msg.ByteValues.Add(0); + msg.ByteValues.Add(1); + msg.ByteValues.Add(2); + + // charValues + msg.CharValues.Add(3); + msg.CharValues.Add(4); + msg.CharValues.Add(5); + + // float32Values + msg.Float32Values.Add(6.1f); + msg.Float32Values.Add(7.1f); + msg.Float32Values.Add(8.1f); + + // float64Values + msg.Float64Values.Add(9.1); + msg.Float64Values.Add(10.1); + msg.Float64Values.Add(11.1); + + // int8Values + msg.Int8Values.Add(12); + msg.Int8Values.Add(13); + msg.Int8Values.Add(14); + + // uint8Values + msg.Uint8Values.Add(15); + msg.Uint8Values.Add(16); + msg.Uint8Values.Add(17); + + // int16Values + msg.Int16Values.Add(18); + msg.Int16Values.Add(19); + msg.Int16Values.Add(20); + + // uint16Values + msg.Uint16Values.Add(21); + msg.Uint16Values.Add(22); + msg.Uint16Values.Add(23); + + // int32Values + msg.Int32Values.Add(24); + msg.Int32Values.Add(25); + msg.Int32Values.Add(26); + + // uint32Values + msg.Uint32Values.Add(27); + msg.Uint32Values.Add(28); + msg.Uint32Values.Add(29); + + // int64Values + msg.Int64Values.Add(30); + msg.Int64Values.Add(31); + msg.Int64Values.Add(32); + + // uint64Values + msg.Uint64Values.Add(33); + msg.Uint64Values.Add(34); + msg.Uint64Values.Add(35); + + // stringValues + msg.StringValues.Add("one"); + msg.StringValues.Add("two"); + msg.StringValues.Add("three"); + + test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); + basic_type_1.BoolValue = true; + basic_type_1.ByteValue = 36; + basic_type_1.CharValue = 37; + basic_type_1.Float32Value = 38.1f; + basic_type_1.Float64Value = 39.1; + basic_type_1.Int8Value = 40; + basic_type_1.Uint8Value = 41; + basic_type_1.Int16Value = 42; + basic_type_1.Uint16Value = 43; + basic_type_1.Int32Value = 44; + basic_type_1.Uint32Value = 45; + basic_type_1.Int64Value = 46; + basic_type_1.Uint64Value = 47; + + test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); + basic_type_2.BoolValue = false; + basic_type_2.ByteValue = 48; + basic_type_2.CharValue = 49; + basic_type_2.Float32Value = 50.1f; + basic_type_2.Float64Value = 51.1; + basic_type_2.Int8Value = 52; + basic_type_2.Uint8Value = 53; + basic_type_2.Int16Value = 54; + basic_type_2.Uint16Value = 55; + basic_type_2.Int32Value = 56; + basic_type_2.Uint32Value = 57; + basic_type_2.Int64Value = 58; + basic_type_2.Uint64Value = 59; + + test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); + basic_type_3.BoolValue = true; + basic_type_3.ByteValue = 60; + basic_type_3.CharValue = 61; + basic_type_3.Float32Value = 62.1f; + basic_type_3.Float64Value = 63.1; + basic_type_3.Int8Value = 64; + basic_type_3.Uint8Value = 65; + basic_type_3.Int16Value = 66; + basic_type_3.Uint16Value = 67; + basic_type_3.Int32Value = 68; + basic_type_3.Uint32Value = 69; + basic_type_3.Int64Value = 70; + basic_type_3.Uint64Value = 71; + + msg.BasicTypesValues.Add(basic_type_1); + msg.BasicTypesValues.Add(basic_type_2); + msg.BasicTypesValues.Add(basic_type_3); + + bool received = false; + Subscription chatter_sub = node_array_2.CreateSubscription( + "topic_unbounded_sequences", rcv_msg => + { + received = true; + msg2 = rcv_msg; + } + ); + + while (!received) + { + chatter_pub.Publish(msg); + + RCLdotnet.SpinOnce(node_array_1, 500); + RCLdotnet.SpinOnce(node_array_2, 500); + } + + // boolValues + Assert.Equal(3, msg2.BoolValues.Count); + Assert.True(msg2.BoolValues[0]); + Assert.False(msg2.BoolValues[1]); + Assert.True(msg2.BoolValues[2]); + + // byteValues + Assert.Equal(3, msg2.ByteValues.Count); + Assert.Equal(0, msg2.ByteValues[0]); + Assert.Equal(1, msg2.ByteValues[1]); + Assert.Equal(2, msg2.ByteValues[2]); + + // charValues + Assert.Equal(3, msg2.CharValues.Count); + Assert.Equal(3, msg2.CharValues[0]); + Assert.Equal(4, msg2.CharValues[1]); + Assert.Equal(5, msg2.CharValues[2]); + + // float32Values + Assert.Equal(3, msg2.Float32Values.Count); + Assert.Equal(6.1f, msg2.Float32Values[0]); + Assert.Equal(7.1f, msg2.Float32Values[1]); + Assert.Equal(8.1f, msg2.Float32Values[2]); + + // float64Values + Assert.Equal(3, msg2.Float64Values.Count); + Assert.Equal(9.1, msg2.Float64Values[0]); + Assert.Equal(10.1, msg2.Float64Values[1]); + Assert.Equal(11.1, msg2.Float64Values[2]); + + // int8Values + Assert.Equal(3, msg2.Int8Values.Count); + Assert.Equal(12, msg2.Int8Values[0]); + Assert.Equal(13, msg2.Int8Values[1]); + Assert.Equal(14, msg2.Int8Values[2]); + + // uint8Values + Assert.Equal(3, msg2.Uint8Values.Count); + Assert.Equal(15, msg2.Uint8Values[0]); + Assert.Equal(16, msg2.Uint8Values[1]); + Assert.Equal(17, msg2.Uint8Values[2]); + + // int16Values + Assert.Equal(3, msg2.Int16Values.Count); + Assert.Equal(18, msg2.Int16Values[0]); + Assert.Equal(19, msg2.Int16Values[1]); + Assert.Equal(20, msg2.Int16Values[2]); + + // uint16Values + Assert.Equal(3, msg2.Uint16Values.Count); + Assert.Equal(21, msg2.Uint16Values[0]); + Assert.Equal(22, msg2.Uint16Values[1]); + Assert.Equal(23, msg2.Uint16Values[2]); + + // int32Values + Assert.Equal(3, msg2.Int32Values.Count); + Assert.Equal(24, msg2.Int32Values[0]); + Assert.Equal(25, msg2.Int32Values[1]); + Assert.Equal(26, msg2.Int32Values[2]); + + // uint32Values + Assert.Equal(3, msg2.Uint32Values.Count); + Assert.Equal((uint)27, msg2.Uint32Values[0]); + Assert.Equal((uint)28, msg2.Uint32Values[1]); + Assert.Equal((uint)29, msg2.Uint32Values[2]); + + // int64Values + Assert.Equal(3, msg2.Int64Values.Count); + Assert.Equal(30, msg2.Int64Values[0]); + Assert.Equal(31, msg2.Int64Values[1]); + Assert.Equal(32, msg2.Int64Values[2]); + + // uint64Values + Assert.Equal(3, msg2.Uint64Values.Count); + Assert.Equal((ulong)33, msg2.Uint64Values[0]); + Assert.Equal((ulong)34, msg2.Uint64Values[1]); + Assert.Equal((ulong)35, msg2.Uint64Values[2]); + + // stringValues + Assert.Equal(3, msg2.StringValues.Count); + Assert.Equal("one", msg2.StringValues[0]); + Assert.Equal("two", msg2.StringValues[1]); + Assert.Equal("three", msg2.StringValues[2]); + + Assert.Equal(3, msg2.BasicTypesValues.Count); + Assert.True(msg2.BasicTypesValues[0].BoolValue); + Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); + Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); + Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); + Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); + Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); + Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); + Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); + Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); + Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); + Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); + Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); + Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); + + Assert.False(msg2.BasicTypesValues[1].BoolValue); + Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); + Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); + Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); + Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); + Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); + Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); + Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); + Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); + Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); + Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); + Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); + Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); + + Assert.True(msg2.BasicTypesValues[2].BoolValue); + Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); + Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); + Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); + Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); + Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); + Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); + Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); + Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); + Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); + Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); + Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); + Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); } - ); - - while(!received) - { - chatter_pub.Publish(msg); - - RCLdotnet.SpinOnce(node_array_1, 500); - RCLdotnet.SpinOnce(node_array_2, 500); - } - - // boolValues - Assert.Equal(3, msg2.BoolValues.Count); - Assert.True(msg2.BoolValues[0]); - Assert.False(msg2.BoolValues[1]); - Assert.True(msg2.BoolValues[2]); - - // byteValues - Assert.Equal(3, msg2.ByteValues.Count); - Assert.Equal(0, msg2.ByteValues[0]); - Assert.Equal(1, msg2.ByteValues[1]); - Assert.Equal(2, msg2.ByteValues[2]); - - // charValues - Assert.Equal(3, msg2.CharValues.Count); - Assert.Equal(3, msg2.CharValues[0]); - Assert.Equal(4, msg2.CharValues[1]); - Assert.Equal(5, msg2.CharValues[2]); - - // float32Values - Assert.Equal(3, msg2.Float32Values.Count); - Assert.Equal(6.1f, msg2.Float32Values[0]); - Assert.Equal(7.1f, msg2.Float32Values[1]); - Assert.Equal(8.1f, msg2.Float32Values[2]); - - // float64Values - Assert.Equal(3, msg2.Float64Values.Count); - Assert.Equal(9.1, msg2.Float64Values[0]); - Assert.Equal(10.1, msg2.Float64Values[1]); - Assert.Equal(11.1, msg2.Float64Values[2]); - - // int8Values - Assert.Equal(3, msg2.Int8Values.Count); - Assert.Equal(12, msg2.Int8Values[0]); - Assert.Equal(13, msg2.Int8Values[1]); - Assert.Equal(14, msg2.Int8Values[2]); - - // uint8Values - Assert.Equal(3, msg2.Uint8Values.Count); - Assert.Equal(15, msg2.Uint8Values[0]); - Assert.Equal(16, msg2.Uint8Values[1]); - Assert.Equal(17, msg2.Uint8Values[2]); - - // int16Values - Assert.Equal(3, msg2.Int16Values.Count); - Assert.Equal(18, msg2.Int16Values[0]); - Assert.Equal(19, msg2.Int16Values[1]); - Assert.Equal(20, msg2.Int16Values[2]); - - // uint16Values - Assert.Equal(3, msg2.Uint16Values.Count); - Assert.Equal(21, msg2.Uint16Values[0]); - Assert.Equal(22, msg2.Uint16Values[1]); - Assert.Equal(23, msg2.Uint16Values[2]); - - // int32Values - Assert.Equal(3, msg2.Int32Values.Count); - Assert.Equal(24, msg2.Int32Values[0]); - Assert.Equal(25, msg2.Int32Values[1]); - Assert.Equal(26, msg2.Int32Values[2]); - - // uint32Values - Assert.Equal(3, msg2.Uint32Values.Count); - Assert.Equal((uint)27, msg2.Uint32Values[0]); - Assert.Equal((uint)28, msg2.Uint32Values[1]); - Assert.Equal((uint)29, msg2.Uint32Values[2]); - - // int64Values - Assert.Equal(3, msg2.Int64Values.Count); - Assert.Equal(30, msg2.Int64Values[0]); - Assert.Equal(31, msg2.Int64Values[1]); - Assert.Equal(32, msg2.Int64Values[2]); - - // uint64Values - Assert.Equal(3, msg2.Uint64Values.Count); - Assert.Equal((ulong)33, msg2.Uint64Values[0]); - Assert.Equal((ulong)34, msg2.Uint64Values[1]); - Assert.Equal((ulong)35, msg2.Uint64Values[2]); - - // stringValues - Assert.Equal(3, msg2.StringValues.Count); - Assert.Equal("one", msg2.StringValues[0]); - Assert.Equal("two", msg2.StringValues[1]); - Assert.Equal("three", msg2.StringValues[2]); - - Assert.Equal(3, msg2.BasicTypesValues.Count); - Assert.True(msg2.BasicTypesValues[0].BoolValue); - Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); - Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); - Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); - Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); - Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); - Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); - Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); - Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); - Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); - Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); - Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); - Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); - - Assert.False(msg2.BasicTypesValues[1].BoolValue); - Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); - Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); - Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); - Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); - Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); - Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); - Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); - Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); - Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); - Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); - Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); - Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); - - Assert.True(msg2.BasicTypesValues[2].BoolValue); - Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); - Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); - Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); - Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); - Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); - Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); - Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); - Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); - Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); - Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); - Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); - Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); - } - [Fact] - public void TestPublishBoundedSequences() - { - RCLdotnet.Init(); - Node node_array_1 = RCLdotnet.CreateNode("test_bounded_sequences_1"); - Node node_array_2 = RCLdotnet.CreateNode("test_bounded_sequences_2"); - Publisher chatter_pub = node_array_1.CreatePublisher("topic_bounded_sequences"); - - test_msgs.msg.BoundedSequences msg = new test_msgs.msg.BoundedSequences(); - test_msgs.msg.BoundedSequences msg2 = new test_msgs.msg.BoundedSequences(); - - // boolValues - msg.BoolValues.Add(true); - msg.BoolValues.Add(false); - msg.BoolValues.Add(true); - - // byteValues - msg.ByteValues.Add(0); - msg.ByteValues.Add(1); - msg.ByteValues.Add(2); - - // charValues - msg.CharValues.Add(3); - msg.CharValues.Add(4); - msg.CharValues.Add(5); - - // float32Values - msg.Float32Values.Add(6.1f); - msg.Float32Values.Add(7.1f); - msg.Float32Values.Add(8.1f); - - // float64Values - msg.Float64Values.Add(9.1); - msg.Float64Values.Add(10.1); - msg.Float64Values.Add(11.1); - - // int8Values - msg.Int8Values.Add(12); - msg.Int8Values.Add(13); - msg.Int8Values.Add(14); - - // uint8Values - msg.Uint8Values.Add(15); - msg.Uint8Values.Add(16); - msg.Uint8Values.Add(17); - - // int16Values - msg.Int16Values.Add(18); - msg.Int16Values.Add(19); - msg.Int16Values.Add(20); - - // uint16Values - msg.Uint16Values.Add(21); - msg.Uint16Values.Add(22); - msg.Uint16Values.Add(23); - - // int32Values - msg.Int32Values.Add(24); - msg.Int32Values.Add(25); - msg.Int32Values.Add(26); - - // uint32Values - msg.Uint32Values.Add(27); - msg.Uint32Values.Add(28); - msg.Uint32Values.Add(29); - - // int64Values - msg.Int64Values.Add(30); - msg.Int64Values.Add(31); - msg.Int64Values.Add(32); - - // uint64Values - msg.Uint64Values.Add(33); - msg.Uint64Values.Add(34); - msg.Uint64Values.Add(35); - - // stringValues - msg.StringValues.Add("one"); - msg.StringValues.Add("two"); - msg.StringValues.Add("three"); - - test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.BoolValue = true; - basic_type_1.ByteValue = 36; - basic_type_1.CharValue = 37; - basic_type_1.Float32Value = 38.1f; - basic_type_1.Float64Value = 39.1; - basic_type_1.Int8Value = 40; - basic_type_1.Uint8Value = 41; - basic_type_1.Int16Value = 42; - basic_type_1.Uint16Value = 43; - basic_type_1.Int32Value = 44; - basic_type_1.Uint32Value = 45; - basic_type_1.Int64Value = 46; - basic_type_1.Uint64Value = 47; - - test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.BoolValue = false; - basic_type_2.ByteValue = 48; - basic_type_2.CharValue = 49; - basic_type_2.Float32Value = 50.1f; - basic_type_2.Float64Value = 51.1; - basic_type_2.Int8Value = 52; - basic_type_2.Uint8Value = 53; - basic_type_2.Int16Value = 54; - basic_type_2.Uint16Value = 55; - basic_type_2.Int32Value = 56; - basic_type_2.Uint32Value = 57; - basic_type_2.Int64Value = 58; - basic_type_2.Uint64Value = 59; - - test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.BoolValue = true; - basic_type_3.ByteValue = 60; - basic_type_3.CharValue = 61; - basic_type_3.Float32Value = 62.1f; - basic_type_3.Float64Value = 63.1; - basic_type_3.Int8Value = 64; - basic_type_3.Uint8Value = 65; - basic_type_3.Int16Value = 66; - basic_type_3.Uint16Value = 67; - basic_type_3.Int32Value = 68; - basic_type_3.Uint32Value = 69; - basic_type_3.Int64Value = 70; - basic_type_3.Uint64Value = 71; - - msg.BasicTypesValues.Add(basic_type_1); - msg.BasicTypesValues.Add(basic_type_2); - msg.BasicTypesValues.Add(basic_type_3); - - bool received=false; - Subscription chatter_sub = node_array_2.CreateSubscription( - "topic_bounded_sequences", rcv_msg => + [Fact] + public void TestPublishBoundedSequences() { - received=true; - msg2 = rcv_msg; + RCLdotnet.Init(); + Node node_array_1 = RCLdotnet.CreateNode("test_bounded_sequences_1"); + Node node_array_2 = RCLdotnet.CreateNode("test_bounded_sequences_2"); + Publisher chatter_pub = node_array_1.CreatePublisher("topic_bounded_sequences"); + + test_msgs.msg.BoundedSequences msg = new test_msgs.msg.BoundedSequences(); + test_msgs.msg.BoundedSequences msg2 = new test_msgs.msg.BoundedSequences(); + + // boolValues + msg.BoolValues.Add(true); + msg.BoolValues.Add(false); + msg.BoolValues.Add(true); + + // byteValues + msg.ByteValues.Add(0); + msg.ByteValues.Add(1); + msg.ByteValues.Add(2); + + // charValues + msg.CharValues.Add(3); + msg.CharValues.Add(4); + msg.CharValues.Add(5); + + // float32Values + msg.Float32Values.Add(6.1f); + msg.Float32Values.Add(7.1f); + msg.Float32Values.Add(8.1f); + + // float64Values + msg.Float64Values.Add(9.1); + msg.Float64Values.Add(10.1); + msg.Float64Values.Add(11.1); + + // int8Values + msg.Int8Values.Add(12); + msg.Int8Values.Add(13); + msg.Int8Values.Add(14); + + // uint8Values + msg.Uint8Values.Add(15); + msg.Uint8Values.Add(16); + msg.Uint8Values.Add(17); + + // int16Values + msg.Int16Values.Add(18); + msg.Int16Values.Add(19); + msg.Int16Values.Add(20); + + // uint16Values + msg.Uint16Values.Add(21); + msg.Uint16Values.Add(22); + msg.Uint16Values.Add(23); + + // int32Values + msg.Int32Values.Add(24); + msg.Int32Values.Add(25); + msg.Int32Values.Add(26); + + // uint32Values + msg.Uint32Values.Add(27); + msg.Uint32Values.Add(28); + msg.Uint32Values.Add(29); + + // int64Values + msg.Int64Values.Add(30); + msg.Int64Values.Add(31); + msg.Int64Values.Add(32); + + // uint64Values + msg.Uint64Values.Add(33); + msg.Uint64Values.Add(34); + msg.Uint64Values.Add(35); + + // stringValues + msg.StringValues.Add("one"); + msg.StringValues.Add("two"); + msg.StringValues.Add("three"); + + test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); + basic_type_1.BoolValue = true; + basic_type_1.ByteValue = 36; + basic_type_1.CharValue = 37; + basic_type_1.Float32Value = 38.1f; + basic_type_1.Float64Value = 39.1; + basic_type_1.Int8Value = 40; + basic_type_1.Uint8Value = 41; + basic_type_1.Int16Value = 42; + basic_type_1.Uint16Value = 43; + basic_type_1.Int32Value = 44; + basic_type_1.Uint32Value = 45; + basic_type_1.Int64Value = 46; + basic_type_1.Uint64Value = 47; + + test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); + basic_type_2.BoolValue = false; + basic_type_2.ByteValue = 48; + basic_type_2.CharValue = 49; + basic_type_2.Float32Value = 50.1f; + basic_type_2.Float64Value = 51.1; + basic_type_2.Int8Value = 52; + basic_type_2.Uint8Value = 53; + basic_type_2.Int16Value = 54; + basic_type_2.Uint16Value = 55; + basic_type_2.Int32Value = 56; + basic_type_2.Uint32Value = 57; + basic_type_2.Int64Value = 58; + basic_type_2.Uint64Value = 59; + + test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); + basic_type_3.BoolValue = true; + basic_type_3.ByteValue = 60; + basic_type_3.CharValue = 61; + basic_type_3.Float32Value = 62.1f; + basic_type_3.Float64Value = 63.1; + basic_type_3.Int8Value = 64; + basic_type_3.Uint8Value = 65; + basic_type_3.Int16Value = 66; + basic_type_3.Uint16Value = 67; + basic_type_3.Int32Value = 68; + basic_type_3.Uint32Value = 69; + basic_type_3.Int64Value = 70; + basic_type_3.Uint64Value = 71; + + msg.BasicTypesValues.Add(basic_type_1); + msg.BasicTypesValues.Add(basic_type_2); + msg.BasicTypesValues.Add(basic_type_3); + + bool received = false; + Subscription chatter_sub = node_array_2.CreateSubscription( + "topic_bounded_sequences", rcv_msg => + { + received = true; + msg2 = rcv_msg; + } + ); + + while (!received) + { + chatter_pub.Publish(msg); + + RCLdotnet.SpinOnce(node_array_1, 500); + RCLdotnet.SpinOnce(node_array_2, 500); + } + + // boolValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.BoolValuesMaxCount); + Assert.Equal(3, msg2.BoolValues.Count); + Assert.True(msg2.BoolValues[0]); + Assert.False(msg2.BoolValues[1]); + Assert.True(msg2.BoolValues[2]); + + // byteValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.ByteValuesMaxCount); + Assert.Equal(3, msg2.ByteValues.Count); + Assert.Equal(0, msg2.ByteValues[0]); + Assert.Equal(1, msg2.ByteValues[1]); + Assert.Equal(2, msg2.ByteValues[2]); + + // charValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.CharValuesMaxCount); + Assert.Equal(3, msg2.CharValues.Count); + Assert.Equal(3, msg2.CharValues[0]); + Assert.Equal(4, msg2.CharValues[1]); + Assert.Equal(5, msg2.CharValues[2]); + + // float32Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Float32ValuesMaxCount); + Assert.Equal(3, msg2.Float32Values.Count); + Assert.Equal(6.1f, msg2.Float32Values[0]); + Assert.Equal(7.1f, msg2.Float32Values[1]); + Assert.Equal(8.1f, msg2.Float32Values[2]); + + // float64Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Float64ValuesMaxCount); + Assert.Equal(3, msg2.Float64Values.Count); + Assert.Equal(9.1, msg2.Float64Values[0]); + Assert.Equal(10.1, msg2.Float64Values[1]); + Assert.Equal(11.1, msg2.Float64Values[2]); + + // int8Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int8ValuesMaxCount); + Assert.Equal(3, msg2.Int8Values.Count); + Assert.Equal(12, msg2.Int8Values[0]); + Assert.Equal(13, msg2.Int8Values[1]); + Assert.Equal(14, msg2.Int8Values[2]); + + // uint8Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint8ValuesMaxCount); + Assert.Equal(3, msg2.Uint8Values.Count); + Assert.Equal(15, msg2.Uint8Values[0]); + Assert.Equal(16, msg2.Uint8Values[1]); + Assert.Equal(17, msg2.Uint8Values[2]); + + // int16Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int16ValuesMaxCount); + Assert.Equal(3, msg2.Int16Values.Count); + Assert.Equal(18, msg2.Int16Values[0]); + Assert.Equal(19, msg2.Int16Values[1]); + Assert.Equal(20, msg2.Int16Values[2]); + + // uint16Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint16ValuesMaxCount); + Assert.Equal(3, msg2.Uint16Values.Count); + Assert.Equal(21, msg2.Uint16Values[0]); + Assert.Equal(22, msg2.Uint16Values[1]); + Assert.Equal(23, msg2.Uint16Values[2]); + + // int32Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int32ValuesMaxCount); + Assert.Equal(3, msg2.Int32Values.Count); + Assert.Equal(24, msg2.Int32Values[0]); + Assert.Equal(25, msg2.Int32Values[1]); + Assert.Equal(26, msg2.Int32Values[2]); + + // uint32Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint32ValuesMaxCount); + Assert.Equal(3, msg2.Uint32Values.Count); + Assert.Equal((uint)27, msg2.Uint32Values[0]); + Assert.Equal((uint)28, msg2.Uint32Values[1]); + Assert.Equal((uint)29, msg2.Uint32Values[2]); + + // int64Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Int64ValuesMaxCount); + Assert.Equal(3, msg2.Int64Values.Count); + Assert.Equal(30, msg2.Int64Values[0]); + Assert.Equal(31, msg2.Int64Values[1]); + Assert.Equal(32, msg2.Int64Values[2]); + + // uint64Values + Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint64ValuesMaxCount); + Assert.Equal(3, msg2.Uint64Values.Count); + Assert.Equal((ulong)33, msg2.Uint64Values[0]); + Assert.Equal((ulong)34, msg2.Uint64Values[1]); + Assert.Equal((ulong)35, msg2.Uint64Values[2]); + + // stringValues + Assert.Equal(3, test_msgs.msg.BoundedSequences.StringValuesMaxCount); + Assert.Equal(3, msg2.StringValues.Count); + Assert.Equal("one", msg2.StringValues[0]); + Assert.Equal("two", msg2.StringValues[1]); + Assert.Equal("three", msg2.StringValues[2]); + + Assert.Equal(3, test_msgs.msg.BoundedSequences.BasicTypesValuesMaxCount); + Assert.Equal(3, msg2.BasicTypesValues.Count); + Assert.True(msg2.BasicTypesValues[0].BoolValue); + Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); + Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); + Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); + Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); + Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); + Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); + Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); + Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); + Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); + Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); + Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); + Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); + + Assert.False(msg2.BasicTypesValues[1].BoolValue); + Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); + Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); + Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); + Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); + Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); + Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); + Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); + Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); + Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); + Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); + Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); + Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); + + Assert.True(msg2.BasicTypesValues[2].BoolValue); + Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); + Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); + Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); + Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); + Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); + Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); + Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); + Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); + Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); + Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); + Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); + Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); } - ); - - while(!received) - { - chatter_pub.Publish(msg); - - RCLdotnet.SpinOnce(node_array_1, 500); - RCLdotnet.SpinOnce(node_array_2, 500); - } - - // boolValues - Assert.Equal(3, test_msgs.msg.BoundedSequences.BoolValuesMaxCount); - Assert.Equal(3, msg2.BoolValues.Count); - Assert.True(msg2.BoolValues[0]); - Assert.False(msg2.BoolValues[1]); - Assert.True(msg2.BoolValues[2]); - - // byteValues - Assert.Equal(3, test_msgs.msg.BoundedSequences.ByteValuesMaxCount); - Assert.Equal(3, msg2.ByteValues.Count); - Assert.Equal(0, msg2.ByteValues[0]); - Assert.Equal(1, msg2.ByteValues[1]); - Assert.Equal(2, msg2.ByteValues[2]); - - // charValues - Assert.Equal(3, test_msgs.msg.BoundedSequences.CharValuesMaxCount); - Assert.Equal(3, msg2.CharValues.Count); - Assert.Equal(3, msg2.CharValues[0]); - Assert.Equal(4, msg2.CharValues[1]); - Assert.Equal(5, msg2.CharValues[2]); - - // float32Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Float32ValuesMaxCount); - Assert.Equal(3, msg2.Float32Values.Count); - Assert.Equal(6.1f, msg2.Float32Values[0]); - Assert.Equal(7.1f, msg2.Float32Values[1]); - Assert.Equal(8.1f, msg2.Float32Values[2]); - - // float64Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Float64ValuesMaxCount); - Assert.Equal(3, msg2.Float64Values.Count); - Assert.Equal(9.1, msg2.Float64Values[0]); - Assert.Equal(10.1, msg2.Float64Values[1]); - Assert.Equal(11.1, msg2.Float64Values[2]); - - // int8Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int8ValuesMaxCount); - Assert.Equal(3, msg2.Int8Values.Count); - Assert.Equal(12, msg2.Int8Values[0]); - Assert.Equal(13, msg2.Int8Values[1]); - Assert.Equal(14, msg2.Int8Values[2]); - - // uint8Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint8ValuesMaxCount); - Assert.Equal(3, msg2.Uint8Values.Count); - Assert.Equal(15, msg2.Uint8Values[0]); - Assert.Equal(16, msg2.Uint8Values[1]); - Assert.Equal(17, msg2.Uint8Values[2]); - - // int16Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int16ValuesMaxCount); - Assert.Equal(3, msg2.Int16Values.Count); - Assert.Equal(18, msg2.Int16Values[0]); - Assert.Equal(19, msg2.Int16Values[1]); - Assert.Equal(20, msg2.Int16Values[2]); - - // uint16Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint16ValuesMaxCount); - Assert.Equal(3, msg2.Uint16Values.Count); - Assert.Equal(21, msg2.Uint16Values[0]); - Assert.Equal(22, msg2.Uint16Values[1]); - Assert.Equal(23, msg2.Uint16Values[2]); - - // int32Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int32ValuesMaxCount); - Assert.Equal(3, msg2.Int32Values.Count); - Assert.Equal(24, msg2.Int32Values[0]); - Assert.Equal(25, msg2.Int32Values[1]); - Assert.Equal(26, msg2.Int32Values[2]); - - // uint32Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint32ValuesMaxCount); - Assert.Equal(3, msg2.Uint32Values.Count); - Assert.Equal((uint)27, msg2.Uint32Values[0]); - Assert.Equal((uint)28, msg2.Uint32Values[1]); - Assert.Equal((uint)29, msg2.Uint32Values[2]); - - // int64Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Int64ValuesMaxCount); - Assert.Equal(3, msg2.Int64Values.Count); - Assert.Equal(30, msg2.Int64Values[0]); - Assert.Equal(31, msg2.Int64Values[1]); - Assert.Equal(32, msg2.Int64Values[2]); - - // uint64Values - Assert.Equal(3, test_msgs.msg.BoundedSequences.Uint64ValuesMaxCount); - Assert.Equal(3, msg2.Uint64Values.Count); - Assert.Equal((ulong)33, msg2.Uint64Values[0]); - Assert.Equal((ulong)34, msg2.Uint64Values[1]); - Assert.Equal((ulong)35, msg2.Uint64Values[2]); - - // stringValues - Assert.Equal(3, test_msgs.msg.BoundedSequences.StringValuesMaxCount); - Assert.Equal(3, msg2.StringValues.Count); - Assert.Equal("one", msg2.StringValues[0]); - Assert.Equal("two", msg2.StringValues[1]); - Assert.Equal("three", msg2.StringValues[2]); - - Assert.Equal(3, test_msgs.msg.BoundedSequences.BasicTypesValuesMaxCount); - Assert.Equal(3, msg2.BasicTypesValues.Count); - Assert.True(msg2.BasicTypesValues[0].BoolValue); - Assert.Equal(36, msg2.BasicTypesValues[0].ByteValue); - Assert.Equal(37, msg2.BasicTypesValues[0].CharValue); - Assert.Equal(38.1f, msg2.BasicTypesValues[0].Float32Value); - Assert.Equal(39.1, msg2.BasicTypesValues[0].Float64Value); - Assert.Equal(40, msg2.BasicTypesValues[0].Int8Value); - Assert.Equal(41, msg2.BasicTypesValues[0].Uint8Value); - Assert.Equal(42, msg2.BasicTypesValues[0].Int16Value); - Assert.Equal(43, msg2.BasicTypesValues[0].Uint16Value); - Assert.Equal(44, msg2.BasicTypesValues[0].Int32Value); - Assert.Equal((uint)45, msg2.BasicTypesValues[0].Uint32Value); - Assert.Equal(46, msg2.BasicTypesValues[0].Int64Value); - Assert.Equal((ulong)47, msg2.BasicTypesValues[0].Uint64Value); - - Assert.False(msg2.BasicTypesValues[1].BoolValue); - Assert.Equal(48, msg2.BasicTypesValues[1].ByteValue); - Assert.Equal(49, msg2.BasicTypesValues[1].CharValue); - Assert.Equal(50.1f, msg2.BasicTypesValues[1].Float32Value); - Assert.Equal(51.1, msg2.BasicTypesValues[1].Float64Value); - Assert.Equal(52, msg2.BasicTypesValues[1].Int8Value); - Assert.Equal(53, msg2.BasicTypesValues[1].Uint8Value); - Assert.Equal(54, msg2.BasicTypesValues[1].Int16Value); - Assert.Equal(55, msg2.BasicTypesValues[1].Uint16Value); - Assert.Equal(56, msg2.BasicTypesValues[1].Int32Value); - Assert.Equal((uint)57, msg2.BasicTypesValues[1].Uint32Value); - Assert.Equal(58, msg2.BasicTypesValues[1].Int64Value); - Assert.Equal((ulong)59, msg2.BasicTypesValues[1].Uint64Value); - - Assert.True(msg2.BasicTypesValues[2].BoolValue); - Assert.Equal(60, msg2.BasicTypesValues[2].ByteValue); - Assert.Equal(61, msg2.BasicTypesValues[2].CharValue); - Assert.Equal(62.1f, msg2.BasicTypesValues[2].Float32Value); - Assert.Equal(63.1, msg2.BasicTypesValues[2].Float64Value); - Assert.Equal(64, msg2.BasicTypesValues[2].Int8Value); - Assert.Equal(65, msg2.BasicTypesValues[2].Uint8Value); - Assert.Equal(66, msg2.BasicTypesValues[2].Int16Value); - Assert.Equal(67, msg2.BasicTypesValues[2].Uint16Value); - Assert.Equal(68, msg2.BasicTypesValues[2].Int32Value); - Assert.Equal((uint)69, msg2.BasicTypesValues[2].Uint32Value); - Assert.Equal(70, msg2.BasicTypesValues[2].Int64Value); - Assert.Equal((ulong)71, msg2.BasicTypesValues[2].Uint64Value); - } - [Fact] - public void TestPublishBoundedSequencesSizeCheck() - { - RCLdotnet.Init(); - var nodeBoundedSequencesSizeCheck = RCLdotnet.CreateNode("test_bounded_sequences_size_check"); - var chatterPub = nodeBoundedSequencesSizeCheck.CreatePublisher("topic_bounded_sequences_size_check"); - - var msg = new test_msgs.msg.BoundedSequences(); - msg.StringValues = new List - { - "0", - "1", - "2", - "3", - }; - - var exception = Assert.Throws(() => chatterPub.Publish(msg)); - Assert.Equal("Invalid size of bounded sequence 'StringValues'.", exception.Message); - } + [Fact] + public void TestPublishBoundedSequencesSizeCheck() + { + RCLdotnet.Init(); + var nodeBoundedSequencesSizeCheck = RCLdotnet.CreateNode("test_bounded_sequences_size_check"); + var chatterPub = nodeBoundedSequencesSizeCheck.CreatePublisher("topic_bounded_sequences_size_check"); + + var msg = new test_msgs.msg.BoundedSequences(); + msg.StringValues = new List + { + "0", + "1", + "2", + "3", + }; + + var exception = Assert.Throws(() => chatterPub.Publish(msg)); + Assert.Equal("Invalid size of bounded sequence 'StringValues'.", exception.Message); + } - [Fact] - public void TestDefaults() - { - var defaultsMsg = new test_msgs.msg.Defaults(); - - Assert.Equal(true, defaultsMsg.BoolValue); - Assert.Equal(50, defaultsMsg.ByteValue); - Assert.Equal(100, defaultsMsg.CharValue); - Assert.Equal(1.125f, defaultsMsg.Float32Value); - Assert.Equal(1.125, defaultsMsg.Float64Value); - Assert.Equal(-50, defaultsMsg.Int8Value); - Assert.Equal(200, defaultsMsg.Uint8Value); - Assert.Equal(-1000, defaultsMsg.Int16Value); - Assert.Equal(2000, defaultsMsg.Uint16Value); - Assert.Equal(-30000, defaultsMsg.Int32Value); - Assert.True(60000 == defaultsMsg.Uint32Value); - Assert.Equal(-40000000, defaultsMsg.Int64Value); - Assert.True(50000000 == defaultsMsg.Uint64Value); - - var stringsMsg = new test_msgs.msg.Strings(); - Assert.Equal("", stringsMsg.StringValue); - Assert.Equal("Hello world!", stringsMsg.StringValueDefault1); - Assert.Equal("Hello'world!", stringsMsg.StringValueDefault2); - Assert.Equal("Hello\"world!", stringsMsg.StringValueDefault3); - Assert.Equal("Hello'world!", stringsMsg.StringValueDefault4); - Assert.Equal("Hello\"world!", stringsMsg.StringValueDefault5); - } + [Fact] + public void TestDefaults() + { + var defaultsMsg = new test_msgs.msg.Defaults(); + + Assert.Equal(true, defaultsMsg.BoolValue); + Assert.Equal(50, defaultsMsg.ByteValue); + Assert.Equal(100, defaultsMsg.CharValue); + Assert.Equal(1.125f, defaultsMsg.Float32Value); + Assert.Equal(1.125, defaultsMsg.Float64Value); + Assert.Equal(-50, defaultsMsg.Int8Value); + Assert.Equal(200, defaultsMsg.Uint8Value); + Assert.Equal(-1000, defaultsMsg.Int16Value); + Assert.Equal(2000, defaultsMsg.Uint16Value); + Assert.Equal(-30000, defaultsMsg.Int32Value); + Assert.True(60000 == defaultsMsg.Uint32Value); + Assert.Equal(-40000000, defaultsMsg.Int64Value); + Assert.True(50000000 == defaultsMsg.Uint64Value); + + var stringsMsg = new test_msgs.msg.Strings(); + Assert.Equal("", stringsMsg.StringValue); + Assert.Equal("Hello world!", stringsMsg.StringValueDefault1); + Assert.Equal("Hello'world!", stringsMsg.StringValueDefault2); + Assert.Equal("Hello\"world!", stringsMsg.StringValueDefault3); + Assert.Equal("Hello'world!", stringsMsg.StringValueDefault4); + Assert.Equal("Hello\"world!", stringsMsg.StringValueDefault5); + } - [Fact] - public void TestDefaultsArrays() - { - var msg = new test_msgs.msg.Arrays(); + [Fact] + public void TestDefaultsArrays() + { + var msg = new test_msgs.msg.Arrays(); - Assert.IsType(msg.BoolValues); - Assert.Equal(new bool[3], msg.BoolValues); + Assert.IsType(msg.BoolValues); + Assert.Equal(new bool[3], msg.BoolValues); - Assert.IsType(msg.ByteValues); - Assert.Equal(new byte[3], msg.ByteValues); - - Assert.IsType(msg.CharValues); - Assert.Equal(new byte[3], msg.CharValues); + Assert.IsType(msg.ByteValues); + Assert.Equal(new byte[3], msg.ByteValues); + + Assert.IsType(msg.CharValues); + Assert.Equal(new byte[3], msg.CharValues); - Assert.IsType(msg.Float32Values); - Assert.Equal(new float[3], msg.Float32Values); + Assert.IsType(msg.Float32Values); + Assert.Equal(new float[3], msg.Float32Values); - Assert.IsType(msg.Float64Values); - Assert.Equal(new double[3], msg.Float64Values); - - Assert.IsType(msg.Int8Values); - Assert.Equal(new sbyte[3], msg.Int8Values); - - Assert.IsType(msg.Uint8Values); - Assert.Equal(new byte[3], msg.Uint8Values); - - Assert.IsType(msg.Int16Values); - Assert.Equal(new short[3], msg.Int16Values); - - Assert.IsType(msg.Uint16Values); - Assert.Equal(new ushort[3], msg.Uint16Values); - - Assert.IsType(msg.Int32Values); - Assert.Equal(new int[3], msg.Int32Values); - - Assert.IsType(msg.Uint32Values); - Assert.Equal(new uint[3], msg.Uint32Values); - - Assert.IsType(msg.Int64Values); - Assert.Equal(new long[3], msg.Int64Values); - - Assert.IsType(msg.Uint64Values); - Assert.Equal(new ulong[3], msg.Uint64Values); - - Assert.IsType(msg.StringValues); - Assert.Equal( - Enumerable.Repeat("", 3).ToArray(), - msg.StringValues); - - Assert.IsType(msg.BasicTypesValues); - Assert.Equal(3, msg.BasicTypesValues.Length); - Assert.NotNull(msg.BasicTypesValues[0]); - Assert.NotNull(msg.BasicTypesValues[1]); - Assert.NotNull(msg.BasicTypesValues[2]); - - Assert.IsType(msg.ConstantsValues); - Assert.Equal(3, msg.ConstantsValues.Length); - Assert.NotNull(msg.ConstantsValues[0]); - Assert.NotNull(msg.ConstantsValues[1]); - Assert.NotNull(msg.ConstantsValues[2]); - - Assert.IsType(msg.DefaultsValues); - Assert.Equal(3, msg.DefaultsValues.Length); - Assert.NotNull(msg.DefaultsValues[0]); - Assert.NotNull(msg.DefaultsValues[1]); - Assert.NotNull(msg.DefaultsValues[2]); - - Assert.IsType(msg.BoolValuesDefault); - Assert.Equal( - new bool[] { false, true, false }, - msg.BoolValuesDefault); - - Assert.IsType(msg.ByteValuesDefault); - Assert.Equal( - new byte[] { 0, 1, 255 }, - msg.ByteValuesDefault); - - Assert.IsType(msg.CharValuesDefault); - Assert.Equal( - new byte[] { 0, 1, 127 }, - msg.CharValuesDefault); - - Assert.IsType(msg.Float32ValuesDefault); - Assert.Equal( - new float[] { 1.125f, 0.0f, -1.125f}, - msg.Float32ValuesDefault); - - Assert.IsType(msg.Float64ValuesDefault); - Assert.Equal( - new double[] { 3.1415, 0.0, -3.1415 }, - msg.Float64ValuesDefault); - - Assert.IsType(msg.Int8ValuesDefault); - Assert.Equal( - new sbyte[] { 0, 127, -128 }, - msg.Int8ValuesDefault); - - Assert.IsType(msg.Uint8ValuesDefault); - Assert.Equal( - new byte[] { 0, 1, 255 }, - msg.Uint8ValuesDefault); - - Assert.IsType(msg.Int16ValuesDefault); - Assert.Equal( - new short[] { 0, 32767, -32768 }, - msg.Int16ValuesDefault); - - Assert.IsType(msg.Uint16ValuesDefault); - Assert.Equal( - new ushort[] { 0, 1, 65535 }, - msg.Uint16ValuesDefault); - - Assert.IsType(msg.Int32ValuesDefault); - Assert.Equal( - new int[] { 0, 2147483647, -2147483648 }, - msg.Int32ValuesDefault); - - Assert.IsType(msg.Uint32ValuesDefault); - Assert.Equal( - new uint[] { 0, 1, 4294967295 }, - msg.Uint32ValuesDefault); - - Assert.IsType(msg.Int64ValuesDefault); - Assert.Equal( - new long[] { 0, 9223372036854775807, -9223372036854775808 }, - msg.Int64ValuesDefault); - - Assert.IsType(msg.Uint64ValuesDefault); - Assert.Equal( - new ulong[] { 0, 1, 18446744073709551615 }, - msg.Uint64ValuesDefault); - - Assert.IsType(msg.StringValuesDefault); - Assert.Equal( - new string[] { "", "max value", "min value" }, - msg.StringValuesDefault); - } + Assert.IsType(msg.Float64Values); + Assert.Equal(new double[3], msg.Float64Values); + + Assert.IsType(msg.Int8Values); + Assert.Equal(new sbyte[3], msg.Int8Values); + + Assert.IsType(msg.Uint8Values); + Assert.Equal(new byte[3], msg.Uint8Values); + + Assert.IsType(msg.Int16Values); + Assert.Equal(new short[3], msg.Int16Values); + + Assert.IsType(msg.Uint16Values); + Assert.Equal(new ushort[3], msg.Uint16Values); + + Assert.IsType(msg.Int32Values); + Assert.Equal(new int[3], msg.Int32Values); + + Assert.IsType(msg.Uint32Values); + Assert.Equal(new uint[3], msg.Uint32Values); + + Assert.IsType(msg.Int64Values); + Assert.Equal(new long[3], msg.Int64Values); + + Assert.IsType(msg.Uint64Values); + Assert.Equal(new ulong[3], msg.Uint64Values); + + Assert.IsType(msg.StringValues); + Assert.Equal( + Enumerable.Repeat("", 3).ToArray(), + msg.StringValues); + + Assert.IsType(msg.BasicTypesValues); + Assert.Equal(3, msg.BasicTypesValues.Length); + Assert.NotNull(msg.BasicTypesValues[0]); + Assert.NotNull(msg.BasicTypesValues[1]); + Assert.NotNull(msg.BasicTypesValues[2]); + + Assert.IsType(msg.ConstantsValues); + Assert.Equal(3, msg.ConstantsValues.Length); + Assert.NotNull(msg.ConstantsValues[0]); + Assert.NotNull(msg.ConstantsValues[1]); + Assert.NotNull(msg.ConstantsValues[2]); + + Assert.IsType(msg.DefaultsValues); + Assert.Equal(3, msg.DefaultsValues.Length); + Assert.NotNull(msg.DefaultsValues[0]); + Assert.NotNull(msg.DefaultsValues[1]); + Assert.NotNull(msg.DefaultsValues[2]); + + Assert.IsType(msg.BoolValuesDefault); + Assert.Equal( + new bool[] { false, true, false }, + msg.BoolValuesDefault); + + Assert.IsType(msg.ByteValuesDefault); + Assert.Equal( + new byte[] { 0, 1, 255 }, + msg.ByteValuesDefault); + + Assert.IsType(msg.CharValuesDefault); + Assert.Equal( + new byte[] { 0, 1, 127 }, + msg.CharValuesDefault); + + Assert.IsType(msg.Float32ValuesDefault); + Assert.Equal( + new float[] { 1.125f, 0.0f, -1.125f }, + msg.Float32ValuesDefault); + + Assert.IsType(msg.Float64ValuesDefault); + Assert.Equal( + new double[] { 3.1415, 0.0, -3.1415 }, + msg.Float64ValuesDefault); + + Assert.IsType(msg.Int8ValuesDefault); + Assert.Equal( + new sbyte[] { 0, 127, -128 }, + msg.Int8ValuesDefault); + + Assert.IsType(msg.Uint8ValuesDefault); + Assert.Equal( + new byte[] { 0, 1, 255 }, + msg.Uint8ValuesDefault); + + Assert.IsType(msg.Int16ValuesDefault); + Assert.Equal( + new short[] { 0, 32767, -32768 }, + msg.Int16ValuesDefault); + + Assert.IsType(msg.Uint16ValuesDefault); + Assert.Equal( + new ushort[] { 0, 1, 65535 }, + msg.Uint16ValuesDefault); + + Assert.IsType(msg.Int32ValuesDefault); + Assert.Equal( + new int[] { 0, 2147483647, -2147483648 }, + msg.Int32ValuesDefault); + + Assert.IsType(msg.Uint32ValuesDefault); + Assert.Equal( + new uint[] { 0, 1, 4294967295 }, + msg.Uint32ValuesDefault); + + Assert.IsType(msg.Int64ValuesDefault); + Assert.Equal( + new long[] { 0, 9223372036854775807, -9223372036854775808 }, + msg.Int64ValuesDefault); + + Assert.IsType(msg.Uint64ValuesDefault); + Assert.Equal( + new ulong[] { 0, 1, 18446744073709551615 }, + msg.Uint64ValuesDefault); + + Assert.IsType(msg.StringValuesDefault); + Assert.Equal( + new string[] { "", "max value", "min value" }, + msg.StringValuesDefault); + } - [Fact] - public void TestDefaultsUnboundedSequences() - { - var msg = new test_msgs.msg.UnboundedSequences(); + [Fact] + public void TestDefaultsUnboundedSequences() + { + var msg = new test_msgs.msg.UnboundedSequences(); - Assert.IsType>(msg.BoolValues); - Assert.Equal(new List(), msg.BoolValues); + Assert.IsType>(msg.BoolValues); + Assert.Equal(new List(), msg.BoolValues); - Assert.IsType>(msg.ByteValues); - Assert.Equal(new List(), msg.ByteValues); + Assert.IsType>(msg.ByteValues); + Assert.Equal(new List(), msg.ByteValues); - Assert.IsType>(msg.CharValues); - Assert.Equal(new List(), msg.CharValues); + Assert.IsType>(msg.CharValues); + Assert.Equal(new List(), msg.CharValues); - Assert.IsType>(msg.Float32Values); - Assert.Equal(new List(), msg.Float32Values); + Assert.IsType>(msg.Float32Values); + Assert.Equal(new List(), msg.Float32Values); - Assert.IsType>(msg.Float64Values); - Assert.Equal(new List(), msg.Float64Values); + Assert.IsType>(msg.Float64Values); + Assert.Equal(new List(), msg.Float64Values); - Assert.IsType>(msg.Int8Values); - Assert.Equal(new List(), msg.Int8Values); + Assert.IsType>(msg.Int8Values); + Assert.Equal(new List(), msg.Int8Values); - Assert.IsType>(msg.Uint8Values); - Assert.Equal(new List(), msg.Uint8Values); + Assert.IsType>(msg.Uint8Values); + Assert.Equal(new List(), msg.Uint8Values); - Assert.IsType>(msg.Int16Values); - Assert.Equal(new List(), msg.Int16Values); + Assert.IsType>(msg.Int16Values); + Assert.Equal(new List(), msg.Int16Values); - Assert.IsType>(msg.Uint16Values); - Assert.Equal(new List(), msg.Uint16Values); + Assert.IsType>(msg.Uint16Values); + Assert.Equal(new List(), msg.Uint16Values); - Assert.IsType>(msg.Int32Values); - Assert.Equal(new List(), msg.Int32Values); + Assert.IsType>(msg.Int32Values); + Assert.Equal(new List(), msg.Int32Values); - Assert.IsType>(msg.Uint32Values); - Assert.Equal(new List(), msg.Uint32Values); + Assert.IsType>(msg.Uint32Values); + Assert.Equal(new List(), msg.Uint32Values); - Assert.IsType>(msg.Int64Values); - Assert.Equal(new List(), msg.Int64Values); + Assert.IsType>(msg.Int64Values); + Assert.Equal(new List(), msg.Int64Values); - Assert.IsType>(msg.Uint64Values); - Assert.Equal(new List(), msg.Uint64Values); + Assert.IsType>(msg.Uint64Values); + Assert.Equal(new List(), msg.Uint64Values); - Assert.IsType>(msg.StringValues); - Assert.Equal(new List(), msg.StringValues); + Assert.IsType>(msg.StringValues); + Assert.Equal(new List(), msg.StringValues); - Assert.IsType>(msg.BasicTypesValues); - Assert.Equal(new List(), msg.BasicTypesValues); + Assert.IsType>(msg.BasicTypesValues); + Assert.Equal(new List(), msg.BasicTypesValues); - Assert.IsType>(msg.ConstantsValues); - Assert.Equal(new List(), msg.ConstantsValues); + Assert.IsType>(msg.ConstantsValues); + Assert.Equal(new List(), msg.ConstantsValues); - Assert.IsType>(msg.DefaultsValues); - Assert.Equal(new List(), msg.DefaultsValues); + Assert.IsType>(msg.DefaultsValues); + Assert.Equal(new List(), msg.DefaultsValues); - Assert.IsType>(msg.BoolValuesDefault); - Assert.Equal( - new List { false, true, false }, - msg.BoolValuesDefault); + Assert.IsType>(msg.BoolValuesDefault); + Assert.Equal( + new List { false, true, false }, + msg.BoolValuesDefault); - Assert.IsType>(msg.ByteValuesDefault); - Assert.Equal( - new List { 0, 1, 255 }, - msg.ByteValuesDefault); + Assert.IsType>(msg.ByteValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.ByteValuesDefault); - Assert.IsType>(msg.CharValuesDefault); - Assert.Equal( - new List { 0, 1, 127 }, - msg.CharValuesDefault); + Assert.IsType>(msg.CharValuesDefault); + Assert.Equal( + new List { 0, 1, 127 }, + msg.CharValuesDefault); - Assert.IsType>(msg.Float32ValuesDefault); - Assert.Equal( - new List { 1.125f, 0.0f, -1.125f}, - msg.Float32ValuesDefault); + Assert.IsType>(msg.Float32ValuesDefault); + Assert.Equal( + new List { 1.125f, 0.0f, -1.125f }, + msg.Float32ValuesDefault); - Assert.IsType>(msg.Float64ValuesDefault); - Assert.Equal( - new List { 3.1415, 0.0, -3.1415 }, - msg.Float64ValuesDefault); + Assert.IsType>(msg.Float64ValuesDefault); + Assert.Equal( + new List { 3.1415, 0.0, -3.1415 }, + msg.Float64ValuesDefault); - Assert.IsType>(msg.Int8ValuesDefault); - Assert.Equal( - new List { 0, 127, -128 }, - msg.Int8ValuesDefault); + Assert.IsType>(msg.Int8ValuesDefault); + Assert.Equal( + new List { 0, 127, -128 }, + msg.Int8ValuesDefault); - Assert.IsType>(msg.Uint8ValuesDefault); - Assert.Equal( - new List { 0, 1, 255 }, - msg.Uint8ValuesDefault); + Assert.IsType>(msg.Uint8ValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.Uint8ValuesDefault); - Assert.IsType>(msg.Int16ValuesDefault); - Assert.Equal( - new List { 0, 32767, -32768 }, - msg.Int16ValuesDefault); + Assert.IsType>(msg.Int16ValuesDefault); + Assert.Equal( + new List { 0, 32767, -32768 }, + msg.Int16ValuesDefault); - Assert.IsType>(msg.Uint16ValuesDefault); - Assert.Equal( - new List { 0, 1, 65535 }, - msg.Uint16ValuesDefault); + Assert.IsType>(msg.Uint16ValuesDefault); + Assert.Equal( + new List { 0, 1, 65535 }, + msg.Uint16ValuesDefault); - Assert.IsType>(msg.Int32ValuesDefault); - Assert.Equal( - new List { 0, 2147483647, -2147483648 }, - msg.Int32ValuesDefault); + Assert.IsType>(msg.Int32ValuesDefault); + Assert.Equal( + new List { 0, 2147483647, -2147483648 }, + msg.Int32ValuesDefault); - Assert.IsType>(msg.Uint32ValuesDefault); - Assert.Equal( - new List { 0, 1, 4294967295 }, - msg.Uint32ValuesDefault); + Assert.IsType>(msg.Uint32ValuesDefault); + Assert.Equal( + new List { 0, 1, 4294967295 }, + msg.Uint32ValuesDefault); - Assert.IsType>(msg.Int64ValuesDefault); - Assert.Equal( - new List { 0, 9223372036854775807, -9223372036854775808 }, - msg.Int64ValuesDefault); + Assert.IsType>(msg.Int64ValuesDefault); + Assert.Equal( + new List { 0, 9223372036854775807, -9223372036854775808 }, + msg.Int64ValuesDefault); - Assert.IsType>(msg.Uint64ValuesDefault); - Assert.Equal( - new List { 0, 1, 18446744073709551615 }, - msg.Uint64ValuesDefault); - - Assert.IsType>(msg.StringValuesDefault); - Assert.Equal( - new List { "", "max value", "min value" }, - msg.StringValuesDefault); - } + Assert.IsType>(msg.Uint64ValuesDefault); + Assert.Equal( + new List { 0, 1, 18446744073709551615 }, + msg.Uint64ValuesDefault); + + Assert.IsType>(msg.StringValuesDefault); + Assert.Equal( + new List { "", "max value", "min value" }, + msg.StringValuesDefault); + } - [Fact] - public void TestDefaultsBoundedSequences() - { - var msg = new test_msgs.msg.BoundedSequences(); + [Fact] + public void TestDefaultsBoundedSequences() + { + var msg = new test_msgs.msg.BoundedSequences(); - Assert.IsType>(msg.BoolValues); - Assert.Equal(new List(), msg.BoolValues); + Assert.IsType>(msg.BoolValues); + Assert.Equal(new List(), msg.BoolValues); - Assert.IsType>(msg.ByteValues); - Assert.Equal(new List(), msg.ByteValues); + Assert.IsType>(msg.ByteValues); + Assert.Equal(new List(), msg.ByteValues); - Assert.IsType>(msg.CharValues); - Assert.Equal(new List(), msg.CharValues); + Assert.IsType>(msg.CharValues); + Assert.Equal(new List(), msg.CharValues); - Assert.IsType>(msg.Float32Values); - Assert.Equal(new List(), msg.Float32Values); + Assert.IsType>(msg.Float32Values); + Assert.Equal(new List(), msg.Float32Values); - Assert.IsType>(msg.Float64Values); - Assert.Equal(new List(), msg.Float64Values); + Assert.IsType>(msg.Float64Values); + Assert.Equal(new List(), msg.Float64Values); - Assert.IsType>(msg.Int8Values); - Assert.Equal(new List(), msg.Int8Values); + Assert.IsType>(msg.Int8Values); + Assert.Equal(new List(), msg.Int8Values); - Assert.IsType>(msg.Uint8Values); - Assert.Equal(new List(), msg.Uint8Values); + Assert.IsType>(msg.Uint8Values); + Assert.Equal(new List(), msg.Uint8Values); - Assert.IsType>(msg.Int16Values); - Assert.Equal(new List(), msg.Int16Values); + Assert.IsType>(msg.Int16Values); + Assert.Equal(new List(), msg.Int16Values); - Assert.IsType>(msg.Uint16Values); - Assert.Equal(new List(), msg.Uint16Values); + Assert.IsType>(msg.Uint16Values); + Assert.Equal(new List(), msg.Uint16Values); - Assert.IsType>(msg.Int32Values); - Assert.Equal(new List(), msg.Int32Values); + Assert.IsType>(msg.Int32Values); + Assert.Equal(new List(), msg.Int32Values); - Assert.IsType>(msg.Uint32Values); - Assert.Equal(new List(), msg.Uint32Values); + Assert.IsType>(msg.Uint32Values); + Assert.Equal(new List(), msg.Uint32Values); - Assert.IsType>(msg.Int64Values); - Assert.Equal(new List(), msg.Int64Values); + Assert.IsType>(msg.Int64Values); + Assert.Equal(new List(), msg.Int64Values); - Assert.IsType>(msg.Uint64Values); - Assert.Equal(new List(), msg.Uint64Values); + Assert.IsType>(msg.Uint64Values); + Assert.Equal(new List(), msg.Uint64Values); - Assert.IsType>(msg.StringValues); - Assert.Equal(new List(), msg.StringValues); + Assert.IsType>(msg.StringValues); + Assert.Equal(new List(), msg.StringValues); - Assert.IsType>(msg.BasicTypesValues); - Assert.Equal(new List(), msg.BasicTypesValues); + Assert.IsType>(msg.BasicTypesValues); + Assert.Equal(new List(), msg.BasicTypesValues); - Assert.IsType>(msg.ConstantsValues); - Assert.Equal(new List(), msg.ConstantsValues); + Assert.IsType>(msg.ConstantsValues); + Assert.Equal(new List(), msg.ConstantsValues); - Assert.IsType>(msg.DefaultsValues); - Assert.Equal(new List(), msg.DefaultsValues); + Assert.IsType>(msg.DefaultsValues); + Assert.Equal(new List(), msg.DefaultsValues); - Assert.IsType>(msg.BoolValuesDefault); - Assert.Equal( - new List { false, true, false }, - msg.BoolValuesDefault); + Assert.IsType>(msg.BoolValuesDefault); + Assert.Equal( + new List { false, true, false }, + msg.BoolValuesDefault); - Assert.IsType>(msg.ByteValuesDefault); - Assert.Equal( - new List { 0, 1, 255 }, - msg.ByteValuesDefault); + Assert.IsType>(msg.ByteValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.ByteValuesDefault); - Assert.IsType>(msg.CharValuesDefault); - Assert.Equal( - new List { 0, 1, 127 }, - msg.CharValuesDefault); + Assert.IsType>(msg.CharValuesDefault); + Assert.Equal( + new List { 0, 1, 127 }, + msg.CharValuesDefault); - Assert.IsType>(msg.Float32ValuesDefault); - Assert.Equal( - new List { 1.125f, 0.0f, -1.125f}, - msg.Float32ValuesDefault); + Assert.IsType>(msg.Float32ValuesDefault); + Assert.Equal( + new List { 1.125f, 0.0f, -1.125f }, + msg.Float32ValuesDefault); - Assert.IsType>(msg.Float64ValuesDefault); - Assert.Equal( - new List { 3.1415, 0.0, -3.1415 }, - msg.Float64ValuesDefault); + Assert.IsType>(msg.Float64ValuesDefault); + Assert.Equal( + new List { 3.1415, 0.0, -3.1415 }, + msg.Float64ValuesDefault); - Assert.IsType>(msg.Int8ValuesDefault); - Assert.Equal( - new List { 0, 127, -128 }, - msg.Int8ValuesDefault); + Assert.IsType>(msg.Int8ValuesDefault); + Assert.Equal( + new List { 0, 127, -128 }, + msg.Int8ValuesDefault); - Assert.IsType>(msg.Uint8ValuesDefault); - Assert.Equal( - new List { 0, 1, 255 }, - msg.Uint8ValuesDefault); + Assert.IsType>(msg.Uint8ValuesDefault); + Assert.Equal( + new List { 0, 1, 255 }, + msg.Uint8ValuesDefault); - Assert.IsType>(msg.Int16ValuesDefault); - Assert.Equal( - new List { 0, 32767, -32768 }, - msg.Int16ValuesDefault); + Assert.IsType>(msg.Int16ValuesDefault); + Assert.Equal( + new List { 0, 32767, -32768 }, + msg.Int16ValuesDefault); - Assert.IsType>(msg.Uint16ValuesDefault); - Assert.Equal( - new List { 0, 1, 65535 }, - msg.Uint16ValuesDefault); + Assert.IsType>(msg.Uint16ValuesDefault); + Assert.Equal( + new List { 0, 1, 65535 }, + msg.Uint16ValuesDefault); - Assert.IsType>(msg.Int32ValuesDefault); - Assert.Equal( - new List { 0, 2147483647, -2147483648 }, - msg.Int32ValuesDefault); + Assert.IsType>(msg.Int32ValuesDefault); + Assert.Equal( + new List { 0, 2147483647, -2147483648 }, + msg.Int32ValuesDefault); - Assert.IsType>(msg.Uint32ValuesDefault); - Assert.Equal( - new List { 0, 1, 4294967295 }, - msg.Uint32ValuesDefault); + Assert.IsType>(msg.Uint32ValuesDefault); + Assert.Equal( + new List { 0, 1, 4294967295 }, + msg.Uint32ValuesDefault); - Assert.IsType>(msg.Int64ValuesDefault); - Assert.Equal( - new List { 0, 9223372036854775807, -9223372036854775808 }, - msg.Int64ValuesDefault); + Assert.IsType>(msg.Int64ValuesDefault); + Assert.Equal( + new List { 0, 9223372036854775807, -9223372036854775808 }, + msg.Int64ValuesDefault); - Assert.IsType>(msg.Uint64ValuesDefault); - Assert.Equal( - new List { 0, 1, 18446744073709551615 }, - msg.Uint64ValuesDefault); - - Assert.IsType>(msg.StringValuesDefault); - Assert.Equal( - new List { "", "max value", "min value" }, - msg.StringValuesDefault); + Assert.IsType>(msg.Uint64ValuesDefault); + Assert.Equal( + new List { 0, 1, 18446744073709551615 }, + msg.Uint64ValuesDefault); + + Assert.IsType>(msg.StringValuesDefault); + Assert.Equal( + new List { "", "max value", "min value" }, + msg.StringValuesDefault); + } } - } } diff --git a/rcldotnet/test/test_services.cs b/rcldotnet/test/test_services.cs index 7712d86b..511dbcf5 100644 --- a/rcldotnet/test/test_services.cs +++ b/rcldotnet/test/test_services.cs @@ -29,11 +29,11 @@ public void TestServiceAndClient() test_msgs.srv.BasicTypes_Request serviceReceivedRequest = null; test_msgs.srv.BasicTypes_Response clientReceivedResponse = null; - + var service = serviceNode.CreateService("unittest_dotnet_service", HandleRequest); var client = clientNode.CreateClient("unittest_dotnet_service"); - + var request = new test_msgs.srv.BasicTypes_Request(); request.BoolValue = true; request.ByteValue = 36; diff --git a/rcldotnet_common/DllLoadUtils.cs b/rcldotnet_common/DllLoadUtils.cs index 280ca654..f85f680f 100644 --- a/rcldotnet_common/DllLoadUtils.cs +++ b/rcldotnet_common/DllLoadUtils.cs @@ -16,273 +16,329 @@ // Based on http://dimitry-i.blogspot.com.es/2013/01/mononet-how-to-dynamically-load-native.html using System; -using System.Runtime; using System.Runtime.InteropServices; -namespace ROS2 { - namespace Utils { - public class UnsatisfiedLinkError : System.Exception { - public UnsatisfiedLinkError () : base () { } - public UnsatisfiedLinkError (string message) : base (message) { } - public UnsatisfiedLinkError (string message, System.Exception inner) : base (message, inner) { } - } - - public class UnknownPlatformError : System.Exception { - public UnknownPlatformError () : base () { } - public UnknownPlatformError (string message) : base (message) { } - public UnknownPlatformError (string message, System.Exception inner) : base (message, inner) { } - } - - public enum Platform { - Unix, - MacOSX, - WindowsDesktop, - UWP, - Unknown - } - - public class DllLoadUtilsFactory { - [DllImport ("api-ms-win-core-libraryloader-l2-1-0.dll", EntryPoint = "LoadPackagedLibrary", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr LoadPackagedLibrary ([MarshalAs (UnmanagedType.LPWStr)] string fileName, int reserved = 0); - - [DllImport ("api-ms-win-core-libraryloader-l1-2-0.dll", EntryPoint = "FreeLibrary", SetLastError = true, ExactSpelling = true)] - private static extern int FreeLibraryUWP (IntPtr handle); - - [DllImport ("kernel32.dll", EntryPoint = "LoadLibraryA", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr LoadLibraryA (string fileName, int reserved = 0); - - [DllImport ("kernel32.dll", EntryPoint = "FreeLibrary", SetLastError = true, ExactSpelling = true)] - private static extern int FreeLibraryDesktop (IntPtr handle); - - [DllImport ("libdl.so", EntryPoint = "dlopen")] - private static extern IntPtr dlopen_unix (String fileName, int flags); - - [DllImport ("libdl.so", EntryPoint = "dlclose")] - private static extern int dlclose_unix (IntPtr handle); - - [DllImport ("libdl.dylib", EntryPoint = "dlopen")] - private static extern IntPtr dlopen_macosx (String fileName, int flags); - - [DllImport ("libdl.dylib", EntryPoint = "dlclose")] - private static extern int dlclose_macosx (IntPtr handle); - - const int RTLD_NOW = 2; - - public static DllLoadUtils GetDllLoadUtils () { - switch (CheckPlatform ()) { - case Platform.Unix: - return new DllLoadUtilsUnix (); - case Platform.MacOSX: - return new DllLoadUtilsMacOSX (); - case Platform.WindowsDesktop: - return new DllLoadUtilsWindowsDesktop (); - case Platform.UWP: - return new DllLoadUtilsUWP (); - case Platform.Unknown: - default: - throw new UnknownPlatformError (); - } - } - - private static bool IsUWP () { - try { - IntPtr ptr = LoadPackagedLibrary ("api-ms-win-core-libraryloader-l2-1-0.dll"); - FreeLibraryUWP (ptr); - return true; - } catch (TypeLoadException) { - return false; - } - } - - private static bool IsWindowsDesktop () { - try { - IntPtr ptr = LoadLibraryA ("kernel32.dll"); - FreeLibraryDesktop (ptr); - return true; - } catch (TypeLoadException) { - return false; - } - } - - private static bool IsUnix () { - try { - IntPtr ptr = dlopen_unix ("libdl.so", RTLD_NOW); - dlclose_unix (ptr); - return true; - } catch (TypeLoadException) { - return false; +namespace ROS2 +{ + namespace Utils + { + public class UnsatisfiedLinkError : System.Exception + { + public UnsatisfiedLinkError() : base() { } + public UnsatisfiedLinkError(string message) : base(message) { } + public UnsatisfiedLinkError(string message, System.Exception inner) : base(message, inner) { } } - } - - private static bool IsMacOSX () { - try { - IntPtr ptr = dlopen_macosx ("libdl.dylib", RTLD_NOW); - dlclose_macosx (ptr); - return true; - } catch (TypeLoadException) { - return false; - } - } - - private static Platform CheckPlatform () { - if (IsUnix ()) { - return Platform.Unix; - } else if (IsMacOSX ()) { - return Platform.MacOSX; - } else if (IsWindowsDesktop ()) { - return Platform.WindowsDesktop; - } else if (IsUWP ()) { - return Platform.UWP; - } else { - return Platform.Unknown; - } - } - } - - public interface DllLoadUtils { - IntPtr LoadLibrary (string fileName); - void FreeLibrary (IntPtr handle); - IntPtr GetProcAddress (IntPtr dllHandle, string name); - } - - public class DllLoadUtilsUWP : DllLoadUtils { - - [DllImport ("api-ms-win-core-libraryloader-l2-1-0.dll", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr LoadPackagedLibrary ([MarshalAs (UnmanagedType.LPWStr)] string fileName, int reserved = 0); - - [DllImport ("api-ms-win-core-libraryloader-l1-2-0.dll", SetLastError = true, ExactSpelling = true)] - private static extern int FreeLibrary (IntPtr handle); - - [DllImport ("api-ms-win-core-libraryloader-l1-2-0.dll", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr GetProcAddress (IntPtr handle, string procedureName); - void DllLoadUtils.FreeLibrary (IntPtr handle) { - FreeLibrary (handle); - } - - IntPtr DllLoadUtils.GetProcAddress (IntPtr dllHandle, string name) { - return GetProcAddress (dllHandle, name); - } - - IntPtr DllLoadUtils.LoadLibrary (string fileName) { - string libraryName = fileName + "_native.dll"; - IntPtr ptr = LoadPackagedLibrary (libraryName); - if (ptr == IntPtr.Zero) { - throw new UnsatisfiedLinkError (libraryName); + public class UnknownPlatformError : System.Exception + { + public UnknownPlatformError() : base() { } + public UnknownPlatformError(string message) : base(message) { } + public UnknownPlatformError(string message, System.Exception inner) : base(message, inner) { } } - return ptr; - } - } - - public class DllLoadUtilsWindowsDesktop : DllLoadUtils { - - [DllImport ("kernel32.dll", EntryPoint = "LoadLibraryA", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr LoadLibraryA (string fileName, int reserved = 0); - - [DllImport ("kernel32.dll", SetLastError = true, ExactSpelling = true)] - private static extern int FreeLibrary (IntPtr handle); - - [DllImport ("kernel32.dll", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr GetProcAddress (IntPtr handle, string procedureName); - void DllLoadUtils.FreeLibrary (IntPtr handle) { - FreeLibrary (handle); - } - - IntPtr DllLoadUtils.GetProcAddress (IntPtr dllHandle, string name) { - return GetProcAddress (dllHandle, name); - } - - IntPtr DllLoadUtils.LoadLibrary (string fileName) { - string libraryName = fileName + "_native.dll"; - IntPtr ptr = LoadLibraryA (libraryName); - if (ptr == IntPtr.Zero) { - throw new UnsatisfiedLinkError (libraryName); + public enum Platform + { + Unix, + MacOSX, + WindowsDesktop, + UWP, + Unknown } - return ptr; - } - } - - internal class DllLoadUtilsUnix : DllLoadUtils { - - [DllImport ("libdl.so", ExactSpelling = true)] - private static extern IntPtr dlopen (String fileName, int flags); - - [DllImport ("libdl.so", ExactSpelling = true)] - private static extern IntPtr dlsym (IntPtr handle, String symbol); - - [DllImport ("libdl.so", ExactSpelling = true)] - private static extern int dlclose (IntPtr handle); - [DllImport ("libdl.so", ExactSpelling = true)] - private static extern IntPtr dlerror (); - - const int RTLD_NOW = 2; - - public void FreeLibrary (IntPtr handle) { - dlclose (handle); - } - - public IntPtr GetProcAddress (IntPtr dllHandle, string name) { - // clear previous errors if any - dlerror (); - var res = dlsym (dllHandle, name); - var errPtr = dlerror (); - if (errPtr != IntPtr.Zero) { - throw new Exception ("dlsym: " + Marshal.PtrToStringAnsi (errPtr)); - } - return res; - } - - public IntPtr LoadLibrary (string fileName) { - string libraryName = "lib" + fileName + "_native.so"; - IntPtr ptr = dlopen (libraryName, RTLD_NOW); - if (ptr == IntPtr.Zero) { - throw new UnsatisfiedLinkError (libraryName); + public class DllLoadUtilsFactory + { + [DllImport("api-ms-win-core-libraryloader-l2-1-0.dll", EntryPoint = "LoadPackagedLibrary", SetLastError = true, ExactSpelling = true)] + private static extern IntPtr LoadPackagedLibrary([MarshalAs(UnmanagedType.LPWStr)] string fileName, int reserved = 0); + + [DllImport("api-ms-win-core-libraryloader-l1-2-0.dll", EntryPoint = "FreeLibrary", SetLastError = true, ExactSpelling = true)] + private static extern int FreeLibraryUWP(IntPtr handle); + + [DllImport("kernel32.dll", EntryPoint = "LoadLibraryA", SetLastError = true, ExactSpelling = true)] + private static extern IntPtr LoadLibraryA(string fileName, int reserved = 0); + + [DllImport("kernel32.dll", EntryPoint = "FreeLibrary", SetLastError = true, ExactSpelling = true)] + private static extern int FreeLibraryDesktop(IntPtr handle); + + [DllImport("libdl.so", EntryPoint = "dlopen")] + private static extern IntPtr dlopen_unix(string fileName, int flags); + + [DllImport("libdl.so", EntryPoint = "dlclose")] + private static extern int dlclose_unix(IntPtr handle); + + [DllImport("libdl.dylib", EntryPoint = "dlopen")] + private static extern IntPtr dlopen_macosx(string fileName, int flags); + + [DllImport("libdl.dylib", EntryPoint = "dlclose")] + private static extern int dlclose_macosx(IntPtr handle); + + private const int RTLD_NOW = 2; + + public static DllLoadUtils GetDllLoadUtils() + { + switch (CheckPlatform()) + { + case Platform.Unix: + return new DllLoadUtilsUnix(); + case Platform.MacOSX: + return new DllLoadUtilsMacOSX(); + case Platform.WindowsDesktop: + return new DllLoadUtilsWindowsDesktop(); + case Platform.UWP: + return new DllLoadUtilsUWP(); + case Platform.Unknown: + default: + throw new UnknownPlatformError(); + } + } + + private static bool IsUWP() + { + try + { + IntPtr ptr = LoadPackagedLibrary("api-ms-win-core-libraryloader-l2-1-0.dll"); + FreeLibraryUWP(ptr); + return true; + } + catch (TypeLoadException) + { + return false; + } + } + + private static bool IsWindowsDesktop() + { + try + { + IntPtr ptr = LoadLibraryA("kernel32.dll"); + FreeLibraryDesktop(ptr); + return true; + } + catch (TypeLoadException) + { + return false; + } + } + + private static bool IsUnix() + { + try + { + IntPtr ptr = dlopen_unix("libdl.so", RTLD_NOW); + dlclose_unix(ptr); + return true; + } + catch (TypeLoadException) + { + return false; + } + } + + private static bool IsMacOSX() + { + try + { + IntPtr ptr = dlopen_macosx("libdl.dylib", RTLD_NOW); + dlclose_macosx(ptr); + return true; + } + catch (TypeLoadException) + { + return false; + } + } + + private static Platform CheckPlatform() + { + if (IsUnix()) + { + return Platform.Unix; + } + else if (IsMacOSX()) + { + return Platform.MacOSX; + } + else if (IsWindowsDesktop()) + { + return Platform.WindowsDesktop; + } + else if (IsUWP()) + { + return Platform.UWP; + } + else + { + return Platform.Unknown; + } + } } - return ptr; - } - } - - internal class DllLoadUtilsMacOSX : DllLoadUtils { - - [DllImport ("libdl.dylib", ExactSpelling = true)] - private static extern IntPtr dlopen (String fileName, int flags); - [DllImport ("libdl.dylib", ExactSpelling = true)] - private static extern IntPtr dlsym (IntPtr handle, String symbol); - - [DllImport ("libdl.dylib", ExactSpelling = true)] - private static extern int dlclose (IntPtr handle); - - [DllImport ("libdl.dylib", ExactSpelling = true)] - private static extern IntPtr dlerror (); + public interface DllLoadUtils + { + IntPtr LoadLibrary(string fileName); + void FreeLibrary(IntPtr handle); + IntPtr GetProcAddress(IntPtr dllHandle, string name); + } - const int RTLD_NOW = 2; + public class DllLoadUtilsUWP : DllLoadUtils + { + + [DllImport("api-ms-win-core-libraryloader-l2-1-0.dll", SetLastError = true, ExactSpelling = true)] + private static extern IntPtr LoadPackagedLibrary([MarshalAs(UnmanagedType.LPWStr)] string fileName, int reserved = 0); + + [DllImport("api-ms-win-core-libraryloader-l1-2-0.dll", SetLastError = true, ExactSpelling = true)] + private static extern int FreeLibrary(IntPtr handle); + + [DllImport("api-ms-win-core-libraryloader-l1-2-0.dll", SetLastError = true, ExactSpelling = true)] + private static extern IntPtr GetProcAddress(IntPtr handle, string procedureName); + + void DllLoadUtils.FreeLibrary(IntPtr handle) + { + FreeLibrary(handle); + } + + IntPtr DllLoadUtils.GetProcAddress(IntPtr dllHandle, string name) + { + return GetProcAddress(dllHandle, name); + } + + IntPtr DllLoadUtils.LoadLibrary(string fileName) + { + string libraryName = fileName + "_native.dll"; + IntPtr ptr = LoadPackagedLibrary(libraryName); + if (ptr == IntPtr.Zero) + { + throw new UnsatisfiedLinkError(libraryName); + } + return ptr; + } + } - public void FreeLibrary (IntPtr handle) { - dlclose (handle); - } + public class DllLoadUtilsWindowsDesktop : DllLoadUtils + { + + [DllImport("kernel32.dll", EntryPoint = "LoadLibraryA", SetLastError = true, ExactSpelling = true)] + private static extern IntPtr LoadLibraryA(string fileName, int reserved = 0); + + [DllImport("kernel32.dll", SetLastError = true, ExactSpelling = true)] + private static extern int FreeLibrary(IntPtr handle); + + [DllImport("kernel32.dll", SetLastError = true, ExactSpelling = true)] + private static extern IntPtr GetProcAddress(IntPtr handle, string procedureName); + + void DllLoadUtils.FreeLibrary(IntPtr handle) + { + FreeLibrary(handle); + } + + IntPtr DllLoadUtils.GetProcAddress(IntPtr dllHandle, string name) + { + return GetProcAddress(dllHandle, name); + } + + IntPtr DllLoadUtils.LoadLibrary(string fileName) + { + string libraryName = fileName + "_native.dll"; + IntPtr ptr = LoadLibraryA(libraryName); + if (ptr == IntPtr.Zero) + { + throw new UnsatisfiedLinkError(libraryName); + } + return ptr; + } + } - public IntPtr GetProcAddress (IntPtr dllHandle, string name) { - // clear previous errors if any - dlerror (); - var res = dlsym (dllHandle, name); - var errPtr = dlerror (); - if (errPtr != IntPtr.Zero) { - throw new Exception ("dlsym: " + Marshal.PtrToStringAnsi (errPtr)); + internal class DllLoadUtilsUnix : DllLoadUtils + { + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern IntPtr dlopen(string fileName, int flags); + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern IntPtr dlsym(IntPtr handle, string symbol); + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern int dlclose(IntPtr handle); + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern IntPtr dlerror(); + + private const int RTLD_NOW = 2; + + public void FreeLibrary(IntPtr handle) + { + dlclose(handle); + } + + public IntPtr GetProcAddress(IntPtr dllHandle, string name) + { + // clear previous errors if any + dlerror(); + var res = dlsym(dllHandle, name); + var errPtr = dlerror(); + if (errPtr != IntPtr.Zero) + { + throw new Exception("dlsym: " + Marshal.PtrToStringAnsi(errPtr)); + } + return res; + } + + public IntPtr LoadLibrary(string fileName) + { + string libraryName = "lib" + fileName + "_native.so"; + IntPtr ptr = dlopen(libraryName, RTLD_NOW); + if (ptr == IntPtr.Zero) + { + throw new UnsatisfiedLinkError(libraryName); + } + return ptr; + } } - return res; - } - - public IntPtr LoadLibrary (string fileName) { - string libraryName = "lib" + fileName + "_native.dylib"; - IntPtr ptr = dlopen (libraryName, RTLD_NOW); - if (ptr == IntPtr.Zero) { - throw new UnsatisfiedLinkError (libraryName); + + internal class DllLoadUtilsMacOSX : DllLoadUtils + { + + [DllImport("libdl.dylib", ExactSpelling = true)] + private static extern IntPtr dlopen(string fileName, int flags); + + [DllImport("libdl.dylib", ExactSpelling = true)] + private static extern IntPtr dlsym(IntPtr handle, string symbol); + + [DllImport("libdl.dylib", ExactSpelling = true)] + private static extern int dlclose(IntPtr handle); + + [DllImport("libdl.dylib", ExactSpelling = true)] + private static extern IntPtr dlerror(); + + private const int RTLD_NOW = 2; + + public void FreeLibrary(IntPtr handle) + { + dlclose(handle); + } + + public IntPtr GetProcAddress(IntPtr dllHandle, string name) + { + // clear previous errors if any + dlerror(); + var res = dlsym(dllHandle, name); + var errPtr = dlerror(); + if (errPtr != IntPtr.Zero) + { + throw new Exception("dlsym: " + Marshal.PtrToStringAnsi(errPtr)); + } + return res; + } + + public IntPtr LoadLibrary(string fileName) + { + string libraryName = "lib" + fileName + "_native.dylib"; + IntPtr ptr = dlopen(libraryName, RTLD_NOW); + if (ptr == IntPtr.Zero) + { + throw new UnsatisfiedLinkError(libraryName); + } + return ptr; + } } - return ptr; - } } - } } diff --git a/rcldotnet_common/IRosMessage.cs b/rcldotnet_common/IRosMessage.cs index 87dc0751..30a04576 100644 --- a/rcldotnet_common/IRosMessage.cs +++ b/rcldotnet_common/IRosMessage.cs @@ -23,7 +23,7 @@ public interface IRosMessage // (static abstract interface members are not supported yet.) // public static abstract IntPtr __GetTypeSupport(); // public static abstract SafeHandle __CreateMessageHandle(); - + void __ReadFromHandle(IntPtr messageHandle); void __WriteToHandle(IntPtr messageHandle); diff --git a/rcldotnet_examples/RCLDotnetClient.cs b/rcldotnet_examples/RCLDotnetClient.cs index 08868ced..f8b538ee 100644 --- a/rcldotnet_examples/RCLDotnetClient.cs +++ b/rcldotnet_examples/RCLDotnetClient.cs @@ -1,9 +1,5 @@ using System; -using System.Reflection; -using System.Runtime; -using System.Runtime.InteropServices; using System.Threading; -using System.Threading.Tasks; using ROS2; namespace ConsoleApplication @@ -16,21 +12,21 @@ public static void Main(string[] args) var node = RCLdotnet.CreateNode("client"); var client = node.CreateClient("test_dotnet_service_name"); - + // TODO: (sh) Add WaitForService(timeout) method that observes the node graph. Console.WriteLine("Waiting for service..."); while (RCLdotnet.Ok() && !client.ServiceIsReady()) { Thread.Sleep(500); } - + var request = new std_srvs.srv.SetBool_Request(); request.Data = true; var task = client.SendRequestAsync(request); Console.WriteLine("Sent request."); - + while (RCLdotnet.Ok()) { RCLdotnet.SpinOnce(node, 500); diff --git a/rcldotnet_examples/RCLDotnetListener.cs b/rcldotnet_examples/RCLDotnetListener.cs index 34a56589..62da2ed6 100644 --- a/rcldotnet_examples/RCLDotnetListener.cs +++ b/rcldotnet_examples/RCLDotnetListener.cs @@ -1,22 +1,21 @@ using System; -using System.Reflection; -using System.Runtime; -using System.Runtime.InteropServices; -using System.Threading; using ROS2; -namespace ConsoleApplication { - public class RCLDotnetListener { - public static void Main (string[] args) { - RCLdotnet.Init (); +namespace ConsoleApplication +{ + public class RCLDotnetListener + { + public static void Main(string[] args) + { + RCLdotnet.Init(); - Node node = RCLdotnet.CreateNode ("listener"); + Node node = RCLdotnet.CreateNode("listener"); - Subscription chatter_sub = node.CreateSubscription ( - "chatter", msg => Console.WriteLine ("I heard: [" + msg.Data + "]")); + Subscription chatter_sub = node.CreateSubscription( + "chatter", msg => Console.WriteLine("I heard: [" + msg.Data + "]")); - RCLdotnet.Spin (node); + RCLdotnet.Spin(node); + } } - } } diff --git a/rcldotnet_examples/RCLDotnetService.cs b/rcldotnet_examples/RCLDotnetService.cs index a74c7d5b..af2b3333 100644 --- a/rcldotnet_examples/RCLDotnetService.cs +++ b/rcldotnet_examples/RCLDotnetService.cs @@ -1,8 +1,4 @@ using System; -using System.Reflection; -using System.Runtime; -using System.Runtime.InteropServices; -using System.Threading; using ROS2; namespace ConsoleApplication diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index 8744bf07..9208f2d7 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -1,34 +1,34 @@ using System; -using System.Reflection; -using System.Runtime; -using System.Runtime.InteropServices; using System.Threading; using ROS2; -using ROS2.Utils; -namespace ConsoleApplication { - public class RCLDotnetTalker { - public static void Main (string[] args) { - RCLdotnet.Init (); +namespace ConsoleApplication +{ + public class RCLDotnetTalker + { + public static void Main(string[] args) + { + RCLdotnet.Init(); - Node node = RCLdotnet.CreateNode ("talker"); + Node node = RCLdotnet.CreateNode("talker"); - Publisher chatter_pub = node.CreatePublisher ("chatter"); + Publisher chatter_pub = node.CreatePublisher("chatter"); - std_msgs.msg.String msg = new std_msgs.msg.String (); + std_msgs.msg.String msg = new std_msgs.msg.String(); - int i = 1; + int i = 1; - while (RCLdotnet.Ok ()) { - msg.Data = "Hello World: " + i; - i++; - Console.WriteLine ("Publishing: \"" + msg.Data + "\""); - chatter_pub.Publish (msg); + while (RCLdotnet.Ok()) + { + msg.Data = "Hello World: " + i; + i++; + Console.WriteLine("Publishing: \"" + msg.Data + "\""); + chatter_pub.Publish(msg); - // Sleep a little bit between each message - Thread.Sleep (1000); - } + // Sleep a little bit between each message + Thread.Sleep(1000); + } + } } - } } From fc68df1c1da76ecbecd65b952e89f268a2a969eb Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 15:51:12 +0200 Subject: [PATCH 36/73] codestyle: fix naming of internal variables --- rcldotnet/Client.cs | 12 +- rcldotnet/Node.cs | 78 ++--- rcldotnet/Publisher.cs | 8 +- rcldotnet/RCLdotnet.cs | 57 ++-- rcldotnet/Subscription.cs | 6 +- rcldotnet/test/test_messages.cs | 376 ++++++++++++------------ rcldotnet_examples/RCLDotnetListener.cs | 2 +- rcldotnet_examples/RCLDotnetTalker.cs | 4 +- 8 files changed, 271 insertions(+), 272 deletions(-) diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 695c8ec4..d0710861 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -23,11 +23,11 @@ namespace ROS2 { internal static class ClientDelegates { - internal static readonly DllLoadUtils dllLoadUtils; + internal static readonly DllLoadUtils _dllLoadUtils; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLSendRequestType( - SafeClientHandle clientHandle, SafeHandle requestHandle, out long seqneceNumber); + SafeClientHandle clientHandle, SafeHandle requestHandle, out long sequenceNumber); internal static NativeRCLSendRequestType native_rcl_send_request = null; @@ -39,14 +39,14 @@ internal delegate RCLRet NativeRCLServiceServerIsAvailableType( static ClientDelegates() { - dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); - IntPtr nativelibrary = dllLoadUtils.LoadLibrary("rcldotnet_client"); + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet_client"); - IntPtr native_rcl_send_request_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "native_rcl_send_request"); + IntPtr native_rcl_send_request_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_send_request"); ClientDelegates.native_rcl_send_request = (NativeRCLSendRequestType)Marshal.GetDelegateForFunctionPointer( native_rcl_send_request_ptr, typeof(NativeRCLSendRequestType)); - IntPtr native_rcl_service_server_is_available_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "native_rcl_service_server_is_available"); + IntPtr native_rcl_service_server_is_available_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_service_server_is_available"); ClientDelegates.native_rcl_service_server_is_available = (NativeRCLServiceServerIsAvailableType)Marshal.GetDelegateForFunctionPointer( native_rcl_service_server_is_available_ptr, typeof(NativeRCLServiceServerIsAvailableType)); } diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 342dbb6b..8a3c585b 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -22,7 +22,7 @@ namespace ROS2 { internal static class NodeDelegates { - private static readonly DllLoadUtils dllLoadUtils; + private static readonly DllLoadUtils _dllLoadUtils; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreatePublisherHandleType( @@ -74,60 +74,60 @@ internal delegate RCLRet NativeRCLDestroyClientHandleType( static NodeDelegates() { - dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); - IntPtr nativelibrary = dllLoadUtils.LoadLibrary("rcldotnet_node"); + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet_node"); - IntPtr native_rcl_create_publisher_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_create_publisher_handle"); + IntPtr native_rcl_create_publisher_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_create_publisher_handle"); NodeDelegates.native_rcl_create_publisher_handle = (NativeRCLCreatePublisherHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_create_publisher_handle_ptr, typeof(NativeRCLCreatePublisherHandleType)); - IntPtr native_rcl_destroy_publisher_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_destroy_publisher_handle"); + IntPtr native_rcl_destroy_publisher_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_destroy_publisher_handle"); NodeDelegates.native_rcl_destroy_publisher_handle = (NativeRCLDestroyPublisherHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_publisher_handle_ptr, typeof(NativeRCLDestroyPublisherHandleType)); - IntPtr native_rcl_create_subscription_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_create_subscription_handle"); + IntPtr native_rcl_create_subscription_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_create_subscription_handle"); NodeDelegates.native_rcl_create_subscription_handle = (NativeRCLCreateSubscriptionHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_create_subscription_handle_ptr, typeof(NativeRCLCreateSubscriptionHandleType)); - IntPtr native_rcl_destroy_subscription_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_destroy_subscription_handle"); + IntPtr native_rcl_destroy_subscription_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_destroy_subscription_handle"); NodeDelegates.native_rcl_destroy_subscription_handle = (NativeRCLDestroySubscriptionHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_subscription_handle_ptr, typeof(NativeRCLDestroySubscriptionHandleType)); - IntPtr native_rcl_create_service_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_create_service_handle"); + IntPtr native_rcl_create_service_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_create_service_handle"); NodeDelegates.native_rcl_create_service_handle = (NativeRCLCreateServiceHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_create_service_handle_ptr, typeof(NativeRCLCreateServiceHandleType)); - IntPtr native_rcl_destroy_service_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_destroy_service_handle"); + IntPtr native_rcl_destroy_service_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_destroy_service_handle"); NodeDelegates.native_rcl_destroy_service_handle = (NativeRCLDestroyServiceHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_service_handle_ptr, typeof(NativeRCLDestroyServiceHandleType)); - IntPtr native_rcl_create_client_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_create_client_handle"); + IntPtr native_rcl_create_client_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_create_client_handle"); NodeDelegates.native_rcl_create_client_handle = (NativeRCLCreateClientHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_create_client_handle_ptr, typeof(NativeRCLCreateClientHandleType)); - IntPtr native_rcl_destroy_client_handle_ptr = dllLoadUtils.GetProcAddress( - nativelibrary, "native_rcl_destroy_client_handle"); + IntPtr native_rcl_destroy_client_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_destroy_client_handle"); NodeDelegates.native_rcl_destroy_client_handle = (NativeRCLDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( @@ -138,28 +138,28 @@ static NodeDelegates() public sealed class Node { - private readonly IList subscriptions_; + private readonly IList _subscriptions; - private readonly IList services_; + private readonly IList _services; - private readonly IList clients_; + private readonly IList _clients; internal Node(SafeNodeHandle handle) { Handle = handle; - subscriptions_ = new List(); - services_ = new List(); - clients_ = new List(); + _subscriptions = new List(); + _services = new List(); + _clients = new List(); } - public IList Subscriptions => subscriptions_; + public IList Subscriptions => _subscriptions; // TODO: (sh) wrap in readonly collection // TODO: (sh) Add posibility to remove Subscriptions/Services/Clients from Node (if this is a thing in ROS) // Now there is no Api to remove these Objects, and there is a strong reference from these fileds to them so they dont't get collected. - public IList Services => services_; + public IList Services => _services; - public IList Clients => clients_; + public IList Clients => _clients; // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its @@ -170,10 +170,10 @@ internal Node(SafeNodeHandle handle) public Publisher CreatePublisher(string topic) where T : IRosMessage { - IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); + IntPtr typeSupport = MessageStaticMemberCache.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); publisherHandle.SetParent(Handle); if (ret != RCLRet.Ok) { @@ -188,10 +188,10 @@ public Publisher CreatePublisher(string topic) where T : IRosMessage public Subscription CreateSubscription(string topic, Action callback) where T : IRosMessage, new() { - IntPtr typesupport = MessageStaticMemberCache.GetTypeSupport(); + IntPtr typeSupport = MessageStaticMemberCache.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); subscriptionHandle.SetParent(Handle); if (ret != RCLRet.Ok) { @@ -201,7 +201,7 @@ public Publisher CreatePublisher(string topic) where T : IRosMessage // TODO: (sh) Add topic as propety to Subscription. Subscription subscription = new Subscription(subscriptionHandle, callback); - subscriptions_.Add(subscription); + _subscriptions.Add(subscription); return subscription; } @@ -210,10 +210,10 @@ public Service CreateService.GetTypeSupport(); + IntPtr typeSupport = ServiceDefinitionStaticMemberCache.GetTypeSupport(); var serviceHandle = new SafeServiceHandle(); - RCLRet ret = NodeDelegates.native_rcl_create_service_handle(ref serviceHandle, Handle, serviceName, typesupport); + RCLRet ret = NodeDelegates.native_rcl_create_service_handle(ref serviceHandle, Handle, serviceName, typeSupport); serviceHandle.SetParent(Handle); if (ret != RCLRet.Ok) { @@ -223,7 +223,7 @@ public Service CreateService(serviceHandle, callback); - services_.Add(service); + _services.Add(service); return service; } @@ -232,10 +232,10 @@ public Client CreateClient.GetTypeSupport(); + IntPtr typeSupport = ServiceDefinitionStaticMemberCache.GetTypeSupport(); var clientHandle = new SafeClientHandle(); - RCLRet ret = NodeDelegates.native_rcl_create_client_handle(ref clientHandle, Handle, serviceName, typesupport); + RCLRet ret = NodeDelegates.native_rcl_create_client_handle(ref clientHandle, Handle, serviceName, typeSupport); clientHandle.SetParent(Handle); if (ret != RCLRet.Ok) { @@ -245,7 +245,7 @@ public Client CreateClient(clientHandle, this); - clients_.Add(client); + _clients.Add(client); return client; } } diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index 6edaa285..a0e55378 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -21,7 +21,7 @@ namespace ROS2 { internal static class PublisherDelegates { - internal static readonly DllLoadUtils dllLoadUtils; + internal static readonly DllLoadUtils _dllLoadUtils; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLPublishType( @@ -31,10 +31,10 @@ internal delegate RCLRet NativeRCLPublishType( static PublisherDelegates() { - dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); - IntPtr nativelibrary = dllLoadUtils.LoadLibrary("rcldotnet_publisher"); + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet_publisher"); - IntPtr native_rcl_publish_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "native_rcl_publish"); + IntPtr native_rcl_publish_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_publish"); PublisherDelegates.native_rcl_publish = (NativeRCLPublishType)Marshal.GetDelegateForFunctionPointer( native_rcl_publish_ptr, typeof(NativeRCLPublishType)); } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 585b1ab1..8fb16a25 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -21,7 +21,7 @@ namespace ROS2 { internal static class RCLdotnetDelegates { - internal static readonly DllLoadUtils dllLoadUtils; + internal static readonly DllLoadUtils _dllLoadUtils; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLInitType(); @@ -137,132 +137,131 @@ internal delegate RCLRet NativeRCLCreateRequestIdHandleType( static RCLdotnetDelegates() { - dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); - string library_name = "rcldotnet"; - IntPtr pDll = dllLoadUtils.LoadLibrary(library_name); + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); IntPtr native_rcl_init_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_init"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_init"); RCLdotnetDelegates.native_rcl_init = (NativeRCLInitType)Marshal.GetDelegateForFunctionPointer( native_rcl_init_ptr, typeof(NativeRCLInitType)); IntPtr native_rcl_get_error_string_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_get_error_string"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_get_error_string"); RCLdotnetDelegates.native_rcl_get_error_string = (NativeRCLGetErrorStringType)Marshal.GetDelegateForFunctionPointer( native_rcl_get_error_string_ptr, typeof(NativeRCLGetErrorStringType)); IntPtr native_rcl_reset_error_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_reset_error"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_reset_error"); RCLdotnetDelegates.native_rcl_reset_error = (NativeRCLResetErrorType)Marshal.GetDelegateForFunctionPointer( native_rcl_reset_error_ptr, typeof(NativeRCLResetErrorType)); IntPtr native_rcl_get_rmw_identifier_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_get_rmw_identifier"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_get_rmw_identifier"); RCLdotnetDelegates.native_rcl_get_rmw_identifier = (NativeRCLGetRMWIdentifierType)Marshal.GetDelegateForFunctionPointer( native_rcl_get_rmw_identifier_ptr, typeof(NativeRCLGetRMWIdentifierType)); IntPtr native_rcl_ok_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_ok"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_ok"); RCLdotnetDelegates.native_rcl_ok = (NativeRCLOkType)Marshal.GetDelegateForFunctionPointer( native_rcl_ok_ptr, typeof(NativeRCLOkType)); IntPtr native_rcl_create_node_handle_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_create_node_handle"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_node_handle"); RCLdotnetDelegates.native_rcl_create_node_handle = (NativeRCLCreateNodeHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_create_node_handle_ptr, typeof(NativeRCLCreateNodeHandleType)); IntPtr native_rcl_destroy_node_handle_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_destroy_node_handle"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_destroy_node_handle"); RCLdotnetDelegates.native_rcl_destroy_node_handle = (NativeRCLDestroyNodeHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_node_handle_ptr, typeof(NativeRCLDestroyNodeHandleType)); IntPtr native_rcl_create_wait_set_handle_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_create_wait_set_handle"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_wait_set_handle"); RCLdotnetDelegates.native_rcl_create_wait_set_handle = (NativeRCLCreateWaitSetHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_create_wait_set_handle_ptr, typeof(NativeRCLCreateWaitSetHandleType)); IntPtr native_rcl_destroy_wait_set_handle_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_destroy_wait_set_handle"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_destroy_wait_set_handle"); RCLdotnetDelegates.native_rcl_destroy_wait_set_handle = (NativeRCLDestroyWaitSetType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_wait_set_handle_ptr, typeof(NativeRCLDestroyWaitSetType)); IntPtr native_rcl_wait_set_clear_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_clear"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_clear"); RCLdotnetDelegates.native_rcl_wait_set_clear = (NativeRCLWaitSetClearType)Marshal.GetDelegateForFunctionPointer( native_rcl_wait_set_clear_ptr, typeof(NativeRCLWaitSetClearType)); IntPtr native_rcl_wait_set_add_subscription_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_add_subscription"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_add_subscription"); RCLdotnetDelegates.native_rcl_wait_set_add_subscription = (NativeRCLWaitSetAddSubscriptionType)Marshal.GetDelegateForFunctionPointer( native_rcl_wait_set_add_subscription_ptr, typeof(NativeRCLWaitSetAddSubscriptionType)); IntPtr native_rcl_wait_set_add_service_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_add_service"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_add_service"); RCLdotnetDelegates.native_rcl_wait_set_add_service = (NativeRCLWaitSetAddServiceType)Marshal.GetDelegateForFunctionPointer( native_rcl_wait_set_add_service_ptr, typeof(NativeRCLWaitSetAddServiceType)); IntPtr native_rcl_wait_set_add_client_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait_set_add_client"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_add_client"); RCLdotnetDelegates.native_rcl_wait_set_add_client = (NativeRCLWaitSetAddClientType)Marshal.GetDelegateForFunctionPointer( native_rcl_wait_set_add_client_ptr, typeof(NativeRCLWaitSetAddClientType)); IntPtr native_rcl_wait_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_wait"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait"); RCLdotnetDelegates.native_rcl_wait = (NativeRCLWaitType)Marshal.GetDelegateForFunctionPointer( native_rcl_wait_ptr, typeof(NativeRCLWaitType)); IntPtr native_rcl_take_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_take"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_take"); RCLdotnetDelegates.native_rcl_take = (NativeRCLTakeType)Marshal.GetDelegateForFunctionPointer( native_rcl_take_ptr, typeof(NativeRCLTakeType)); IntPtr native_rcl_create_request_id_handle_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_create_request_id_handle"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_request_id_handle"); RCLdotnetDelegates.native_rcl_create_request_id_handle = (NativeRCLCreateRequestIdHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_create_request_id_handle_ptr, typeof(NativeRCLCreateRequestIdHandleType)); IntPtr native_rcl_destroy_request_id_handle_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_destroy_request_id_handle"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_destroy_request_id_handle"); RCLdotnetDelegates.native_rcl_destroy_request_id_handle = (NativeRCLDestroyRequestIdHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_request_id_handle_ptr, typeof(NativeRCLDestroyRequestIdHandleType)); IntPtr native_rcl_request_id_get_sequence_number_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_request_id_get_sequence_number"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_request_id_get_sequence_number"); RCLdotnetDelegates.native_rcl_request_id_get_sequence_number = (NativeRCLRequestIdGetSequenceNumberType)Marshal.GetDelegateForFunctionPointer( native_rcl_request_id_get_sequence_number_ptr, typeof(NativeRCLRequestIdGetSequenceNumberType)); IntPtr native_rcl_take_request_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_take_request"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_take_request"); RCLdotnetDelegates.native_rcl_take_request = (NativeRCLTakeRequestType)Marshal.GetDelegateForFunctionPointer( native_rcl_take_request_ptr, typeof(NativeRCLTakeRequestType)); IntPtr native_rcl_send_response_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_send_response"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_send_response"); RCLdotnetDelegates.native_rcl_send_response = (NativeRCLSendResponseType)Marshal.GetDelegateForFunctionPointer( native_rcl_send_response_ptr, typeof(NativeRCLSendResponseType)); IntPtr native_rcl_take_response_ptr = - dllLoadUtils.GetProcAddress(pDll, "native_rcl_take_response"); + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_take_response"); RCLdotnetDelegates.native_rcl_take_response = (NativeRCLTakeResponseType)Marshal.GetDelegateForFunctionPointer( native_rcl_take_response_ptr, typeof(NativeRCLTakeResponseType)); @@ -316,8 +315,8 @@ private static SafeWaitSetHandle CreateWaitSet( { var waitSetHandle = new SafeWaitSetHandle(); RCLRet ret = RCLdotnetDelegates.native_rcl_create_wait_set_handle( - ref waitSetHandle, numberOfSubscriptions, numberOfGuardConditions, - numberOfTimers, numberOfClients, numberOfServices, numberOfEvents); + ref waitSetHandle, numberOfSubscriptions, numberOfGuardConditions, + numberOfTimers, numberOfClients, numberOfServices, numberOfEvents); if (ret != RCLRet.Ok) { waitSetHandle.Dispose(); @@ -359,8 +358,8 @@ private static void WaitSetAddClient(SafeWaitSetHandle waitSetHandle, SafeClient /// True if wait set is ready, False on timeout. private static bool Wait(SafeWaitSetHandle waitSetHandle, long timeout) { - long ns_timeout = timeout * 1000000; - RCLRet ret = RCLdotnetDelegates.native_rcl_wait(waitSetHandle, ns_timeout); + long nsTimeout = timeout * 1000000; + RCLRet ret = RCLdotnetDelegates.native_rcl_wait(waitSetHandle, nsTimeout); if (ret == RCLRet.Ok || ret == RCLRet.Timeout) { return ret == RCLRet.Ok; diff --git a/rcldotnet/Subscription.cs b/rcldotnet/Subscription.cs index 0cd7211a..cc639f37 100644 --- a/rcldotnet/Subscription.cs +++ b/rcldotnet/Subscription.cs @@ -46,12 +46,12 @@ internal Subscription() public class Subscription : Subscription where T : IRosMessage, new() { - private readonly Action callback_; + private readonly Action _callback; internal Subscription(SafeSubscriptionHandle handle, Action callback) { Handle = handle; - callback_ = callback; + _callback = callback; } internal override SafeSubscriptionHandle Handle { get; } @@ -62,7 +62,7 @@ internal Subscription(SafeSubscriptionHandle handle, Action callback) internal override void TriggerCallback(IRosMessage message) { - callback_((T)message); + _callback((T)message); } } } diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index a344152f..39d27eb3 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -12,29 +12,29 @@ public class TestMessages public void TestPublishString() { RCLdotnet.Init(); - Node node_string_1 = RCLdotnet.CreateNode("test_string_1"); - Node node_string_2 = RCLdotnet.CreateNode("test_string_2"); - Publisher chatter_pub = node_string_1.CreatePublisher("topic_string"); + Node nodeString1 = RCLdotnet.CreateNode("test_string_1"); + Node nodeString2 = RCLdotnet.CreateNode("test_string_2"); + Publisher chatterPub = nodeString1.CreatePublisher("topic_string"); std_msgs.msg.String msg = new std_msgs.msg.String(); std_msgs.msg.String msg2 = new std_msgs.msg.String(); msg.Data = "Hello"; bool received = false; - Subscription chatter_sub = node_string_2.CreateSubscription( - "topic_string", rcv_msg => + Subscription chatter_sub = nodeString2.CreateSubscription( + "topic_string", rcvMsg => { received = true; - msg2 = rcv_msg; + msg2 = rcvMsg; } ); while (!received) { - chatter_pub.Publish(msg); + chatterPub.Publish(msg); - RCLdotnet.SpinOnce(node_string_1, 500); - RCLdotnet.SpinOnce(node_string_2, 500); + RCLdotnet.SpinOnce(nodeString1, 500); + RCLdotnet.SpinOnce(nodeString2, 500); } Assert.Equal("Hello", msg2.Data); } @@ -43,9 +43,9 @@ public void TestPublishString() public void TestPublishBuiltins() { RCLdotnet.Init(); - Node node_builtins_1 = RCLdotnet.CreateNode("test_builtins_1"); - Node node_builtins_2 = RCLdotnet.CreateNode("test_builtins_2"); - Publisher chatter_pub = node_builtins_1.CreatePublisher("topic_builtins"); + Node nodeBuiltins1 = RCLdotnet.CreateNode("test_builtins_1"); + Node nodeBuiltins2 = RCLdotnet.CreateNode("test_builtins_2"); + Publisher chatterPub = nodeBuiltins1.CreatePublisher("topic_builtins"); test_msgs.msg.Builtins msg = new test_msgs.msg.Builtins(); test_msgs.msg.Builtins msg2 = new test_msgs.msg.Builtins(); @@ -55,20 +55,20 @@ public void TestPublishBuiltins() msg.TimeValue.Nanosec = 4u; bool received = false; - Subscription chatter_sub = node_builtins_2.CreateSubscription( - "topic_builtins", rcv_msg => + Subscription chatterSub = nodeBuiltins2.CreateSubscription( + "topic_builtins", rcvMsg => { received = true; - msg2 = rcv_msg; + msg2 = rcvMsg; } ); while (!received) { - chatter_pub.Publish(msg); + chatterPub.Publish(msg); - RCLdotnet.SpinOnce(node_builtins_1, 500); - RCLdotnet.SpinOnce(node_builtins_2, 500); + RCLdotnet.SpinOnce(nodeBuiltins1, 500); + RCLdotnet.SpinOnce(nodeBuiltins2, 500); } Assert.Equal(1, msg2.DurationValue.Sec); @@ -81,9 +81,9 @@ public void TestPublishBuiltins() public void TestPublishArrays() { RCLdotnet.Init(); - Node node_array_1 = RCLdotnet.CreateNode("test_arrays_1"); - Node node_array_2 = RCLdotnet.CreateNode("test_arrays_2"); - Publisher chatter_pub = node_array_1.CreatePublisher("topic_array"); + Node nodeArray1 = RCLdotnet.CreateNode("test_arrays_1"); + Node nodeArray2 = RCLdotnet.CreateNode("test_arrays_2"); + Publisher chatterPub = nodeArray1.CreatePublisher("topic_array"); test_msgs.msg.Arrays msg = new test_msgs.msg.Arrays(); test_msgs.msg.Arrays msg2 = new test_msgs.msg.Arrays(); @@ -159,70 +159,70 @@ public void TestPublishArrays() msg.StringValues[2] = "three"; // basicTypesValues - test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.BoolValue = true; - basic_type_1.ByteValue = 36; - basic_type_1.CharValue = 37; - basic_type_1.Float32Value = 38.1f; - basic_type_1.Float64Value = 39.1; - basic_type_1.Int8Value = 40; - basic_type_1.Uint8Value = 41; - basic_type_1.Int16Value = 42; - basic_type_1.Uint16Value = 43; - basic_type_1.Int32Value = 44; - basic_type_1.Uint32Value = 45; - basic_type_1.Int64Value = 46; - basic_type_1.Uint64Value = 47; - - test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.BoolValue = false; - basic_type_2.ByteValue = 48; - basic_type_2.CharValue = 49; - basic_type_2.Float32Value = 50.1f; - basic_type_2.Float64Value = 51.1; - basic_type_2.Int8Value = 52; - basic_type_2.Uint8Value = 53; - basic_type_2.Int16Value = 54; - basic_type_2.Uint16Value = 55; - basic_type_2.Int32Value = 56; - basic_type_2.Uint32Value = 57; - basic_type_2.Int64Value = 58; - basic_type_2.Uint64Value = 59; - - test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.BoolValue = true; - basic_type_3.ByteValue = 60; - basic_type_3.CharValue = 61; - basic_type_3.Float32Value = 62.1f; - basic_type_3.Float64Value = 63.1; - basic_type_3.Int8Value = 64; - basic_type_3.Uint8Value = 65; - basic_type_3.Int16Value = 66; - basic_type_3.Uint16Value = 67; - basic_type_3.Int32Value = 68; - basic_type_3.Uint32Value = 69; - basic_type_3.Int64Value = 70; - basic_type_3.Uint64Value = 71; - - msg.BasicTypesValues[0] = basic_type_1; - msg.BasicTypesValues[1] = basic_type_2; - msg.BasicTypesValues[2] = basic_type_3; + test_msgs.msg.BasicTypes basicType1 = new test_msgs.msg.BasicTypes(); + basicType1.BoolValue = true; + basicType1.ByteValue = 36; + basicType1.CharValue = 37; + basicType1.Float32Value = 38.1f; + basicType1.Float64Value = 39.1; + basicType1.Int8Value = 40; + basicType1.Uint8Value = 41; + basicType1.Int16Value = 42; + basicType1.Uint16Value = 43; + basicType1.Int32Value = 44; + basicType1.Uint32Value = 45; + basicType1.Int64Value = 46; + basicType1.Uint64Value = 47; + + test_msgs.msg.BasicTypes basicType2 = new test_msgs.msg.BasicTypes(); + basicType2.BoolValue = false; + basicType2.ByteValue = 48; + basicType2.CharValue = 49; + basicType2.Float32Value = 50.1f; + basicType2.Float64Value = 51.1; + basicType2.Int8Value = 52; + basicType2.Uint8Value = 53; + basicType2.Int16Value = 54; + basicType2.Uint16Value = 55; + basicType2.Int32Value = 56; + basicType2.Uint32Value = 57; + basicType2.Int64Value = 58; + basicType2.Uint64Value = 59; + + test_msgs.msg.BasicTypes basicType3 = new test_msgs.msg.BasicTypes(); + basicType3.BoolValue = true; + basicType3.ByteValue = 60; + basicType3.CharValue = 61; + basicType3.Float32Value = 62.1f; + basicType3.Float64Value = 63.1; + basicType3.Int8Value = 64; + basicType3.Uint8Value = 65; + basicType3.Int16Value = 66; + basicType3.Uint16Value = 67; + basicType3.Int32Value = 68; + basicType3.Uint32Value = 69; + basicType3.Int64Value = 70; + basicType3.Uint64Value = 71; + + msg.BasicTypesValues[0] = basicType1; + msg.BasicTypesValues[1] = basicType2; + msg.BasicTypesValues[2] = basicType3; bool received = false; - Subscription chatter_sub = node_array_2.CreateSubscription( - "topic_array", rcv_msg => + Subscription chatterSub = nodeArray2.CreateSubscription( + "topic_array", rcvMsg => { received = true; - msg2 = rcv_msg; + msg2 = rcvMsg; } ); while (!received) { - chatter_pub.Publish(msg); + chatterPub.Publish(msg); - RCLdotnet.SpinOnce(node_array_1, 500); - RCLdotnet.SpinOnce(node_array_2, 500); + RCLdotnet.SpinOnce(nodeArray1, 500); + RCLdotnet.SpinOnce(nodeArray2, 500); } // boolValues @@ -412,9 +412,9 @@ public void TestPublishArraysSizeCheckToMuch() public void TestPublishUnboundedSequences() { RCLdotnet.Init(); - Node node_array_1 = RCLdotnet.CreateNode("test_unbounded_sequences_1"); - Node node_array_2 = RCLdotnet.CreateNode("test_unbounded_sequences_2"); - Publisher chatter_pub = node_array_1.CreatePublisher("topic_unbounded_sequences"); + Node nodeArray1 = RCLdotnet.CreateNode("test_unbounded_sequences_1"); + Node nodeArray2 = RCLdotnet.CreateNode("test_unbounded_sequences_2"); + Publisher chatterPub = nodeArray1.CreatePublisher("topic_unbounded_sequences"); test_msgs.msg.UnboundedSequences msg = new test_msgs.msg.UnboundedSequences(); test_msgs.msg.UnboundedSequences msg2 = new test_msgs.msg.UnboundedSequences(); @@ -489,70 +489,70 @@ public void TestPublishUnboundedSequences() msg.StringValues.Add("two"); msg.StringValues.Add("three"); - test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.BoolValue = true; - basic_type_1.ByteValue = 36; - basic_type_1.CharValue = 37; - basic_type_1.Float32Value = 38.1f; - basic_type_1.Float64Value = 39.1; - basic_type_1.Int8Value = 40; - basic_type_1.Uint8Value = 41; - basic_type_1.Int16Value = 42; - basic_type_1.Uint16Value = 43; - basic_type_1.Int32Value = 44; - basic_type_1.Uint32Value = 45; - basic_type_1.Int64Value = 46; - basic_type_1.Uint64Value = 47; - - test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.BoolValue = false; - basic_type_2.ByteValue = 48; - basic_type_2.CharValue = 49; - basic_type_2.Float32Value = 50.1f; - basic_type_2.Float64Value = 51.1; - basic_type_2.Int8Value = 52; - basic_type_2.Uint8Value = 53; - basic_type_2.Int16Value = 54; - basic_type_2.Uint16Value = 55; - basic_type_2.Int32Value = 56; - basic_type_2.Uint32Value = 57; - basic_type_2.Int64Value = 58; - basic_type_2.Uint64Value = 59; - - test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.BoolValue = true; - basic_type_3.ByteValue = 60; - basic_type_3.CharValue = 61; - basic_type_3.Float32Value = 62.1f; - basic_type_3.Float64Value = 63.1; - basic_type_3.Int8Value = 64; - basic_type_3.Uint8Value = 65; - basic_type_3.Int16Value = 66; - basic_type_3.Uint16Value = 67; - basic_type_3.Int32Value = 68; - basic_type_3.Uint32Value = 69; - basic_type_3.Int64Value = 70; - basic_type_3.Uint64Value = 71; - - msg.BasicTypesValues.Add(basic_type_1); - msg.BasicTypesValues.Add(basic_type_2); - msg.BasicTypesValues.Add(basic_type_3); + test_msgs.msg.BasicTypes basicType1 = new test_msgs.msg.BasicTypes(); + basicType1.BoolValue = true; + basicType1.ByteValue = 36; + basicType1.CharValue = 37; + basicType1.Float32Value = 38.1f; + basicType1.Float64Value = 39.1; + basicType1.Int8Value = 40; + basicType1.Uint8Value = 41; + basicType1.Int16Value = 42; + basicType1.Uint16Value = 43; + basicType1.Int32Value = 44; + basicType1.Uint32Value = 45; + basicType1.Int64Value = 46; + basicType1.Uint64Value = 47; + + test_msgs.msg.BasicTypes basicType2 = new test_msgs.msg.BasicTypes(); + basicType2.BoolValue = false; + basicType2.ByteValue = 48; + basicType2.CharValue = 49; + basicType2.Float32Value = 50.1f; + basicType2.Float64Value = 51.1; + basicType2.Int8Value = 52; + basicType2.Uint8Value = 53; + basicType2.Int16Value = 54; + basicType2.Uint16Value = 55; + basicType2.Int32Value = 56; + basicType2.Uint32Value = 57; + basicType2.Int64Value = 58; + basicType2.Uint64Value = 59; + + test_msgs.msg.BasicTypes basicType3 = new test_msgs.msg.BasicTypes(); + basicType3.BoolValue = true; + basicType3.ByteValue = 60; + basicType3.CharValue = 61; + basicType3.Float32Value = 62.1f; + basicType3.Float64Value = 63.1; + basicType3.Int8Value = 64; + basicType3.Uint8Value = 65; + basicType3.Int16Value = 66; + basicType3.Uint16Value = 67; + basicType3.Int32Value = 68; + basicType3.Uint32Value = 69; + basicType3.Int64Value = 70; + basicType3.Uint64Value = 71; + + msg.BasicTypesValues.Add(basicType1); + msg.BasicTypesValues.Add(basicType2); + msg.BasicTypesValues.Add(basicType3); bool received = false; - Subscription chatter_sub = node_array_2.CreateSubscription( - "topic_unbounded_sequences", rcv_msg => + Subscription chatterSub = nodeArray2.CreateSubscription( + "topic_unbounded_sequences", rcvMsg => { received = true; - msg2 = rcv_msg; + msg2 = rcvMsg; } ); while (!received) { - chatter_pub.Publish(msg); + chatterPub.Publish(msg); - RCLdotnet.SpinOnce(node_array_1, 500); - RCLdotnet.SpinOnce(node_array_2, 500); + RCLdotnet.SpinOnce(nodeArray1, 500); + RCLdotnet.SpinOnce(nodeArray2, 500); } // boolValues @@ -687,9 +687,9 @@ public void TestPublishUnboundedSequences() public void TestPublishBoundedSequences() { RCLdotnet.Init(); - Node node_array_1 = RCLdotnet.CreateNode("test_bounded_sequences_1"); - Node node_array_2 = RCLdotnet.CreateNode("test_bounded_sequences_2"); - Publisher chatter_pub = node_array_1.CreatePublisher("topic_bounded_sequences"); + Node nodeBoundedSequences1 = RCLdotnet.CreateNode("test_bounded_sequences_1"); + Node nodeBoundedSequences2 = RCLdotnet.CreateNode("test_bounded_sequences_2"); + Publisher chatter_pub = nodeBoundedSequences1.CreatePublisher("topic_bounded_sequences"); test_msgs.msg.BoundedSequences msg = new test_msgs.msg.BoundedSequences(); test_msgs.msg.BoundedSequences msg2 = new test_msgs.msg.BoundedSequences(); @@ -764,61 +764,61 @@ public void TestPublishBoundedSequences() msg.StringValues.Add("two"); msg.StringValues.Add("three"); - test_msgs.msg.BasicTypes basic_type_1 = new test_msgs.msg.BasicTypes(); - basic_type_1.BoolValue = true; - basic_type_1.ByteValue = 36; - basic_type_1.CharValue = 37; - basic_type_1.Float32Value = 38.1f; - basic_type_1.Float64Value = 39.1; - basic_type_1.Int8Value = 40; - basic_type_1.Uint8Value = 41; - basic_type_1.Int16Value = 42; - basic_type_1.Uint16Value = 43; - basic_type_1.Int32Value = 44; - basic_type_1.Uint32Value = 45; - basic_type_1.Int64Value = 46; - basic_type_1.Uint64Value = 47; - - test_msgs.msg.BasicTypes basic_type_2 = new test_msgs.msg.BasicTypes(); - basic_type_2.BoolValue = false; - basic_type_2.ByteValue = 48; - basic_type_2.CharValue = 49; - basic_type_2.Float32Value = 50.1f; - basic_type_2.Float64Value = 51.1; - basic_type_2.Int8Value = 52; - basic_type_2.Uint8Value = 53; - basic_type_2.Int16Value = 54; - basic_type_2.Uint16Value = 55; - basic_type_2.Int32Value = 56; - basic_type_2.Uint32Value = 57; - basic_type_2.Int64Value = 58; - basic_type_2.Uint64Value = 59; - - test_msgs.msg.BasicTypes basic_type_3 = new test_msgs.msg.BasicTypes(); - basic_type_3.BoolValue = true; - basic_type_3.ByteValue = 60; - basic_type_3.CharValue = 61; - basic_type_3.Float32Value = 62.1f; - basic_type_3.Float64Value = 63.1; - basic_type_3.Int8Value = 64; - basic_type_3.Uint8Value = 65; - basic_type_3.Int16Value = 66; - basic_type_3.Uint16Value = 67; - basic_type_3.Int32Value = 68; - basic_type_3.Uint32Value = 69; - basic_type_3.Int64Value = 70; - basic_type_3.Uint64Value = 71; - - msg.BasicTypesValues.Add(basic_type_1); - msg.BasicTypesValues.Add(basic_type_2); - msg.BasicTypesValues.Add(basic_type_3); + test_msgs.msg.BasicTypes basicType1 = new test_msgs.msg.BasicTypes(); + basicType1.BoolValue = true; + basicType1.ByteValue = 36; + basicType1.CharValue = 37; + basicType1.Float32Value = 38.1f; + basicType1.Float64Value = 39.1; + basicType1.Int8Value = 40; + basicType1.Uint8Value = 41; + basicType1.Int16Value = 42; + basicType1.Uint16Value = 43; + basicType1.Int32Value = 44; + basicType1.Uint32Value = 45; + basicType1.Int64Value = 46; + basicType1.Uint64Value = 47; + + test_msgs.msg.BasicTypes basicType2 = new test_msgs.msg.BasicTypes(); + basicType2.BoolValue = false; + basicType2.ByteValue = 48; + basicType2.CharValue = 49; + basicType2.Float32Value = 50.1f; + basicType2.Float64Value = 51.1; + basicType2.Int8Value = 52; + basicType2.Uint8Value = 53; + basicType2.Int16Value = 54; + basicType2.Uint16Value = 55; + basicType2.Int32Value = 56; + basicType2.Uint32Value = 57; + basicType2.Int64Value = 58; + basicType2.Uint64Value = 59; + + test_msgs.msg.BasicTypes basicType3 = new test_msgs.msg.BasicTypes(); + basicType3.BoolValue = true; + basicType3.ByteValue = 60; + basicType3.CharValue = 61; + basicType3.Float32Value = 62.1f; + basicType3.Float64Value = 63.1; + basicType3.Int8Value = 64; + basicType3.Uint8Value = 65; + basicType3.Int16Value = 66; + basicType3.Uint16Value = 67; + basicType3.Int32Value = 68; + basicType3.Uint32Value = 69; + basicType3.Int64Value = 70; + basicType3.Uint64Value = 71; + + msg.BasicTypesValues.Add(basicType1); + msg.BasicTypesValues.Add(basicType2); + msg.BasicTypesValues.Add(basicType3); bool received = false; - Subscription chatter_sub = node_array_2.CreateSubscription( - "topic_bounded_sequences", rcv_msg => + Subscription chatterSub = nodeBoundedSequences2.CreateSubscription( + "topic_bounded_sequences", rcvMsg => { received = true; - msg2 = rcv_msg; + msg2 = rcvMsg; } ); @@ -826,8 +826,8 @@ public void TestPublishBoundedSequences() { chatter_pub.Publish(msg); - RCLdotnet.SpinOnce(node_array_1, 500); - RCLdotnet.SpinOnce(node_array_2, 500); + RCLdotnet.SpinOnce(nodeBoundedSequences1, 500); + RCLdotnet.SpinOnce(nodeBoundedSequences2, 500); } // boolValues diff --git a/rcldotnet_examples/RCLDotnetListener.cs b/rcldotnet_examples/RCLDotnetListener.cs index 62da2ed6..9a98330e 100644 --- a/rcldotnet_examples/RCLDotnetListener.cs +++ b/rcldotnet_examples/RCLDotnetListener.cs @@ -12,7 +12,7 @@ public static void Main(string[] args) Node node = RCLdotnet.CreateNode("listener"); - Subscription chatter_sub = node.CreateSubscription( + Subscription chatterSub = node.CreateSubscription( "chatter", msg => Console.WriteLine("I heard: [" + msg.Data + "]")); RCLdotnet.Spin(node); diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index 9208f2d7..89de127c 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -13,7 +13,7 @@ public static void Main(string[] args) Node node = RCLdotnet.CreateNode("talker"); - Publisher chatter_pub = node.CreatePublisher("chatter"); + Publisher chatterPub = node.CreatePublisher("chatter"); std_msgs.msg.String msg = new std_msgs.msg.String(); @@ -24,7 +24,7 @@ public static void Main(string[] args) msg.Data = "Hello World: " + i; i++; Console.WriteLine("Publishing: \"" + msg.Data + "\""); - chatter_pub.Publish(msg); + chatterPub.Publish(msg); // Sleep a little bit between each message Thread.Sleep(1000); From be8439dba656032be716703c202ba1fcf1746282 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 15 Jun 2022 15:52:07 +0200 Subject: [PATCH 37/73] codestyle: fix xunit waring --- rcldotnet/test/test_messages.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcldotnet/test/test_messages.cs b/rcldotnet/test/test_messages.cs index 39d27eb3..0b82d71e 100644 --- a/rcldotnet/test/test_messages.cs +++ b/rcldotnet/test/test_messages.cs @@ -998,7 +998,7 @@ public void TestDefaults() { var defaultsMsg = new test_msgs.msg.Defaults(); - Assert.Equal(true, defaultsMsg.BoolValue); + Assert.True(defaultsMsg.BoolValue); Assert.Equal(50, defaultsMsg.ByteValue); Assert.Equal(100, defaultsMsg.CharValue); Assert.Equal(1.125f, defaultsMsg.Float32Value); From 093c045b57faf03496f1a10335801941b5077118 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 17 Jun 2022 09:22:57 +0200 Subject: [PATCH 38/73] guard conditions: only build one native library --- rcldotnet/CMakeLists.txt | 40 +++++++++++++++++++--------------------- rcldotnet/Client.cs | 2 +- rcldotnet/Node.cs | 2 +- rcldotnet/Publisher.cs | 2 +- 4 files changed, 22 insertions(+), 24 deletions(-) diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index aeaa49d2..adc44e8e 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -62,29 +62,27 @@ add_dotnet_library(${PROJECT_NAME}_assemblies install_dotnet(${PROJECT_NAME}_assemblies DESTINATION lib/${PROJECT_NAME}/dotnet) ament_export_assemblies_dll("lib/${PROJECT_NAME}/dotnet/${PROJECT_NAME}_assemblies.dll") -set(_native_sources "rcldotnet;rcldotnet_node;rcldotnet_publisher;rcldotnet_client") +add_library(${PROJECT_NAME}_native SHARED + rcldotnet_client.c + rcldotnet_node.c + rcldotnet_publisher.c + rcldotnet.c +) -foreach(_target_name ${_native_sources}) - add_library(${_target_name} SHARED - ${_target_name}.c - ) - set_target_properties(${_target_name} - PROPERTIES - OUTPUT_NAME ${_target_name}_native) - ament_target_dependencies(${_target_name} - "builtin_interfaces" - "rcl" - "rosidl_generator_c" - "rosidl_typesupport_c" - ) - ament_export_libraries(${_target_name}_native) +ament_target_dependencies(${PROJECT_NAME}_native + "builtin_interfaces" + "rcl" + "rosidl_generator_c" + "rosidl_typesupport_c" +) - install(TARGETS ${_target_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - ) -endforeach() +ament_export_libraries(${PROJECT_NAME}_native) + +install(TARGETS ${PROJECT_NAME}_native + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) ament_export_dependencies(ament_cmake) ament_export_dependencies(builtin_interfaces) diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index d0710861..97bda29e 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -40,7 +40,7 @@ internal delegate RCLRet NativeRCLServiceServerIsAvailableType( static ClientDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); - IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet_client"); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); IntPtr native_rcl_send_request_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_send_request"); ClientDelegates.native_rcl_send_request = (NativeRCLSendRequestType)Marshal.GetDelegateForFunctionPointer( diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 8a3c585b..ee064b70 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -75,7 +75,7 @@ internal delegate RCLRet NativeRCLDestroyClientHandleType( static NodeDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); - IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet_node"); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); IntPtr native_rcl_create_publisher_handle_ptr = _dllLoadUtils.GetProcAddress( nativeLibrary, "native_rcl_create_publisher_handle"); diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index a0e55378..ef99966d 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -32,7 +32,7 @@ internal delegate RCLRet NativeRCLPublishType( static PublisherDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); - IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet_publisher"); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); IntPtr native_rcl_publish_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_publish"); PublisherDelegates.native_rcl_publish = (NativeRCLPublishType)Marshal.GetDelegateForFunctionPointer( From b7713c5084367a998e6e04a109b0990414c56110 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 17 Jun 2022 11:22:42 +0200 Subject: [PATCH 39/73] guard conditions: add guard conditions --- rcldotnet/CMakeLists.txt | 4 + rcldotnet/GuardCondition.cs | 71 ++++++++++++++ rcldotnet/Node.cs | 21 ++++- rcldotnet/RCLdotnet.cs | 120 +++++++++++++++++++----- rcldotnet/SafeGuardConditionHandle.cs | 41 ++++++++ rcldotnet/rcldotnet.c | 45 ++++++++- rcldotnet/rcldotnet.h | 12 +++ rcldotnet/rcldotnet_guard_condition.c | 35 +++++++ rcldotnet/rcldotnet_guard_condition.h | 23 +++++ rcldotnet/test/test_guard_conditions.cs | 85 +++++++++++++++++ 10 files changed, 429 insertions(+), 28 deletions(-) create mode 100644 rcldotnet/GuardCondition.cs create mode 100644 rcldotnet/SafeGuardConditionHandle.cs create mode 100644 rcldotnet/rcldotnet_guard_condition.c create mode 100644 rcldotnet/rcldotnet_guard_condition.h create mode 100644 rcldotnet/test/test_guard_conditions.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index adc44e8e..997df132 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -29,6 +29,7 @@ set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") set(CS_SOURCES Client.cs + GuardCondition.cs MessageStaticMemberCache.cs Node.cs Publisher.cs @@ -36,6 +37,7 @@ set(CS_SOURCES RCLExceptionHelper.cs RCLRet.cs SafeClientHandle.cs + SafeGuardConditionHandle.cs SafeNodeHandle.cs SafePublisherHandle.cs SafeRequestIdHandle.cs @@ -64,6 +66,7 @@ ament_export_assemblies_dll("lib/${PROJECT_NAME}/dotnet/${PROJECT_NAME}_assembli add_library(${PROJECT_NAME}_native SHARED rcldotnet_client.c + rcldotnet_guard_condition.c rcldotnet_node.c rcldotnet_publisher.c rcldotnet.c @@ -108,6 +111,7 @@ if(BUILD_TESTING) add_dotnet_test(test_messages ${CS_SOURCES} + test/test_guard_conditions.cs test/test_messages.cs test/test_services.cs INCLUDE_DLLS diff --git a/rcldotnet/GuardCondition.cs b/rcldotnet/GuardCondition.cs new file mode 100644 index 00000000..c444b940 --- /dev/null +++ b/rcldotnet/GuardCondition.cs @@ -0,0 +1,71 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 static class GuardConditionDelegates + { + internal static readonly DllLoadUtils _dllLoadUtils; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLTriggerGuardConditionType( + SafeGuardConditionHandle guardConditionHandle); + + internal static NativeRCLTriggerGuardConditionType native_rcl_trigger_guard_condition = null; + + static GuardConditionDelegates() + { + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); + + IntPtr native_rcl_trigger_guard_condition_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_trigger_guard_condition"); + GuardConditionDelegates.native_rcl_trigger_guard_condition = (NativeRCLTriggerGuardConditionType)Marshal.GetDelegateForFunctionPointer( + native_rcl_trigger_guard_condition_ptr, typeof(NativeRCLTriggerGuardConditionType)); + } + } + + public sealed class GuardCondition + { + private readonly Action _callback; + + internal GuardCondition(SafeGuardConditionHandle handle, Action callback) + { + Handle = handle; + _callback = callback; + } + + // GuardCondition does intentionally (for now) not implement IDisposable as this + // needs some extra consideration how the type works after its + // internal handle is disposed. + // By relying on the GC/Finalizer of SafeHandle the handle only gets + // Disposed if the publisher is not live anymore. + internal SafeGuardConditionHandle Handle { get; } + + public void Trigger() + { + RCLRet ret = GuardConditionDelegates.native_rcl_trigger_guard_condition(Handle); + RCLExceptionHelper.CheckReturnValue(ret); + } + + internal void TriggerCallback() + { + _callback(); + } + } +} diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index ee064b70..f6177d4e 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -137,19 +137,21 @@ static NodeDelegates() public sealed class Node { - private readonly IList _subscriptions; private readonly IList _services; private readonly IList _clients; + private readonly IList _guardConditions; + internal Node(SafeNodeHandle handle) { Handle = handle; _subscriptions = new List(); _services = new List(); _clients = new List(); + _guardConditions = new List(); } public IList Subscriptions => _subscriptions; @@ -161,6 +163,8 @@ internal Node(SafeNodeHandle handle) public IList Clients => _clients; + public IList GuardConditions => _guardConditions; + // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its // internal handle is disposed. @@ -248,5 +252,20 @@ public Client CreateClient /// Block until the wait set is ready or until the timeout has been exceeded. /// @@ -531,7 +583,7 @@ private static void SendResponse(Service service, SafeRequestIdHandle requestHea public static void SpinOnce(Node node, long timeout) { int numberOfSubscriptions = node.Subscriptions.Count; - int numberOfGuardConditions = 0; + int numberOfGuardConditions = node.GuardConditions.Count; int numberOfTimers = 0; int numberOfClients = node.Clients.Count; int numberOfServices = node.Services.Count; @@ -577,50 +629,66 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddClient(waitSetHandle, client.Handle); } + foreach (var guardCondition in node.GuardConditions) + { + WaitSetAddGuardCondition(waitSetHandle, guardCondition.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { return; // timeout } - } - foreach (Subscription subscription in node.Subscriptions) - { - IRosMessage message = subscription.CreateMessage(); - bool result = Take(subscription, message); - if (result) + foreach (Subscription subscription in node.Subscriptions) { - subscription.TriggerCallback(message); + IRosMessage message = subscription.CreateMessage(); + bool result = Take(subscription, message); + if (result) + { + subscription.TriggerCallback(message); + } } - } - // requestIdHandle gets reused for each element in the loop. - using (SafeRequestIdHandle requestIdHandle = CreateRequestId()) - { - foreach (var service in node.Services) + // requestIdHandle gets reused for each element in the loop. + using (SafeRequestIdHandle requestIdHandle = CreateRequestId()) { - var request = service.CreateRequest(); - var response = service.CreateResponse(); + foreach (var service in node.Services) + { + var request = service.CreateRequest(); + var response = service.CreateResponse(); - var result = TakeRequest(service, requestIdHandle, request); - if (result) + var result = TakeRequest(service, requestIdHandle, request); + if (result) + { + service.TriggerCallback(request, response); + + SendResponse(service, requestIdHandle, response); + } + } + + foreach (var client in node.Clients) { - service.TriggerCallback(request, response); + var response = client.CreateResponse(); - SendResponse(service, requestIdHandle, response); + var result = TakeResponse(client, requestIdHandle, response); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + client.HandleResponse(sequenceNumber, response); + } } } - foreach (var client in node.Clients) + int index = 0; + foreach (GuardCondition guardCondition in node.GuardConditions) { - var response = client.CreateResponse(); - - var result = TakeResponse(client, requestIdHandle, response); - if (result) + if (RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready(waitSetHandle, index)) { - var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); - client.HandleResponse(sequenceNumber, response); + guardCondition.TriggerCallback(); } + + index++; } } } diff --git a/rcldotnet/SafeGuardConditionHandle.cs b/rcldotnet/SafeGuardConditionHandle.cs new file mode 100644 index 00000000..3b9e8106 --- /dev/null +++ b/rcldotnet/SafeGuardConditionHandle.cs @@ -0,0 +1,41 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_guard_condition_t + /// + internal sealed class SafeGuardConditionHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeGuardConditionHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_destroy_guard_condition_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index ad955536..bbc20642 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -49,7 +49,7 @@ void native_rcl_get_error_string(char *buffer, int32_t bufferSize) { size_t minBufferSize = (size_t)bufferSize < (size_t)RCUTILS_ERROR_MESSAGE_MAX_LENGTH ? (size_t)bufferSize : (size_t)RCUTILS_ERROR_MESSAGE_MAX_LENGTH; - + strncpy(buffer, rcl_get_error_string().str, minBufferSize); } @@ -78,6 +78,30 @@ int32_t native_rcl_destroy_node_handle(void *node_handle) { return ret; } +int32_t native_rcl_create_guard_condition_handle(void **guard_condition_handle) { + rcl_guard_condition_t *guard_condition = + (rcl_guard_condition_t *)malloc(sizeof(rcl_guard_condition_t)); + *guard_condition = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_options_t guard_condition_ops = + rcl_guard_condition_get_default_options(); + + rcl_ret_t ret = + rcl_guard_condition_init(guard_condition, &context, guard_condition_ops); + + *guard_condition_handle = (void *)guard_condition; + + return ret; +} + +int32_t native_rcl_destroy_guard_condition_handle(void *guard_condition_handle) { + rcl_guard_condition_t *guard_condition = (rcl_guard_condition_t *)guard_condition_handle; + + rcl_ret_t ret = rcl_guard_condition_fini(guard_condition); + free(guard_condition); + + return ret; +} + int32_t native_rcl_create_wait_set_handle( void **wait_set_handle, int32_t number_of_subscriptions, @@ -139,6 +163,14 @@ int32_t native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handl return ret; } +int32_t native_rcl_wait_set_add_guard_condition(void *wait_set_handle, void *guard_condition_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_guard_condition_t *guard_condition = (rcl_guard_condition_t *)guard_condition_handle; + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition, NULL); + + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); @@ -146,6 +178,17 @@ int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { return ret; } +bool native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + + if (index >= wait_set->size_of_guard_conditions) + { + return false; + } + + return wait_set->guard_conditions[index] != NULL; +} + int32_t native_rcl_take(void *subscription_handle, void *message_handle) { rcl_subscription_t * subscription = (rcl_subscription_t *)subscription_handle; diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index c6283a94..de26c8bf 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -38,6 +38,12 @@ int32_t RCLDOTNET_CDECL native_rcl_create_node_handle(void **, const char *, con RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_destroy_node_handle(void *node_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_create_guard_condition_handle(void **guard_condition_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_guard_condition_handle(void *guard_condition_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_wait_set_handle( void **wait_set_handle, @@ -63,9 +69,15 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_service(void *wait_set_handle, v RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_guard_condition_handle(void *wait_set_handle, void *guard_condition_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); diff --git a/rcldotnet/rcldotnet_guard_condition.c b/rcldotnet/rcldotnet_guard_condition.c new file mode 100644 index 00000000..2441fb90 --- /dev/null +++ b/rcldotnet/rcldotnet_guard_condition.c @@ -0,0 +1,35 @@ +// Copyright 2022 Stefan Hoffmann +// +// 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 +#include + +#include +#include +#include +#include + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcldotnet_guard_condition.h" + +int32_t native_rcl_trigger_guard_condition(void *guard_condition_handle) +{ + rcl_guard_condition_t * guard_condition = (rcl_guard_condition_t *)guard_condition_handle; + + rcl_ret_t ret = rcl_trigger_guard_condition(guard_condition); + + return ret; +} diff --git a/rcldotnet/rcldotnet_guard_condition.h b/rcldotnet/rcldotnet_guard_condition.h new file mode 100644 index 00000000..cf99e3c9 --- /dev/null +++ b/rcldotnet/rcldotnet_guard_condition.h @@ -0,0 +1,23 @@ +// Copyright 2022 Stefan Hoffmann +// +// 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_GUARD_CONDITION_H +#define RCLDOTNET_GUARD_CONDITION_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_trigger_guard_condition(void *guard_condition); + +#endif // RCLDOTNET_GUARD_CONDITION_H diff --git a/rcldotnet/test/test_guard_conditions.cs b/rcldotnet/test/test_guard_conditions.cs new file mode 100644 index 00000000..15d590b8 --- /dev/null +++ b/rcldotnet/test/test_guard_conditions.cs @@ -0,0 +1,85 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading.Tasks; +using ROS2; +using Xunit; + +namespace RCLdotnetTests +{ + public sealed class TestGuardConditions + { + [Fact] + public void TestGuardCondition() + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("guard_condition"); + + int guardConditionTriggeredCount = 0; + + var guardCondition = node.CreateGuardCondition(HandleGuardCondition); + + // spin once without trigger + RCLdotnet.SpinOnce(node, 50); + Assert.Equal(0, guardConditionTriggeredCount); + + // spin once with trigger + guardCondition.Trigger(); + RCLdotnet.SpinOnce(node, 50); + Assert.Equal(1, guardConditionTriggeredCount); + + // spin once without trigger + RCLdotnet.SpinOnce(node, 50); + Assert.Equal(1, guardConditionTriggeredCount); + + void HandleGuardCondition() + { + guardConditionTriggeredCount++; + } + } + + [Fact] + public void TestGuardConditionFromBackgroundWorkerThread() + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("guard_condition"); + + int guardConditionTriggeredCount = 0; + + var guardCondition = node.CreateGuardCondition(HandleGuardCondition); + + TriggerAfterDelay(); + + // spin once and wait for trigger + RCLdotnet.SpinOnce(node, 1000); + Assert.Equal(1, guardConditionTriggeredCount); + + // spin once without trigger + RCLdotnet.SpinOnce(node, 50); + Assert.Equal(1, guardConditionTriggeredCount); + + void HandleGuardCondition() + { + guardConditionTriggeredCount++; + } + + async void TriggerAfterDelay() + { + await Task.Delay(10).ConfigureAwait(false); + guardCondition.Trigger(); + } + } + } +} From 664ee0b3d1c039be4bc7f92719cc4eb7e43d955f Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 21 Jun 2022 10:24:57 +0200 Subject: [PATCH 40/73] guard conditions: only try to take waitables if they are ready This avoids doing unnecessary allocations of native ROS messages only to check that there is no new data. This might be a good thing to do right now as with guard_conditions there could be probably more times the waitset is ready to fire some callbacks, so work should be minimized here especially. --- rcldotnet/RCLdotnet.cs | 91 ++++++++++++++++++++++++++++++++---------- rcldotnet/rcldotnet.c | 33 +++++++++++++++ rcldotnet/rcldotnet.h | 9 +++++ 3 files changed, 113 insertions(+), 20 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index b197d7a7..4f0079a0 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -116,6 +116,21 @@ internal delegate RCLRet NativeRCLCreateWaitSetHandleType( internal static NativeRCLWaitType native_rcl_wait = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate bool NativeRCLWaitSetSubscriptionReady(SafeWaitSetHandle waitSetHandle, int index); + + internal static NativeRCLWaitSetSubscriptionReady native_rcl_wait_set_subscription_ready = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate bool NativeRCLWaitSetClientReady(SafeWaitSetHandle waitSetHandle, int index); + + internal static NativeRCLWaitSetClientReady native_rcl_wait_set_client_ready = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate bool NativeRCLWaitSetServiceReady(SafeWaitSetHandle waitSetHandle, int index); + + internal static NativeRCLWaitSetServiceReady native_rcl_wait_set_service_ready = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate bool NativeRCLWaitSetGuardConditionReady(SafeWaitSetHandle waitSetHandle, int index); @@ -264,6 +279,24 @@ static RCLdotnetDelegates() (NativeRCLWaitType)Marshal.GetDelegateForFunctionPointer( native_rcl_wait_ptr, typeof(NativeRCLWaitType)); + IntPtr native_rcl_wait_set_subscription_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_subscription_ready"); + RCLdotnetDelegates.native_rcl_wait_set_subscription_ready = + (NativeRCLWaitSetSubscriptionReady)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_subscription_ready_ptr, typeof(NativeRCLWaitSetSubscriptionReady)); + + IntPtr native_rcl_wait_set_client_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_client_ready"); + RCLdotnetDelegates.native_rcl_wait_set_client_ready = + (NativeRCLWaitSetClientReady)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_client_ready_ptr, typeof(NativeRCLWaitSetClientReady)); + + IntPtr native_rcl_wait_set_service_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_service_ready"); + RCLdotnetDelegates.native_rcl_wait_set_service_ready = + (NativeRCLWaitSetServiceReady)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_service_ready_ptr, typeof(NativeRCLWaitSetServiceReady)); + IntPtr native_rcl_wait_set_guard_condition_ready_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_guard_condition_ready"); RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready = @@ -640,55 +673,73 @@ public static void SpinOnce(Node node, long timeout) return; // timeout } + int subscriptionIndex = 0; foreach (Subscription subscription in node.Subscriptions) { - IRosMessage message = subscription.CreateMessage(); - bool result = Take(subscription, message); - if (result) + if (RCLdotnetDelegates.native_rcl_wait_set_subscription_ready(waitSetHandle, subscriptionIndex)) { - subscription.TriggerCallback(message); + IRosMessage message = subscription.CreateMessage(); + bool result = Take(subscription, message); + if (result) + { + subscription.TriggerCallback(message); + } } + + subscriptionIndex++; } // requestIdHandle gets reused for each element in the loop. using (SafeRequestIdHandle requestIdHandle = CreateRequestId()) { + int serviceIndex = 0; foreach (var service in node.Services) { - var request = service.CreateRequest(); - var response = service.CreateResponse(); - - var result = TakeRequest(service, requestIdHandle, request); - if (result) + if (RCLdotnetDelegates.native_rcl_wait_set_service_ready(waitSetHandle, serviceIndex)) { - service.TriggerCallback(request, response); + var request = service.CreateRequest(); + var response = service.CreateResponse(); - SendResponse(service, requestIdHandle, response); + var result = TakeRequest(service, requestIdHandle, request); + if (result) + { + service.TriggerCallback(request, response); + + SendResponse(service, requestIdHandle, response); + } } + + serviceIndex++; } + int clientIndex = 0; foreach (var client in node.Clients) { - var response = client.CreateResponse(); - - var result = TakeResponse(client, requestIdHandle, response); - if (result) + if (RCLdotnetDelegates.native_rcl_wait_set_client_ready(waitSetHandle, clientIndex)) { - var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); - client.HandleResponse(sequenceNumber, response); + var response = client.CreateResponse(); + + var result = TakeResponse(client, requestIdHandle, response); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + client.HandleResponse(sequenceNumber, response); + } } + + clientIndex++; } } - int index = 0; + int guardConditionIndex = 0; foreach (GuardCondition guardCondition in node.GuardConditions) { - if (RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready(waitSetHandle, index)) + if (RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready(waitSetHandle, guardConditionIndex)) { guardCondition.TriggerCallback(); } - index++; + guardConditionIndex++; } } } diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index bbc20642..cee09a14 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -178,6 +178,39 @@ int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { return ret; } +bool native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + + if (index >= wait_set->size_of_subscriptions) + { + return false; + } + + return wait_set->subscriptions[index] != NULL; +} + +bool native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + + if (index >= wait_set->size_of_clients) + { + return false; + } + + return wait_set->clients[index] != NULL; +} + +bool native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + + if (index >= wait_set->size_of_services) + { + return false; + } + + return wait_set->services[index] != NULL; +} + bool native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index de26c8bf..6869fffa 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -75,6 +75,15 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_guard_condition_handle(void *wai RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index); + +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index); + +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index); + RCLDOTNET_EXPORT bool RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index); From adebb261624f84b6a92cc3a3f4211b2eaa7d4177 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 21 Jun 2022 10:58:09 +0200 Subject: [PATCH 41/73] actions: initial action idl generation --- rcldotnet_common/CMakeLists.txt | 1 + rcldotnet_common/IRosActionDefinition.cs | 27 +++++++ ...generator_dotnet_generate_interfaces.cmake | 20 ++++- rosidl_generator_dotnet/resource/action.c.em | 8 ++ rosidl_generator_dotnet/resource/action.cs.em | 61 ++++++++++++++++ rosidl_generator_dotnet/resource/action.h.em | 33 +++++++++ rosidl_generator_dotnet/resource/idl.c.em | 73 ++++++++++++++++++- rosidl_generator_dotnet/resource/idl.cs.em | 73 ++++++++++++++++++- rosidl_generator_dotnet/resource/idl.h.em | 71 +++++++++++++++++- 9 files changed, 360 insertions(+), 7 deletions(-) create mode 100644 rcldotnet_common/IRosActionDefinition.cs create mode 100644 rosidl_generator_dotnet/resource/action.c.em create mode 100644 rosidl_generator_dotnet/resource/action.cs.em create mode 100644 rosidl_generator_dotnet/resource/action.h.em diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index 246c87b2..e97e99b2 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(DotNETExtra REQUIRED) set(CS_SOURCES DllLoadUtils.cs + IRosActionDefinition.cs IRosMessage.cs IRosServiceDefinition.cs ) diff --git a/rcldotnet_common/IRosActionDefinition.cs b/rcldotnet_common/IRosActionDefinition.cs new file mode 100644 index 00000000..7fcf0104 --- /dev/null +++ b/rcldotnet_common/IRosActionDefinition.cs @@ -0,0 +1,27 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + public interface IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // must be implemented on deriving types, gets called via reflection + // (static abstract interface members are not supported yet.) + // public static abstract IntPtr __GetTypeSupport(); + } +} diff --git a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake index 617654ce..52e9aadc 100644 --- a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake +++ b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake @@ -68,6 +68,17 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_module_name}.c" ) elseif(_parent_folder STREQUAL "action") + list(APPEND _generated_cs_files + "${_output_path}/${_parent_folder}/${_module_name}.cs" + ) + + list(APPEND _generated_h_files + "${_output_path}/${_parent_folder}/rcldotnet_${_module_name}.h" + ) + + list(APPEND _generated_c_ts_files + "${_output_path}/${_parent_folder}/${_module_name}.c" + ) else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") endif() @@ -87,13 +98,18 @@ endforeach() set(target_dependencies "${rosidl_generator_dotnet_BIN}" ${rosidl_generator_dotnet_GENERATOR_FILES} - "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.c.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.h.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.cs.em" - "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.h.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.h.em" ${rosidl_generate_interfaces_ABS_IDL_FILES} ${_dependency_files}) foreach(dep ${target_dependencies}) diff --git a/rosidl_generator_dotnet/resource/action.c.em b/rosidl_generator_dotnet/resource/action.c.em new file mode 100644 index 00000000..a9b7843d --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.c.em @@ -0,0 +1,8 @@ +@{ +type_name = action.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +}@ +const void * @(action_typename)__get_typesupport(void) { + const void * ptr = ROSIDL_TYPESUPPORT_INTERFACE__ACTION_SYMBOL_NAME(rosidl_typesupport_c, @(', '.join(action.namespaced_type.namespaced_name())))(); + return ptr; +} diff --git a/rosidl_generator_dotnet/resource/action.cs.em b/rosidl_generator_dotnet/resource/action.cs.em new file mode 100644 index 00000000..7c930458 --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.cs.em @@ -0,0 +1,61 @@ +@{ +from rosidl_generator_dotnet import get_field_name +from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_builtin_dotnet_type +from rosidl_generator_dotnet import constant_value_to_dotnet + +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractString +from rosidl_parser.definition import AbstractWString +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import Array +from rosidl_parser.definition import BasicType +from rosidl_parser.definition import NamespacedType + +type_name = action.namespaced_type.name +goal_type_name = action.goal.structure.namespaced_type.name +result_type_name = action.result.structure.namespaced_type.name +feedback_type_name = action.feedback.structure.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +} +namespace @('.'.join(action.namespaced_type.namespaces)) +{ +@# sealed class with private constructor -> no instance can be created +@# static classes can't implement an interface (or any other basetype), +@# but we need some type that can hold the typesupport and be passed to the Node.CreateActionServce/Client() method. +@# So sealed + private constructor is as static as it gets. +@# static abstract interface members are currently in preview, so maybe we could use the feature in the future. +@# (if hey add support to derive from static only interfaces in static classes) +@# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. + public sealed class @(type_name) : global::ROS2.IRosActionDefinition<@(goal_type_name), @(result_type_name), @(feedback_type_name)> + { + private static readonly DllLoadUtils dllLoadUtils; + + private @(type_name)() + { + } + + static @(type_name)() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativelibrary = dllLoadUtils.LoadLibrary("@(package_name)__dotnetext"); + + IntPtr native_get_typesupport_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(action_typename)__get_typesupport"); + + @(type_name).native_get_typesupport = (NativeGetTypeSupportType)Marshal.GetDelegateForFunctionPointer( + native_get_typesupport_ptr, typeof(NativeGetTypeSupportType)); + } + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + private delegate IntPtr NativeGetTypeSupportType(); + + private static NativeGetTypeSupportType native_get_typesupport = null; + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static IntPtr __GetTypeSupport() { + return native_get_typesupport(); + } + } +} diff --git a/rosidl_generator_dotnet/resource/action.h.em b/rosidl_generator_dotnet/resource/action.h.em new file mode 100644 index 00000000..dab48679 --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.h.em @@ -0,0 +1,33 @@ +@{ +type_name = action.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +action_prefix = "RCLDOTNET_{0}_{1}_{2}".format(package_name, '_'.join(action.namespaced_type.namespaces), type_name).upper() +}@ +#if defined(_MSC_VER) + // Microsoft + #define @(action_prefix)_EXPORT __declspec(dllexport) + #define @(action_prefix)_IMPORT __declspec(dllimport) + #if defined(_M_IX86) + #define @(action_prefix)_CDECL __cdecl + #else + #define @(action_prefix)_CDECL + #endif +#elif defined(__GNUC__) + // GCC + #define @(action_prefix)_EXPORT __attribute__((visibility("default"))) + #define @(action_prefix)_IMPORT + #if defined(__i386__) + #define @(action_prefix)_CDECL __attribute__((__cdecl__)) + #else + #define @(action_prefix)_CDECL + #endif +#else + // do nothing and hope for the best? + #define @(action_prefix)_EXPORT + #define @(action_prefix)_IMPORT + #define @(action_prefix)_CDECL + #pragma warning Unknown dynamic link import/export semantics. +#endif + +@(action_prefix)_EXPORT +const void * @(action_prefix)_CDECL @(action_typename)__get_typesupport(void); diff --git a/rosidl_generator_dotnet/resource/idl.c.em b/rosidl_generator_dotnet/resource/idl.c.em index 14efb40c..2b2f4231 100644 --- a/rosidl_generator_dotnet/resource/idl.c.em +++ b/rosidl_generator_dotnet/resource/idl.c.em @@ -87,5 +87,74 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@#TODO - actions not implemented -@ \ No newline at end of file +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.c.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ +@ diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index 3cfc1ede..1f18e48d 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -62,5 +62,74 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@#TODO - actions not implemented -@ \ No newline at end of file +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.cs.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ +@ diff --git a/rosidl_generator_dotnet/resource/idl.h.em b/rosidl_generator_dotnet/resource/idl.h.em index cf1fc096..e1439b98 100644 --- a/rosidl_generator_dotnet/resource/idl.h.em +++ b/rosidl_generator_dotnet/resource/idl.h.em @@ -68,7 +68,76 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@# TODO - actions not implemented +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.h.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.h.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.h.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ @ #endif // @(header_guard) From 2aa60cee683bd094ad4292239381dbcd8a4a2ad6 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 21 Jun 2022 13:48:39 +0200 Subject: [PATCH 42/73] actions: extend idl generation to include interfaces for "action wrapper types" With "action wrapper types" this means the ROS messages and services that add the `GoalId`, `TimeStamp` or other fields on top of the user defined types for the action. The introduced methods in `IRosActionDefinition` are needed to work around the missing existential type support in C# (https://github.com/dotnet/csharplang/issues/5556). --- rcldotnet_common/CMakeLists.txt | 5 ++ rcldotnet_common/IRosActionDefinition.cs | 15 ++++ rcldotnet_common/IRosActionFeedbackMessage.cs | 26 ++++++ .../IRosActionGetResultRequest.cs | 23 +++++ .../IRosActionGetResultResponse.cs | 25 ++++++ rcldotnet_common/IRosActionSendGoalRequest.cs | 42 +++++++++ .../IRosActionSendGoalResponse.cs | 25 ++++++ rosidl_generator_dotnet/resource/action.cs.em | 90 ++++++++++++++++++- rosidl_generator_dotnet/resource/idl.cs.em | 23 +++-- rosidl_generator_dotnet/resource/msg.cs.em | 14 ++- rosidl_generator_dotnet/resource/srv.cs.em | 11 ++- .../rosidl_generator_dotnet/__init__.py | 5 ++ 12 files changed, 288 insertions(+), 16 deletions(-) create mode 100644 rcldotnet_common/IRosActionFeedbackMessage.cs create mode 100644 rcldotnet_common/IRosActionGetResultRequest.cs create mode 100644 rcldotnet_common/IRosActionGetResultResponse.cs create mode 100644 rcldotnet_common/IRosActionSendGoalRequest.cs create mode 100644 rcldotnet_common/IRosActionSendGoalResponse.cs diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index e97e99b2..eb2f4387 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -11,6 +11,11 @@ find_package(DotNETExtra REQUIRED) set(CS_SOURCES DllLoadUtils.cs IRosActionDefinition.cs + IRosActionFeedbackMessage.cs + IRosActionGetResultRequest.cs + IRosActionGetResultResponse.cs + IRosActionSendGoalRequest.cs + IRosActionSendGoalResponse.cs IRosMessage.cs IRosServiceDefinition.cs ) diff --git a/rcldotnet_common/IRosActionDefinition.cs b/rcldotnet_common/IRosActionDefinition.cs index 7fcf0104..470df3db 100644 --- a/rcldotnet_common/IRosActionDefinition.cs +++ b/rcldotnet_common/IRosActionDefinition.cs @@ -23,5 +23,20 @@ public interface IRosActionDefinition // must be implemented on deriving types, gets called via reflection // (static abstract interface members are not supported yet.) // public static abstract IntPtr __GetTypeSupport(); + + // public static abstract IRosActionSendGoalRequest __CreateSendGoalRequest(); + // public static abstract SafeHandle __CreateSendGoalRequestHandle(); + + // public static abstract IRosActionSendGoalResponse __CreateSendGoalResponse(); + // public static abstract SafeHandle __CreateSendGoalResponseHandle(); + + // public static abstract IRosActionGetResultRequest __CreateGetResultRequest(); + // public static abstract SafeHandle __CreateGetResultRequestHandle(); + + // public static abstract IRosActionGetResultResponse __CreateGetResultResponse(); + // public static abstract SafeHandle __CreateGetResultResponseHandle(); + + // public static abstract IRosActionFeedbackMessage __CreateFeedbackMessage(); + // public static abstract SafeHandle __CreateFeedbackMessageHandle(); } } diff --git a/rcldotnet_common/IRosActionFeedbackMessage.cs b/rcldotnet_common/IRosActionFeedbackMessage.cs new file mode 100644 index 00000000..819b11cf --- /dev/null +++ b/rcldotnet_common/IRosActionFeedbackMessage.cs @@ -0,0 +1,26 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + public interface IRosActionFeedbackMessage : IRosMessage + where TFeedback : IRosMessage, new() + { + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + TFeedback Feedback { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionGetResultRequest.cs b/rcldotnet_common/IRosActionGetResultRequest.cs new file mode 100644 index 00000000..25425a07 --- /dev/null +++ b/rcldotnet_common/IRosActionGetResultRequest.cs @@ -0,0 +1,23 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + public interface IRosActionGetResultRequest : IRosMessage + { + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionGetResultResponse.cs b/rcldotnet_common/IRosActionGetResultResponse.cs new file mode 100644 index 00000000..283ab298 --- /dev/null +++ b/rcldotnet_common/IRosActionGetResultResponse.cs @@ -0,0 +1,25 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + public interface IRosActionGetResultResponse : IRosMessage + where TResult : IRosMessage, new() + { + sbyte Status { get; set; } + + TResult Result { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionSendGoalRequest.cs b/rcldotnet_common/IRosActionSendGoalRequest.cs new file mode 100644 index 00000000..0bd87079 --- /dev/null +++ b/rcldotnet_common/IRosActionSendGoalRequest.cs @@ -0,0 +1,42 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + public interface IRosActionSendGoalRequest : IRosMessage + where TGoal : IRosMessage, new() + { + // NOTICE: This would cause a cyclic reference: + // + // - `unique_identifier_msgs.msg.UUID` in the `unique_identifier_msgs` + // assembly references `IRosMessage` in the `rcldotnet_common` + // assembly. + // - `IRosActionSendGoalRequest` in the `rcldotnet_common` + // assembly references `unique_identifier_msgs.msg.UUID` in the + // `unique_identifier_msgs` assembly. + // + // So we need a workaround: + // - Use reflection later on to get to this. + // - Or use types like `byte[]` (or ValueTuple for + // `builtin_interfaces.msg.Time`) and generate accessor methods that + // convert and use those types. + // - Or provide property `IRosMessage GoalIdRosMessage { get; set; }` + // and cast to the concrete type on usage. + // + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + TGoal Goal { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionSendGoalResponse.cs b/rcldotnet_common/IRosActionSendGoalResponse.cs new file mode 100644 index 00000000..535d14b3 --- /dev/null +++ b/rcldotnet_common/IRosActionSendGoalResponse.cs @@ -0,0 +1,25 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + public interface IRosActionSendGoalResponse : IRosMessage + { + bool Accepted { get; set; } + + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // builtin_interfaces.msg.Time Stamp { get; set; } + } +} diff --git a/rosidl_generator_dotnet/resource/action.cs.em b/rosidl_generator_dotnet/resource/action.cs.em index 7c930458..36bdb2bf 100644 --- a/rosidl_generator_dotnet/resource/action.cs.em +++ b/rosidl_generator_dotnet/resource/action.cs.em @@ -1,6 +1,7 @@ @{ from rosidl_generator_dotnet import get_field_name from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_dotnet_type_for_message from rosidl_generator_dotnet import get_builtin_dotnet_type from rosidl_generator_dotnet import constant_value_to_dotnet @@ -14,9 +15,17 @@ from rosidl_parser.definition import BasicType from rosidl_parser.definition import NamespacedType type_name = action.namespaced_type.name -goal_type_name = action.goal.structure.namespaced_type.name -result_type_name = action.result.structure.namespaced_type.name -feedback_type_name = action.feedback.structure.namespaced_type.name + +goal_type_name = get_dotnet_type_for_message(action.goal) +result_type_name = get_dotnet_type_for_message(action.result) +feedback_type_name = get_dotnet_type_for_message(action.feedback) + +send_goal_request_type_name = get_dotnet_type_for_message(action.send_goal_service.request_message) +send_goal_response_type_name = get_dotnet_type_for_message(action.send_goal_service.response_message) +get_result_request_type_name = get_dotnet_type_for_message(action.get_result_service.request_message) +get_result_response_type_name = get_dotnet_type_for_message(action.get_result_service.response_message) +feedback_message_type_name = get_dotnet_type_for_message(action.feedback_message) + action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) } namespace @('.'.join(action.namespaced_type.namespaces)) @@ -28,7 +37,10 @@ namespace @('.'.join(action.namespaced_type.namespaces)) @# static abstract interface members are currently in preview, so maybe we could use the feature in the future. @# (if hey add support to derive from static only interfaces in static classes) @# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. - public sealed class @(type_name) : global::ROS2.IRosActionDefinition<@(goal_type_name), @(result_type_name), @(feedback_type_name)> + public sealed class @(type_name) : global::ROS2.IRosActionDefinition< + global::@(goal_type_name), + global::@(result_type_name), + global::@(feedback_type_name)> { private static readonly DllLoadUtils dllLoadUtils; @@ -57,5 +69,75 @@ namespace @('.'.join(action.namespaced_type.namespaces)) public static IntPtr __GetTypeSupport() { return native_get_typesupport(); } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionSendGoalRequest __CreateSendGoalRequest() + { + return new global::@(send_goal_request_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateSendGoalRequestHandle() + { + return global::@(send_goal_request_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionSendGoalResponse __CreateSendGoalResponse() + { + return new global::@(send_goal_response_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateSendGoalResponseHandle() + { + return global::@(send_goal_response_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionGetResultRequest __CreateGetResultRequest() + { + return new global::@(get_result_request_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateGetResultRequestHandle() + { + return global::@(get_result_request_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionGetResultResponse __CreateGetResultResponse() + { + return new global::@(get_result_response_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateGetResultResponseHandle() + { + return global::@(get_result_response_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionFeedbackMessage __CreateFeedbackMessage() + { + return new global::@(feedback_message_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateFeedbackMessageHandle() + { + return global::@(feedback_message_type_name).__CreateMessageHandle(); + } } } diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index 1f18e48d..18a9b314 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -1,6 +1,9 @@ // generated from rosidl_generator_dotnet/resource/idl.cs.em // with input from @(package_name):@(interface_path) // generated code does not contain a copyright notice +@{ +from rosidl_generator_dotnet import get_dotnet_type_for_message +}@ @ @####################################################################### @# EmPy template for generating .cs files @@ -87,13 +90,17 @@ TEMPLATE( @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message, + action_interface='global::ROS2.IRosActionSendGoalRequest' % get_dotnet_type_for_message(action.goal) +) }@ @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message, + action_interface='global::ROS2.IRosActionSendGoalResponse' +) }@ @{ @@ -105,13 +112,17 @@ TEMPLATE( @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message, + action_interface='global::ROS2.IRosActionGetResultRequest' +) }@ @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message, + action_interface='global::ROS2.IRosActionGetResultResponse' % get_dotnet_type_for_message(action.result) +) }@ @{ @@ -123,7 +134,9 @@ TEMPLATE( @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.feedback_message) + package_name=package_name, interface_path=interface_path, message=action.feedback_message, + action_interface='global::ROS2.IRosActionFeedbackMessage' % get_dotnet_type_for_message(action.feedback) +) }@ @{ diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 69350cde..d07c9c43 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -17,11 +17,17 @@ from rosidl_parser.definition import NamespacedType type_name = message.structure.namespaced_type.name msg_typename = '%s__%s' % ('__'.join(message.structure.namespaced_type.namespaces), type_name) + +if 'action_interface' not in locals(): + action_interface = None + +additional_interfaces_str = ', ' + action_interface if action_interface is not None else '' + } namespace @('.'.join(message.structure.namespaced_type.namespaces)) { -public class @(type_name) : global::ROS2.IRosMessage { +public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) { private static readonly DllLoadUtils dllLoadUtils; @[for member in message.structure.members]@ @@ -277,7 +283,7 @@ public class @(type_name) : global::ROS2.IRosMessage { IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @(get_field_name(type_name, member.name))[i__local_variable] = Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name))); @[ elif isinstance(member.type.value_type, AbstractWString)]@ - // TODO: Unicode types are not supported + // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name))[i__local_variable] = new @(get_dotnet_type(member.type.value_type))(); @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @@ -296,7 +302,7 @@ public class @(type_name) : global::ROS2.IRosMessage { IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @(get_field_name(type_name, member.name)).Add(Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name)))); @[ elif isinstance(member.type.value_type, AbstractWString)]@ - // TODO: Unicode types are not supported + // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name)).Add(new @(get_dotnet_type(member.type.value_type))()); @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @@ -348,7 +354,7 @@ public class @(type_name) : global::ROS2.IRosMessage { @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable), value__local_variable); @[ elif isinstance(member.type.value_type, AbstractWString)] -// TODO: Unicode types are not supported +// TODO: Unicode types are not supported @[ else]@ value__local_variable.__WriteToHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if]@ diff --git a/rosidl_generator_dotnet/resource/srv.cs.em b/rosidl_generator_dotnet/resource/srv.cs.em index bd541280..8231e40f 100644 --- a/rosidl_generator_dotnet/resource/srv.cs.em +++ b/rosidl_generator_dotnet/resource/srv.cs.em @@ -1,6 +1,7 @@ @{ from rosidl_generator_dotnet import get_field_name from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_dotnet_type_for_message from rosidl_generator_dotnet import get_builtin_dotnet_type from rosidl_generator_dotnet import constant_value_to_dotnet @@ -14,8 +15,10 @@ from rosidl_parser.definition import BasicType from rosidl_parser.definition import NamespacedType type_name = service.namespaced_type.name -request_type_name = service.request_message.structure.namespaced_type.name -response_type_name = service.response_message.structure.namespaced_type.name + +request_type_name = get_dotnet_type_for_message(service.request_message) +response_type_name = get_dotnet_type_for_message(service.response_message) + srv_typename = '%s__%s' % ('__'.join(service.namespaced_type.namespaces), type_name) } namespace @('.'.join(service.namespaced_type.namespaces)) @@ -27,7 +30,9 @@ namespace @('.'.join(service.namespaced_type.namespaces)) @# static abstract interface members are currently in preview, so maybe we could use the feature in the future. @# (if hey add support to derive from static only interfaces in static classes) @# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. - public sealed class @(type_name) : global::ROS2.IRosServiceDefinition<@(request_type_name), @(response_type_name)> + public sealed class @(type_name) : global::ROS2.IRosServiceDefinition< + global::@(request_type_name), + global::@(response_type_name)> { private static readonly DllLoadUtils dllLoadUtils; diff --git a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py index 95310ecc..39b67272 100644 --- a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py +++ b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py @@ -142,6 +142,11 @@ def get_dotnet_type(type_, use_primitives=True): return get_builtin_dotnet_type(type_.typename, use_primitives=use_primitives) +def get_dotnet_type_for_message(message): + return '%s.%s' % ( + '.'.join(message.structure.namespaced_type.namespaces), + message.structure.namespaced_type.name) + def msg_type_to_c(type_): if isinstance(type_, AbstractString): return 'char *' From 37845b06c76620103c44fe6061ee27ebba8030b6 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 22 Jun 2022 17:12:29 +0200 Subject: [PATCH 43/73] actions: add accessors to `UUID` and `Time` properties for "action wrapper types" This uses the base type `IMessage` to get around the cyclic references. --- rcldotnet_common/IRosActionFeedbackMessage.cs | 4 ++++ .../IRosActionGetResultRequest.cs | 4 ++++ rcldotnet_common/IRosActionSendGoalRequest.cs | 7 +++++++ .../IRosActionSendGoalResponse.cs | 4 ++++ rosidl_generator_dotnet/resource/msg.cs.em | 19 +++++++++++++++++++ 5 files changed, 38 insertions(+) diff --git a/rcldotnet_common/IRosActionFeedbackMessage.cs b/rcldotnet_common/IRosActionFeedbackMessage.cs index 819b11cf..95ce0a5a 100644 --- a/rcldotnet_common/IRosActionFeedbackMessage.cs +++ b/rcldotnet_common/IRosActionFeedbackMessage.cs @@ -21,6 +21,10 @@ public interface IRosActionFeedbackMessage : IRosMessage // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` // unique_identifier_msgs.msg.UUID GoalId { get; set; } + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } + TFeedback Feedback { get; set; } } } diff --git a/rcldotnet_common/IRosActionGetResultRequest.cs b/rcldotnet_common/IRosActionGetResultRequest.cs index 25425a07..6313d6f3 100644 --- a/rcldotnet_common/IRosActionGetResultRequest.cs +++ b/rcldotnet_common/IRosActionGetResultRequest.cs @@ -19,5 +19,9 @@ public interface IRosActionGetResultRequest : IRosMessage { // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } } } diff --git a/rcldotnet_common/IRosActionSendGoalRequest.cs b/rcldotnet_common/IRosActionSendGoalRequest.cs index 0bd87079..52af436c 100644 --- a/rcldotnet_common/IRosActionSendGoalRequest.cs +++ b/rcldotnet_common/IRosActionSendGoalRequest.cs @@ -35,8 +35,15 @@ public interface IRosActionSendGoalRequest : IRosMessage // - Or provide property `IRosMessage GoalIdRosMessage { get; set; }` // and cast to the concrete type on usage. // + // The later one was chosen as it gives the most control over object + // references and avoids using of reflection. + // // unique_identifier_msgs.msg.UUID GoalId { get; set; } + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } + TGoal Goal { get; set; } } } diff --git a/rcldotnet_common/IRosActionSendGoalResponse.cs b/rcldotnet_common/IRosActionSendGoalResponse.cs index 535d14b3..c906400d 100644 --- a/rcldotnet_common/IRosActionSendGoalResponse.cs +++ b/rcldotnet_common/IRosActionSendGoalResponse.cs @@ -21,5 +21,9 @@ public interface IRosActionSendGoalResponse : IRosMessage // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` // builtin_interfaces.msg.Time Stamp { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage StampAsRosMessage { get; set; } } } diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index d07c9c43..5d4ca694 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -388,6 +388,25 @@ public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) @[ end if]@ @[end for]@ +@[if action_interface is not None and ( + action_interface.startswith("global::ROS2.IRosActionSendGoalRequest") or + action_interface.startswith("global::ROS2.IRosActionGetResultRequest") or + action_interface.startswith("global::ROS2.IRosActionFeedbackMessage") +)]@ + global::ROS2.IRosMessage @(action_interface).GoalIdAsRosMessage + { + get => GoalId; + set => GoalId = (global::unique_identifier_msgs.msg.UUID)value; + } +@[end if]@ +@[if action_interface is not None and action_interface.startswith("global::ROS2.IRosActionSendGoalResponse")]@ + global::ROS2.IRosMessage @(action_interface).StampAsRosMessage + { + get => Stamp; + set => Stamp = (global::builtin_interfaces.msg.Time)value; + } +@[end if]@ + private sealed class Safe@(type_name)Handle : global::System.Runtime.InteropServices.SafeHandle { public Safe@(type_name)Handle() : base(global::System.IntPtr.Zero, true) { } From ab8aff4f7161a19a63bbcf3fd41c676de2d682dd Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 22 Jun 2022 17:37:29 +0200 Subject: [PATCH 44/73] actions: add ActionDefinitionStaticMemberCache --- .../ActionDefinitionStaticMemberCache.cs | 259 ++++++++++++++++++ rcldotnet/CMakeLists.txt | 1 + 2 files changed, 260 insertions(+) create mode 100644 rcldotnet/ActionDefinitionStaticMemberCache.cs diff --git a/rcldotnet/ActionDefinitionStaticMemberCache.cs b/rcldotnet/ActionDefinitionStaticMemberCache.cs new file mode 100644 index 00000000..c07c3015 --- /dev/null +++ b/rcldotnet/ActionDefinitionStaticMemberCache.cs @@ -0,0 +1,259 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Reflection; +using System.Runtime.InteropServices; + +namespace ROS2 +{ + internal static class ActionDefinitionStaticMemberCache + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + private static readonly IntPtr s_typeSupport; + private static readonly Func> s_createSendGoalRequest; + private static readonly Func s_createSendGoalRequestHandle; + private static readonly Func s_createSendGoalResponse; + private static readonly Func s_createSendGoalResponseHandle; + private static readonly Func s_createGetResultRequest; + private static readonly Func s_createGetResultRequestHandle; + private static readonly Func> s_createGetResultResponse; + private static readonly Func s_createGetResultResponseHandle; + private static readonly Func> s_createFeedbackMessage; + private static readonly Func s_createFeedbackMessageHandle; + + static ActionDefinitionStaticMemberCache() + { + TypeInfo typeInfo = typeof(TAction).GetTypeInfo(); + + MethodInfo getTypeSupport = typeInfo.GetDeclaredMethod("__GetTypeSupport"); + if (getTypeSupport != null) + { + try + { + s_typeSupport = (IntPtr)getTypeSupport.Invoke(null, new object[] { }); + } + catch + { + s_typeSupport = IntPtr.Zero; + } + } + else + { + s_typeSupport = IntPtr.Zero; + } + + s_createSendGoalRequest = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateSendGoalRequest"); + + s_createSendGoalRequestHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalRequestHandle"); + + s_createSendGoalResponse = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalResponse"); + + s_createSendGoalResponseHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalResponseHandle"); + + s_createGetResultRequest = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultRequest"); + + s_createGetResultRequestHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultRequestHandle"); + + s_createGetResultResponse = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateGetResultResponse"); + + s_createGetResultResponseHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultResponseHandle"); + + s_createFeedbackMessage = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateFeedbackMessage"); + + s_createFeedbackMessageHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateFeedbackMessageHandle"); + } + + public static IntPtr GetTypeSupport() + { + // This is a Method because it could throw. + if (s_typeSupport == IntPtr.Zero) + { + throw CreateMethodNotDefinedCorrectlyException("__GetTypeSupport"); + } + + return s_typeSupport; + } + + public static IRosActionSendGoalRequest CreateSendGoalRequest() + { + if (s_createSendGoalRequest != null) + { + return s_createSendGoalRequest(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalRequest"); + } + } + + public static SafeHandle CreateSendGoalRequestHandle() + { + if (s_createSendGoalRequestHandle != null) + { + return s_createSendGoalRequestHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalRequestHandle"); + } + } + + public static IRosActionSendGoalResponse CreateSendGoalResponse() + { + if (s_createSendGoalResponse != null) + { + return s_createSendGoalResponse(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalResponse"); + } + } + + public static SafeHandle CreateSendGoalResponseHandle() + { + if (s_createSendGoalResponseHandle != null) + { + return s_createSendGoalResponseHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalResponseHandle"); + } + } + + public static IRosActionGetResultRequest CreateGetResultRequest() + { + if (s_createGetResultRequest != null) + { + return s_createGetResultRequest(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultRequest"); + } + } + + public static SafeHandle CreateGetResultRequestHandle() + { + if (s_createGetResultRequestHandle != null) + { + return s_createGetResultRequestHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultRequestHandle"); + } + } + + public static IRosActionGetResultResponse CreateGetResultResponse() + { + if (s_createGetResultResponse != null) + { + return s_createGetResultResponse(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultResponse"); + } + } + + public static SafeHandle CreateGetResultResponseHandle() + { + if (s_createGetResultResponseHandle != null) + { + return s_createGetResultResponseHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultResponseHandle"); + } + } + + public static IRosActionFeedbackMessage CreateFeedbackMessage() + { + if (s_createFeedbackMessage != null) + { + return s_createFeedbackMessage(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateFeedbackMessage"); + } + } + + public static SafeHandle CreateFeedbackMessageHandle() + { + if (s_createFeedbackMessageHandle != null) + { + return s_createFeedbackMessageHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateFeedbackMessageHandle"); + } + } + + private static TDelegate CreateDelegateForDeclaredMethod(TypeInfo typeInfo, string methodName) + where TDelegate : Delegate + { + MethodInfo methodInfo = typeInfo.GetDeclaredMethod(methodName); + if (methodInfo != null) + { + try + { + return (TDelegate)methodInfo.CreateDelegate(typeof(TDelegate)); + } + catch + { + return null; + } + } + else + { + return null; + } + } + + private static Exception CreateMethodNotDefinedCorrectlyException(string methodName) + { + return new InvalidOperationException($"Type '{typeof(TAction).FullName}' did not define a correct {methodName} method."); + } + } +} diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 997df132..d79669ef 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -28,6 +28,7 @@ endif() set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") set(CS_SOURCES + ActionDefinitionStaticMemberCache.cs Client.cs GuardCondition.cs MessageStaticMemberCache.cs From 0b8538725e90cb215e2a3501e55258343a94280d Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 22 Jun 2022 17:53:15 +0200 Subject: [PATCH 45/73] actions: add public action client API without implementation --- rcldotnet/ActionClient.cs | 55 +++++++++++++++++++++++ rcldotnet/ActionClientGoalHandle.cs | 67 +++++++++++++++++++++++++++++ rcldotnet/ActionGoalStatus.cs | 60 ++++++++++++++++++++++++++ rcldotnet/CMakeLists.txt | 3 ++ rcldotnet/Node.cs | 9 ++++ 5 files changed, 194 insertions(+) create mode 100644 rcldotnet/ActionClient.cs create mode 100644 rcldotnet/ActionClientGoalHandle.cs create mode 100644 rcldotnet/ActionGoalStatus.cs diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs new file mode 100644 index 00000000..2d3c5e62 --- /dev/null +++ b/rcldotnet/ActionClient.cs @@ -0,0 +1,55 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading.Tasks; + +namespace ROS2 +{ + public abstract class ActionClient + { + // Only allow internal subclasses. + internal ActionClient() + { + } + } + + public sealed class ActionClient : ActionClient + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionClient() + { + } + + public bool ServiceIsReady() + { + throw new NotImplementedException(); + } + + public Task> SendGoalAsync(TGoal goal) + { + throw new NotImplementedException(); + } + + public Task> SendGoalAsync(TGoal goal, Action feedbackCallback) + { + throw new NotImplementedException(); + } + } +} diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs new file mode 100644 index 00000000..df1dc7ee --- /dev/null +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -0,0 +1,67 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading.Tasks; +using builtin_interfaces.msg; + +namespace ROS2 +{ + public abstract class ActionClientGoalHandle + { + // Only allow internal subclasses. + internal ActionClientGoalHandle() + { + } + + public abstract Guid GoalId { get; } + + public abstract bool Accepted { get; } + + public abstract Time Stamp { get; } + + public abstract ActionGoalStatus Status { get; } + } + + public sealed class ActionClientGoalHandle : ActionClientGoalHandle + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionClientGoalHandle() + { + } + + public override Guid GoalId { get; } + + public override bool Accepted { get; } + + public override Time Stamp { get; } + + public override ActionGoalStatus Status { get; } + + public Task CancelGoalAsync() + { + throw new NotImplementedException(); + } + + public Task GetResultAsync() + { + throw new NotImplementedException(); + } + } +} diff --git a/rcldotnet/ActionGoalStatus.cs b/rcldotnet/ActionGoalStatus.cs new file mode 100644 index 00000000..52a5d0b8 --- /dev/null +++ b/rcldotnet/ActionGoalStatus.cs @@ -0,0 +1,60 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + public enum ActionGoalStatus + { + // see definition here: https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/msg/GoalStatus.msg + + // + // Indicates status has not been properly set. + // + Unknown = 0, + + // + // The goal has been accepted and is awaiting execution. + // + Accepted = 1, + + // + // The goal is currently being executed by the action server. + // + Executing = 2, + + // + // The client has requested that the goal be canceled and the action + // server has accepted the cancel request. + // + Canceling = 3, + + // + // The goal was achieved successfully by the action server. + // + Succeeded = 4, + + // + // The goal was canceled after an external request from an + // action client. + // + Canceled = 5, + + // + // The goal was terminated by the action server without an + // external request. + // + Aborted = 6, + } +} diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index d79669ef..99f5e6f9 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -28,7 +28,10 @@ endif() set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") set(CS_SOURCES + ActionClient.cs + ActionClientGoalHandle.cs ActionDefinitionStaticMemberCache.cs + ActionGoalStatus.cs Client.cs GuardCondition.cs MessageStaticMemberCache.cs diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index f6177d4e..c6fb7fe5 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -267,5 +267,14 @@ public GuardCondition CreateGuardCondition(Action callback) _guardConditions.Add(guardCondition); return guardCondition; } + + public ActionClient CreateActionClient(string actionName) + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + throw new NotImplementedException(); + } } } From eec1f7f261c1bea94ae2740a7b0b03b88e0002db Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 21 Jun 2022 17:03:54 +0200 Subject: [PATCH 46/73] actions: rename `ServiceIsReady()` to `ServerIsReady() `and move to base class --- rcldotnet/ActionClient.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index 2d3c5e62..67702314 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -24,6 +24,8 @@ public abstract class ActionClient internal ActionClient() { } + + public abstract bool ServerIsReady(); } public sealed class ActionClient : ActionClient @@ -37,7 +39,7 @@ internal ActionClient() { } - public bool ServiceIsReady() + public override bool ServerIsReady() { throw new NotImplementedException(); } From 71ad79b707ec875e7d7fe417137a6c07c8350708 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 23 Jun 2022 08:13:42 +0200 Subject: [PATCH 47/73] actions: add helper for converting guid <-> uuid --- rcldotnet/CMakeLists.txt | 1 + rcldotnet/GuidExtensions.cs | 71 +++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 rcldotnet/GuidExtensions.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 99f5e6f9..c9d84df3 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -34,6 +34,7 @@ set(CS_SOURCES ActionGoalStatus.cs Client.cs GuardCondition.cs + GuidExtensions.cs MessageStaticMemberCache.cs Node.cs Publisher.cs diff --git a/rcldotnet/GuidExtensions.cs b/rcldotnet/GuidExtensions.cs new file mode 100644 index 00000000..c75c0a68 --- /dev/null +++ b/rcldotnet/GuidExtensions.cs @@ -0,0 +1,71 @@ +using System; +using System.Linq; +using unique_identifier_msgs.msg; + +namespace ROS2 +{ + public static class GuidExtensions + { + public static UUID ToUuidMsg(this Guid guid) + { + // Microsoft uses some mixed endian representation for Guid in + // Windows and .NET, which the Guid.ToByteArray() does as well. + // Most/All other implementations of UUID/GUIDs use an unique big + // endian representation on the wire. See this StackOverflow answer + // for more context: https://stackoverflow.com/a/9195681 + // + // So there is a need to swap some bytes before converting them to + // the UUID ROS message. + // + // Even on BigEndian machines .NET does the swapping of the first + // three fields to little endian, so it is at least consistent on + // all platforms, no need to check the endianness of the runtime. + // https://github.com/dotnet/runtime/blob/1ba0394d71a4ea6bee7f6b28a22d666b7b56f913/src/libraries/System.Private.CoreLib/src/System/Guid.cs#L819 + var guidBytes = guid.ToByteArray(); + EndianSwap(guidBytes); + + var uuidMsg = new UUID(); + uuidMsg.Uuid = guidBytes; + return uuidMsg; + } + + public static byte[] ToUuidByteArray(this Guid guid) + { + // See comments in `ToUuidMsg` for info on byte swapping. + + var guidBytes = guid.ToByteArray(); + EndianSwap(guidBytes); + + return guidBytes; + } + + public static Guid ToGuid(this UUID uuidMsg) + { + // See comments in `ToUuidMsg` for info on byte swapping. + + var uuidBytesCopy = uuidMsg.Uuid.ToArray(); + EndianSwap(uuidBytesCopy); + + var guid = new Guid(uuidBytesCopy); + return guid; + } + + private static void EndianSwap(byte[] guid) + { + // taken from https://github.com/StephenCleary/Guids/blob/a9bd91835d536636d13249bd1bfbb1cd76c43533/src/Nito.Guids/GuidUtility.cs#L47 + Swap(guid, 0, 3); + Swap(guid, 1, 2); + + Swap(guid, 4, 5); + + Swap(guid, 6, 7); + } + + private static void Swap(byte[] array, int index1, int index2) + { + var temp = array[index1]; + array[index1] = array[index2]; + array[index2] = temp; + } + } +} From 83746ebce5398d588a36f7841709b7f9ac754647 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 24 Jun 2022 12:59:30 +0200 Subject: [PATCH 48/73] actions: implement basic action client --- rcldotnet/ActionClient.cs | 459 +++++++++++++++++++++++++++- rcldotnet/ActionClientGoalHandle.cs | 42 ++- rcldotnet/CMakeLists.txt | 26 +- rcldotnet/Node.cs | 47 ++- rcldotnet/RCLdotnet.cs | 393 ++++++++++++++++++++++++ rcldotnet/SafeActionClientHandle.cs | 75 +++++ rcldotnet/package.xml | 8 + rcldotnet/rcldotnet.c | 103 +++++++ rcldotnet/rcldotnet.h | 37 +++ rcldotnet/rcldotnet_action_client.c | 58 ++++ rcldotnet/rcldotnet_action_client.h | 32 ++ rcldotnet/rcldotnet_node.c | 36 ++- rcldotnet/rcldotnet_node.h | 9 + 13 files changed, 1307 insertions(+), 18 deletions(-) create mode 100644 rcldotnet/SafeActionClientHandle.cs create mode 100644 rcldotnet/rcldotnet_action_client.c create mode 100644 rcldotnet/rcldotnet_action_client.h diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index 67702314..0e058918 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -14,10 +14,76 @@ */ using System; +using System.Collections.Concurrent; +using System.Runtime.InteropServices; using System.Threading.Tasks; +using action_msgs.msg; +using action_msgs.srv; +using builtin_interfaces.msg; +using ROS2.Utils; +using unique_identifier_msgs.msg; namespace ROS2 { + internal static class ActionClientDelegates + { + internal static readonly DllLoadUtils _dllLoadUtils; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendGoalRequestType( + SafeActionClientHandle clientHandle, SafeHandle goalRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendGoalRequestType native_rcl_action_send_goal_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendResultRequestType( + SafeActionClientHandle clientHandle, SafeHandle resultRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendResultRequestType native_rcl_action_send_result_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendCancelRequestType( + SafeActionClientHandle clientHandle, SafeHandle cancelRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendCancelRequestType native_rcl_action_send_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerIsAvailableType( + SafeNodeHandle nodeHandle, SafeActionClientHandle clientHandle, out bool isAvailable); + + internal static NativeRCLActionServerIsAvailableType native_rcl_action_server_is_available = null; + + static ActionClientDelegates() + { + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); + + IntPtr native_rcl_action_send_goal_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_goal_request"); + ActionClientDelegates.native_rcl_action_send_goal_request = + (NativeRCLActionSendGoalRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_goal_request_ptr, typeof(NativeRCLActionSendGoalRequestType)); + + IntPtr native_rcl_action_send_result_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_result_request"); + ActionClientDelegates.native_rcl_action_send_result_request = + (NativeRCLActionSendResultRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_result_request_ptr, typeof(NativeRCLActionSendResultRequestType)); + + IntPtr native_rcl_action_send_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_cancel_request"); + ActionClientDelegates.native_rcl_action_send_cancel_request = + (NativeRCLActionSendCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_cancel_request_ptr, typeof(NativeRCLActionSendCancelRequestType)); + + IntPtr native_rcl_action_server_is_available_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_is_available"); + ActionClientDelegates.native_rcl_action_server_is_available = + (NativeRCLActionServerIsAvailableType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_is_available_ptr, typeof(NativeRCLActionServerIsAvailableType)); + } + } + public abstract class ActionClient { // Only allow internal subclasses. @@ -25,7 +91,36 @@ internal ActionClient() { } + // ActionClient does intentionally (for now) not implement IDisposable + // as this needs some extra consideration how the type works after its + // internal handle is disposed. By relying on the GC/Finalizer of + // SafeHandle the handle only gets Disposed if the action client is not + // live anymore. + internal abstract SafeActionClientHandle Handle { get; } + public abstract bool ServerIsReady(); + + internal abstract IRosMessage CreateSendGoalResponse(); + + internal abstract SafeHandle CreateSendGoalResponseHandle(); + + internal abstract IRosMessage CreateGetResultResponse(); + + internal abstract SafeHandle CreateGetResultResponseHandle(); + + internal abstract IRosMessage CreateFeedbackMessage(); + + internal abstract SafeHandle CreateFeedbackMessageHandle(); + + internal abstract void HandleFeedbackMessage(IRosMessage feedbackMessage); + + internal abstract void HandleStatusMessage(GoalStatusArray statusMessage); + + internal abstract void HandleGoalResponse(long sequenceNumber, IRosMessage goalResponse); + + internal abstract void HandleCancelResponse(long sequenceNumber, CancelGoal_Response cancelResponse); + + internal abstract void HandleResultResponse(long sequenceNumber, IRosMessage resultResponse); } public sealed class ActionClient : ActionClient @@ -34,24 +129,380 @@ public sealed class ActionClient : ActionCli where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + // ros2_java uses a WeakReference here. Not sure if its needed or not. + private readonly Node _node; + + // TODO: (sh) rclpy uses week references to the goal handles here? Is this needed? + private readonly ConcurrentDictionary> _goalHandles + = new ConcurrentDictionary>(); + + private readonly ConcurrentDictionary _pendingGoalRequests + = new ConcurrentDictionary(); + + private readonly ConcurrentDictionary _pendingCancelRequests + = new ConcurrentDictionary(); + + private readonly ConcurrentDictionary _pendingResultRequests + = new ConcurrentDictionary(); + // No public constructor. - internal ActionClient() + internal ActionClient(SafeActionClientHandle handle, Node node) { + Handle = handle; + _node = node; } + internal override SafeActionClientHandle Handle { get; } + public override bool ServerIsReady() { - throw new NotImplementedException(); + RCLRet ret = ActionClientDelegates.native_rcl_action_server_is_available(_node.Handle, Handle, out var serverIsReady); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_server_is_available)}() failed."); + + return serverIsReady; } public Task> SendGoalAsync(TGoal goal) { - throw new NotImplementedException(); + return SendGoalAsyncInternal(goal, null); } public Task> SendGoalAsync(TGoal goal, Action feedbackCallback) { - throw new NotImplementedException(); + return SendGoalAsyncInternal(goal, feedbackCallback); + } + + private Task> SendGoalAsyncInternal(TGoal goal, Action feedbackCallback) + { + long sequenceNumber; + + var goalId = Guid.NewGuid(); + var goalRequest = ActionDefinitionStaticMemberCache.CreateSendGoalRequest(); + goalRequest.GoalIdAsRosMessage = goalId.ToUuidMsg(); + goalRequest.Goal = goal; + + using (var goalRequestHandle = ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + goalRequestHandle.DangerousAddRef(ref mustRelease); + goalRequest.__WriteToHandle(goalRequestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + goalRequestHandle.DangerousRelease(); + } + } + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_goal_request(Handle, goalRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_goal_request)}() failed."); + } + + var taskCompletionSource = new TaskCompletionSource>(); + var pendingGoalRequest = new PendingGoalRequest(goalId, taskCompletionSource, feedbackCallback); + if (!_pendingGoalRequests.TryAdd(sequenceNumber, pendingGoalRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + return taskCompletionSource.Task; + } + + // TODO: (sh) Add CancelAllGoalsAsync(), like rclcpp Client.async_cancel_all_goals() + // TODO: (sh) Add CancelGoalsBeforeAsync(), like rclcpp Client.async_cancel_goals_before() + + // TODO: (sh) Maybe make this public. + // (but then check if goalHandle belongs to this actionClient is needed) + internal Task CancelGoalAsync(ActionClientGoalHandle goalHandle) + { + long sequenceNumber; + + var cancelRequest = new CancelGoal_Request(); + cancelRequest.GoalInfo.GoalId.Uuid = goalHandle.GoalId.ToUuidByteArray(); + + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + cancelRequestHandle.DangerousAddRef(ref mustRelease); + cancelRequest.__WriteToHandle(cancelRequestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + cancelRequestHandle.DangerousRelease(); + } + } + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_cancel_request(Handle, cancelRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_cancel_request)}() failed."); + } + + var taskCompletionSource = new TaskCompletionSource(); + var pendingCancelRequest = new PendingCancelRequest(taskCompletionSource); + if (!_pendingCancelRequests.TryAdd(sequenceNumber, pendingCancelRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + return taskCompletionSource.Task; + } + + internal Task GetResultAsync( + ActionClientGoalHandle goalHandle, + TaskCompletionSource resultTaskCompletionSource) + { + try + { + long sequenceNumber; + + var resultRequest = ActionDefinitionStaticMemberCache.CreateGetResultRequest(); + resultRequest.GoalIdAsRosMessage = goalHandle.GoalId.ToUuidMsg(); + + using (var resultRequestHandle = ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + resultRequestHandle.DangerousAddRef(ref mustRelease); + resultRequest.__WriteToHandle(resultRequestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + resultRequestHandle.DangerousRelease(); + } + } + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_result_request(Handle, resultRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_result_request)}() failed."); + } + + var pendingResultRequest = new PendingResultRequest(resultTaskCompletionSource); + if (!_pendingResultRequests.TryAdd(sequenceNumber, pendingResultRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + } + catch (Exception exception) + { + // Don't directly return Task.FromException() so that the tasks + // returned from other threads get the exception as well. + resultTaskCompletionSource.TrySetException(exception); + } + + return resultTaskCompletionSource.Task; + } + + internal override IRosMessage CreateSendGoalResponse() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponse(); + } + + internal override SafeHandle CreateSendGoalResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponseHandle(); + } + + internal override IRosMessage CreateGetResultResponse() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponse(); + } + + internal override SafeHandle CreateGetResultResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponseHandle(); + } + + internal override IRosMessage CreateFeedbackMessage() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessage(); + } + + internal override SafeHandle CreateFeedbackMessageHandle() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessageHandle(); + } + + internal override void HandleFeedbackMessage(IRosMessage feedbackMessage) + { + var feedbackMessageCasted = (IRosActionFeedbackMessage)feedbackMessage; + + var goalIdUuid = (UUID)feedbackMessageCasted.GoalIdAsRosMessage; + var goalId = goalIdUuid.ToGuid(); + + if (!_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + // TODO: (sh) Add error handling/logging. GoalId not found. + return; + } + + goalHandle.FeedbackCallback?.Invoke(feedbackMessageCasted.Feedback); + } + + internal override void HandleStatusMessage(GoalStatusArray statusMessage) + { + foreach (var statusMessage1 in statusMessage.StatusList) + { + var goalId = statusMessage1.GoalInfo.GoalId.ToGuid(); + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + var status = statusMessage1.Status; + goalHandle.Status = ConvertGoalStatus(status); + + // rclpy does this, rclcpp does not. + if (status == GoalStatus.STATUS_SUCCEEDED + || status == GoalStatus.STATUS_CANCELED + || status == GoalStatus.STATUS_ABORTED) + { + // Remove "done" goals from the dictionary. + // TODO: (sh) Should we remove goalHandles that are not in the status list as well? After a timeout? + _goalHandles.TryRemove(goalId, out _); + } + } + } + } + + private static ActionGoalStatus ConvertGoalStatus(sbyte status) + { + switch (status) + { + case GoalStatus.STATUS_UNKNOWN: + return ActionGoalStatus.Unknown; + + case GoalStatus.STATUS_ACCEPTED: + return ActionGoalStatus.Accepted; + + case GoalStatus.STATUS_EXECUTING: + return ActionGoalStatus.Executing; + + case GoalStatus.STATUS_CANCELING: + return ActionGoalStatus.Canceling; + + case GoalStatus.STATUS_SUCCEEDED: + return ActionGoalStatus.Succeeded; + + case GoalStatus.STATUS_CANCELED: + return ActionGoalStatus.Canceled; + + case GoalStatus.STATUS_ABORTED: + return ActionGoalStatus.Aborted; + + default: + throw new ArgumentException("Unknown status.", nameof(status)); + } + } + + internal override void HandleGoalResponse(long sequenceNumber, IRosMessage goalResponse) + { + if (!_pendingGoalRequests.TryRemove(sequenceNumber, out var pendingGoalRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + var goalId = pendingGoalRequest.GoalId; + var goalResponseCasted = (IRosActionSendGoalResponse)goalResponse; + var accepted = goalResponseCasted.Accepted; + var stamp = (Time)goalResponseCasted.StampAsRosMessage; + + var goalHandle = new ActionClientGoalHandle(this, goalId, accepted, stamp, pendingGoalRequest.FeedbackCallback); + + if (accepted) + { + if (!_goalHandles.TryAdd(goalId, goalHandle)) + { + var exception = new Exception($"Two goals were accepted with the same Id '{goalId}'."); + pendingGoalRequest.TaskCompletionSource.TrySetException(exception); + return; + }; + } + + pendingGoalRequest.TaskCompletionSource.TrySetResult(goalHandle); + } + + internal override void HandleCancelResponse(long sequenceNumber, CancelGoal_Response cancelResponse) + { + if (!_pendingCancelRequests.TryRemove(sequenceNumber, out var pendingCancelRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + pendingCancelRequest.TaskCompletionSource.TrySetResult(cancelResponse); + } + + internal override void HandleResultResponse(long sequenceNumber, IRosMessage resultResponse) + { + if (!_pendingResultRequests.TryRemove(sequenceNumber, out var pendingResultRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + var resultResponseCasted = (IRosActionGetResultResponse)resultResponse; + var result = resultResponseCasted.Result; + + pendingResultRequest.TaskCompletionSource.TrySetResult(result); + } + + private sealed class PendingGoalRequest + { + public PendingGoalRequest( + Guid goalId, + TaskCompletionSource> taskCompletionSource, + Action feedbackCallback) + { + GoalId = goalId; + TaskCompletionSource = taskCompletionSource; + FeedbackCallback = feedbackCallback; + } + + public Guid GoalId { get; } + public TaskCompletionSource> TaskCompletionSource { get; } + public Action FeedbackCallback { get; } + } + + private sealed class PendingCancelRequest + { + public PendingCancelRequest(TaskCompletionSource taskCompletionSource) + { + TaskCompletionSource = taskCompletionSource; + } + + public TaskCompletionSource TaskCompletionSource { get; } + } + + private sealed class PendingResultRequest + { + public PendingResultRequest(TaskCompletionSource taskCompletionSource) + { + TaskCompletionSource = taskCompletionSource; + } + + public TaskCompletionSource TaskCompletionSource { get; } } } } diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs index df1dc7ee..bdae9f8b 100644 --- a/rcldotnet/ActionClientGoalHandle.cs +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -14,6 +14,7 @@ */ using System; +using System.Threading; using System.Threading.Tasks; using builtin_interfaces.msg; @@ -32,7 +33,7 @@ internal ActionClientGoalHandle() public abstract Time Stamp { get; } - public abstract ActionGoalStatus Status { get; } + public abstract ActionGoalStatus Status { get; internal set; } } public sealed class ActionClientGoalHandle : ActionClientGoalHandle @@ -41,9 +42,23 @@ public sealed class ActionClientGoalHandle : where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + private readonly ActionClient _actionClient; + + private TaskCompletionSource _resultTaskCompletionSource; + // No public constructor. - internal ActionClientGoalHandle() + internal ActionClientGoalHandle( + ActionClient actionClient, + Guid goalId, + bool accepted, + Time stamp, + Action feedbackCallback) { + GoalId = goalId; + Accepted = accepted; + Stamp = stamp; + FeedbackCallback = feedbackCallback; + _actionClient = actionClient; } public override Guid GoalId { get; } @@ -52,16 +67,33 @@ internal ActionClientGoalHandle() public override Time Stamp { get; } - public override ActionGoalStatus Status { get; } + public override ActionGoalStatus Status { get; internal set; } + + internal Action FeedbackCallback { get; } + // TODO: (sh) should we return the CancelGoal_Response? Wrap it in type with dotnet enum/Guid? public Task CancelGoalAsync() { - throw new NotImplementedException(); + return _actionClient.CancelGoalAsync(this); } public Task GetResultAsync() { - throw new NotImplementedException(); + // Fast case to avoid allocation and calling Interlocked.CompareExchange. + if (_resultTaskCompletionSource != null) + { + return _resultTaskCompletionSource.Task; + } + + var resultTaskCompletionSource = new TaskCompletionSource(); + + if (Interlocked.CompareExchange(ref _resultTaskCompletionSource, resultTaskCompletionSource, null) != null) + { + // Some other thread was first. + return _resultTaskCompletionSource.Task; + } + + return _actionClient.GetResultAsync(this, resultTaskCompletionSource); } } } diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index c9d84df3..9ab60df2 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -6,7 +6,11 @@ find_package(ament_cmake_export_assemblies REQUIRED) find_package(ament_cmake REQUIRED) find_package(rcl REQUIRED) +find_package(rcl_action REQUIRED) +find_package(action_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(unique_identifier_msgs REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rmw_implementation_cmake REQUIRED) @@ -41,6 +45,7 @@ set(CS_SOURCES RCLdotnet.cs RCLExceptionHelper.cs RCLRet.cs + SafeActionClientHandle.cs SafeClientHandle.cs SafeGuardConditionHandle.cs SafeNodeHandle.cs @@ -55,21 +60,26 @@ set(CS_SOURCES ) find_package(rcldotnet_common REQUIRED) -foreach(_assembly_dep ${rcldotnet_common_ASSEMBLIES_DLL}) - list(APPEND _assembly_deps_dll "${_assembly_dep}") -endforeach() + +set(_assemblies_dep_dlls + ${action_msgs_ASSEMBLIES_DLL} + ${builtin_interfaces_ASSEMBLIES_DLL} + ${rcldotnet_common_ASSEMBLIES_DLL} + ${unique_identifier_msgs_ASSEMBLIES_DLL} +) add_dotnet_library(${PROJECT_NAME}_assemblies SOURCES ${CS_SOURCES} INCLUDE_DLLS - ${_assembly_deps_dll} + ${_assemblies_dep_dlls} ) install_dotnet(${PROJECT_NAME}_assemblies DESTINATION lib/${PROJECT_NAME}/dotnet) ament_export_assemblies_dll("lib/${PROJECT_NAME}/dotnet/${PROJECT_NAME}_assemblies.dll") add_library(${PROJECT_NAME}_native SHARED + rcldotnet_action_client.c rcldotnet_client.c rcldotnet_guard_condition.c rcldotnet_node.c @@ -78,8 +88,11 @@ add_library(${PROJECT_NAME}_native SHARED ) ament_target_dependencies(${PROJECT_NAME}_native + "action_msgs" "builtin_interfaces" + "unique_identifier_msgs" "rcl" + "rcl_action" "rosidl_generator_c" "rosidl_typesupport_c" ) @@ -107,11 +120,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() - set(_assemblies_dep_dlls - ${rcldotnet_common_ASSEMBLIES_DLL} + set(_test_assemblies_dep_dlls ${std_msgs_ASSEMBLIES_DLL} ${test_msgs_ASSEMBLIES_DLL} - ${builtin_interfaces_ASSEMBLIES_DLL} ) add_dotnet_test(test_messages @@ -121,6 +132,7 @@ if(BUILD_TESTING) test/test_services.cs INCLUDE_DLLS ${_assemblies_dep_dlls} + ${_test_assemblies_dep_dlls} INCLUDE_REFERENCES "Microsoft.NET.Test.Sdk=15.9.0" "xunit=2.4.1" diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index c6fb7fe5..7d30335c 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -72,6 +72,18 @@ internal delegate RCLRet NativeRCLDestroyClientHandleType( internal static NativeRCLDestroyClientHandleType native_rcl_destroy_client_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionCreateClientHandleType( + ref SafeActionClientHandle actionClientHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); + + internal static NativeRCLActionCreateClientHandleType native_rcl_action_create_client_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyClientHandleType( + IntPtr actionClientHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLActionDestroyClientHandleType native_rcl_action_destroy_client_handle = null; + static NodeDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -132,6 +144,20 @@ static NodeDelegates() NodeDelegates.native_rcl_destroy_client_handle = (NativeRCLDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_client_handle_ptr, typeof(NativeRCLDestroyClientHandleType)); + + IntPtr native_rcl_action_create_client_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_action_create_client_handle"); + + NodeDelegates.native_rcl_action_create_client_handle = + (NativeRCLActionCreateClientHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_create_client_handle_ptr, typeof(NativeRCLActionCreateClientHandleType)); + + IntPtr native_rcl_action_destroy_client_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_action_destroy_client_handle"); + + NodeDelegates.native_rcl_action_destroy_client_handle = + (NativeRCLActionDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_destroy_client_handle_ptr, typeof(NativeRCLActionDestroyClientHandleType)); } } @@ -145,6 +171,8 @@ public sealed class Node private readonly IList _guardConditions; + private readonly IList _actionClients; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -152,6 +180,7 @@ internal Node(SafeNodeHandle handle) _services = new List(); _clients = new List(); _guardConditions = new List(); + _actionClients = new List(); } public IList Subscriptions => _subscriptions; @@ -165,6 +194,8 @@ internal Node(SafeNodeHandle handle) public IList GuardConditions => _guardConditions; + public IList ActionClients => _actionClients; + // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its // internal handle is disposed. @@ -274,7 +305,21 @@ public ActionClient CreateActionClient.GetTypeSupport(); + + var actionClientHandle = new SafeActionClientHandle(); + RCLRet ret = NodeDelegates.native_rcl_action_create_client_handle(ref actionClientHandle, Handle, actionName, typeSupport); + actionClientHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + actionClientHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_action_create_client_handle)}() failed."); + } + + // TODO: (sh) Add actionName to ActionClient. + var actionClient = new ActionClient(actionClientHandle, this); + _actionClients.Add(actionClient); + return actionClient; } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 4f0079a0..8b61ffd6 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -15,6 +15,8 @@ using System; using System.Runtime.InteropServices; +using action_msgs.msg; +using action_msgs.srv; using ROS2.Utils; namespace ROS2 @@ -172,6 +174,56 @@ internal delegate RCLRet NativeRCLCreateRequestIdHandleType( internal static NativeRCLTakeResponseType native_rcl_take_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionClientWaitSetGetNumEntriesType( + SafeActionClientHandle actionClientHandle, out int numSubscriptions, out int numGuardConditions, out int numTimers, out int numClients, out int numServices); + + internal static NativeRCLActionClientWaitSetGetNumEntriesType native_rcl_action_client_wait_set_get_num_entries = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionWaitSetAddActionClientType( + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle); + + internal static NativeRCLActionWaitSetAddActionClientType native_rcl_action_wait_set_add_action_client = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle, out bool isFeedbackReady, out bool isStatusReady, out bool isGoalResponseReady, out bool isCancelResponseReady, out bool isResultResponseReady); + + internal static NativeRCLActionClientWaitSetGetEntitiesReadyType native_rcl_action_client_wait_set_get_entities_ready = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeFeedbackType( + SafeActionClientHandle actionClientHandle, SafeHandle feedbackMessageHandle); + + internal static NativeRCLActionTakeFeedbackType native_rcl_action_take_feedback = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeStatusType( + SafeActionClientHandle actionClientHandle, SafeHandle statusMessageHandle); + + internal static NativeRCLActionTakeStatusType native_rcl_action_take_status = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeGoalResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle goalResponseHandle); + + internal static NativeRCLActionTakeGoalResponseType native_rcl_action_take_goal_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeCancelResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionTakeCancelResponseType native_rcl_action_take_cancel_response = null; + + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeResultResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultResponseHandle); + + internal static NativeRCLActionTakeResultResponseType native_rcl_action_take_result_response = null; + + static RCLdotnetDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -344,6 +396,55 @@ static RCLdotnetDelegates() RCLdotnetDelegates.native_rcl_take_response = (NativeRCLTakeResponseType)Marshal.GetDelegateForFunctionPointer( native_rcl_take_response_ptr, typeof(NativeRCLTakeResponseType)); + + IntPtr native_rcl_action_client_wait_set_get_num_entries_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_client_wait_set_get_num_entries"); + RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries = + (NativeRCLActionClientWaitSetGetNumEntriesType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_client_wait_set_get_num_entries_ptr, typeof(NativeRCLActionClientWaitSetGetNumEntriesType)); + + IntPtr native_rcl_action_wait_set_add_action_client_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_wait_set_add_action_client"); + RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client = + (NativeRCLActionWaitSetAddActionClientType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_wait_set_add_action_client_ptr, typeof(NativeRCLActionWaitSetAddActionClientType)); + + IntPtr native_rcl_action_client_wait_set_get_entities_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_client_wait_set_get_entities_ready"); + RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready = + (NativeRCLActionClientWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_client_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionClientWaitSetGetEntitiesReadyType)); + + IntPtr native_rcl_action_take_feedback_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_feedback"); + RCLdotnetDelegates.native_rcl_action_take_feedback + = (NativeRCLActionTakeFeedbackType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_feedback_ptr, typeof(NativeRCLActionTakeFeedbackType)); + + IntPtr native_rcl_action_take_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_status"); + RCLdotnetDelegates.native_rcl_action_take_status = + (NativeRCLActionTakeStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_status_ptr, typeof(NativeRCLActionTakeStatusType)); + + IntPtr native_rcl_action_take_goal_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_goal_response"); + RCLdotnetDelegates.native_rcl_action_take_goal_response = + (NativeRCLActionTakeGoalResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_goal_response_ptr, typeof(NativeRCLActionTakeGoalResponseType)); + + IntPtr native_rcl_action_take_cancel_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_cancel_response"); + RCLdotnetDelegates.native_rcl_action_take_cancel_response = + (NativeRCLActionTakeCancelResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_cancel_response_ptr, typeof(NativeRCLActionTakeCancelResponseType)); + + IntPtr native_rcl_action_take_result_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_result_response"); + RCLdotnetDelegates.native_rcl_action_take_result_response = + (NativeRCLActionTakeResultResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_result_response_ptr, typeof(NativeRCLActionTakeResultResponseType)); + } } @@ -435,6 +536,12 @@ private static void WaitSetAddGuardCondition(SafeWaitSetHandle waitSetHandle, Sa RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_guard_condition)}() failed."); } + private static void WaitSetAddActionClient(SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client(waitSetHandle, actionClientHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client)}() failed."); + } + /// /// Block until the wait set is ready or until the timeout has been exceeded. /// @@ -582,6 +689,196 @@ private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeade } } + private static bool TakeFeedbackMessage(ActionClient actionClient, IRosMessage feedbackMessage) + { + using (var feedbackMessageHandle = actionClient.CreateFeedbackMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_feedback(actionClient.Handle, feedbackMessageHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + feedbackMessageHandle.DangerousAddRef(ref mustRelease); + feedbackMessage.__ReadFromHandle(feedbackMessageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + feedbackMessageHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.SubscriptionTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_feedback)}() failed."); + } + } + } + + private static bool TakeStatusMessage(ActionClient actionClient, IRosMessage statusMessage) + { + using (var statusMessageHandle = GoalStatusArray.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_status(actionClient.Handle, statusMessageHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + statusMessageHandle.DangerousAddRef(ref mustRelease); + statusMessage.__ReadFromHandle(statusMessageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + statusMessageHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.SubscriptionTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_status)}() failed."); + } + } + } + + private static bool TakeGoalResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage goalResponse) + { + using (var goalResponseHandle = actionClient.CreateSendGoalResponseHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_goal_response(actionClient.Handle, requestHeaderHandle, goalResponseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + goalResponseHandle.DangerousAddRef(ref mustRelease); + goalResponse.__ReadFromHandle(goalResponseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + goalResponseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_goal_response)}() failed."); + } + } + } + + private static bool TakeCancelResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage cancelResponse) + { + using (var cancelResponseHandle = CancelGoal_Response.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_cancel_response(actionClient.Handle, requestHeaderHandle, cancelResponseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + cancelResponseHandle.DangerousAddRef(ref mustRelease); + cancelResponse.__ReadFromHandle(cancelResponseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + cancelResponseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_cancel_response)}() failed."); + } + } + } + + private static bool TakeResultResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage resultResponse) + { + using (var resultResponseHandle = actionClient.CreateGetResultResponseHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_result_response(actionClient.Handle, requestHeaderHandle, resultResponseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + resultResponseHandle.DangerousAddRef(ref mustRelease); + resultResponse.__ReadFromHandle(resultResponseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + resultResponseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_result_response)}() failed."); + } + } + } + private static void SendResponse(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { using (var responseHandle = service.CreateResponseHandle()) @@ -622,6 +919,25 @@ public static void SpinOnce(Node node, long timeout) int numberOfServices = node.Services.Count; int numberOfEvents = 0; + foreach (var actionClient in node.ActionClients) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries( + actionClient.Handle, + out int acNumberOfSubscriptions, + out int acNumberOfGuardConditions, + out int acNumberOfTimers, + out int acNumberOfClients, + out int acNumberOfServices); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries)}() failed."); + + numberOfSubscriptions += acNumberOfSubscriptions; + numberOfGuardConditions += acNumberOfGuardConditions; + numberOfTimers += acNumberOfTimers; + numberOfClients += acNumberOfClients; + numberOfServices += acNumberOfServices; + } + bool waitSetEmpty = numberOfSubscriptions == 0 && numberOfGuardConditions == 0 && numberOfTimers == 0 @@ -667,6 +983,11 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddGuardCondition(waitSetHandle, guardCondition.Handle); } + foreach (var actionClient in node.ActionClients) + { + WaitSetAddActionClient(waitSetHandle, actionClient.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { @@ -729,6 +1050,78 @@ public static void SpinOnce(Node node, long timeout) clientIndex++; } + + foreach (var actionClient in node.ActionClients) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready( + waitSetHandle, + actionClient.Handle, + out bool isFeedbackReady, + out bool isStatusReady, + out bool isGoalResponseReady, + out bool isCancelResponseReady, + out bool isResultResponseReady); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready)}() failed."); + + if (isFeedbackReady) + { + var feedbackMessage = actionClient.CreateFeedbackMessage(); + + var result = TakeFeedbackMessage(actionClient, feedbackMessage); + if (result) + { + actionClient.HandleFeedbackMessage(feedbackMessage); + } + } + + if (isStatusReady) + { + var statusMessage = new GoalStatusArray(); + + var result = TakeStatusMessage(actionClient, statusMessage); + if (result) + { + actionClient.HandleStatusMessage(statusMessage); + } + } + + if (isGoalResponseReady) + { + var goalResponse = actionClient.CreateSendGoalResponse(); + + var result = TakeGoalResponse(actionClient, requestIdHandle, goalResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleGoalResponse(sequenceNumber, goalResponse); + } + } + + if (isCancelResponseReady) + { + var cancelResponse = new CancelGoal_Response(); + + var result = TakeCancelResponse(actionClient, requestIdHandle, cancelResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleCancelResponse(sequenceNumber, cancelResponse); + } + } + + if (isResultResponseReady) + { + var resultResponse = actionClient.CreateGetResultResponse(); + + var result = TakeResultResponse(actionClient, requestIdHandle, resultResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleResultResponse(sequenceNumber, resultResponse); + } + } + } } int guardConditionIndex = 0; diff --git a/rcldotnet/SafeActionClientHandle.cs b/rcldotnet/SafeActionClientHandle.cs new file mode 100644 index 00000000..92391fc0 --- /dev/null +++ b/rcldotnet/SafeActionClientHandle.cs @@ -0,0 +1,75 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_action_client_t + /// + /// + /// Since we need to delete the action client handle before the node is released we need to actually hold a + /// pointer to a rcl_action_client_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the client handle and node handle. + /// + internal sealed class SafeActionClientHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_client_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeActionClientHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_action_destroy_client_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index 5aa04a4b..db01ebe6 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -19,9 +19,17 @@ rmw_implementation_cmake rcl + rcl_actions rmw + action_msgs + builtin_interfaces + unique_identifier_msgs rcl + rcl_actions + action_msgs + builtin_interfaces + unique_identifier_msgs std_msgs test_msgs diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index cee09a14..0e172d3a 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "rosidl_runtime_c/message_type_support_struct.h" @@ -171,6 +172,47 @@ int32_t native_rcl_wait_set_add_guard_condition(void *wait_set_handle, void *gua return ret; } +int32_t native_rcl_action_client_wait_set_get_num_entries( + void *action_client_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services) +{ + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + + size_t num_subscriptions_as_size_t; + size_t num_guard_conditions_as_size_t; + size_t num_timers_as_size_t; + size_t num_clients_as_size_t; + size_t num_services_as_size_t; + + rcl_ret_t ret = rcl_action_client_wait_set_get_num_entities( + action_client, + &num_subscriptions_as_size_t, + &num_guard_conditions_as_size_t, + &num_timers_as_size_t, + &num_clients_as_size_t, + &num_services_as_size_t); + + *num_subscriptions = (int32_t)num_subscriptions_as_size_t; + *num_guard_conditions = (int32_t)num_guard_conditions_as_size_t; + *num_timers = (int32_t)num_timers_as_size_t; + *num_clients = (int32_t)num_clients_as_size_t; + *num_services = (int32_t)num_services_as_size_t; + + return ret; +} + +int32_t native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + rcl_ret_t ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); + + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); @@ -222,6 +264,30 @@ bool native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t in return wait_set->guard_conditions[index] != NULL; } +int32_t native_rcl_action_client_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_client_handle, + bool *is_feedback_ready, + bool *is_status_ready, + bool *is_goal_response_ready, + bool *is_cancel_response_ready, + bool *is_result_response_ready) +{ + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( + wait_set, + action_client, + is_feedback_ready, + is_status_ready, + is_goal_response_ready, + is_cancel_response_ready, + is_result_response_ready); + + return ret; +} + int32_t native_rcl_take(void *subscription_handle, void *message_handle) { rcl_subscription_t * subscription = (rcl_subscription_t *)subscription_handle; @@ -271,3 +337,40 @@ int32_t native_rcl_take_response(void *client_handle, void *request_header_handl return ret; } +int32_t native_rcl_action_take_feedback(void *action_client_handle, void *feedback_message_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_take_feedback(action_client, feedback_message_handle); + return ret; +} + +int32_t native_rcl_action_take_status(void *action_client_handle, void *status_message_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_take_status(action_client, status_message_handle); + return ret; +} + +int32_t native_rcl_action_take_goal_response(void *action_client_handle, void *request_header_handle, void *goal_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_goal_response(action_client, request_header, goal_response_handle); + return ret; +} + +int32_t native_rcl_action_take_cancel_response(void *action_client_handle, void *request_header_handle, void *cancel_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_cancel_response(action_client, request_header, cancel_response_handle); + return ret; +} + +int32_t native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_result_response(action_client, request_header, result_response_handle); + return ret; +} diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 6869fffa..ff06497c 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -72,6 +72,18 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_client(void *wait_set_handle, vo RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_guard_condition_handle(void *wait_set_handle, void *guard_condition_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_num_entries( + void *action_client_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); @@ -87,6 +99,16 @@ bool RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, in RCLDOTNET_EXPORT bool RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_client_handle, + bool *is_feedback_ready, + bool *is_status_ready, + bool *is_goal_response_ready, + bool *is_cancel_response_ready, + bool *is_result_response_ready); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -108,4 +130,19 @@ int32_t RCLDOTNET_CDECL native_rcl_send_response(void *service_handle, void *req RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take_response(void *client_handle, void *request_header_handle, void *response_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_feedback(void *action_client_handle, void *feedback_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_status(void *action_client_handle, void *status_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_goal_response(void *action_client_handle, void *request_header_handle, void *goal_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_response(void *action_client_handle, void *request_header_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle); + #endif // RCLDOTNET_H diff --git a/rcldotnet/rcldotnet_action_client.c b/rcldotnet/rcldotnet_action_client.c new file mode 100644 index 00000000..a03e6491 --- /dev/null +++ b/rcldotnet/rcldotnet_action_client.c @@ -0,0 +1,58 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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 +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcldotnet_client.h" + +int32_t native_rcl_action_send_goal_request(void *action_client_handle, void *goal_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_goal_request(action_client, goal_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_send_result_request(void *action_client_handle, void *result_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_result_request(action_client, result_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_cancel_request(action_client, cancel_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_server_is_available(void *node_handle, void *client_handle, bool *is_available) { + rcl_node_t * node = (rcl_node_t *)node_handle; + rcl_action_client_t * client = (rcl_action_client_t *)client_handle; + + rcl_ret_t ret = rcl_action_server_is_available(node, client, is_available); + return ret; +} diff --git a/rcldotnet/rcldotnet_action_client.h b/rcldotnet/rcldotnet_action_client.h new file mode 100644 index 00000000..93d06d4c --- /dev/null +++ b/rcldotnet/rcldotnet_action_client.h @@ -0,0 +1,32 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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_CLIENT_H +#define RCLDOTNET_CLIENT_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_goal_request(void *action_client_handle, void *goal_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_result_request(void *action_client_handle, void *result_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_is_available(void *node_handle, void *action_client_handle, bool *is_available); + +#endif // RCLDOTNET_CLIENT_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index 40879caa..1844c033 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -20,6 +20,7 @@ #include #include #include +#include #include "rosidl_runtime_c/message_type_support_struct.h" @@ -137,7 +138,7 @@ int32_t native_rcl_create_client_handle(void **client_handle, rcl_client_options_t client_ops = rcl_client_get_default_options(); - rcl_ret_t ret = + rcl_ret_t ret = rcl_client_init(client, node, ts, service_name, &client_ops); *client_handle = (void *)client; @@ -154,3 +155,36 @@ int32_t native_rcl_destroy_client_handle(void *client_handle, void *node_handle) return ret; } + +int32_t native_rcl_action_create_client_handle(void **action_client_handle, + void *node_handle, + const char *action_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_action_type_support_t *ts = + (rosidl_action_type_support_t *)typesupport; + + rcl_action_client_t *action_client = + (rcl_action_client_t *)malloc(sizeof(rcl_action_client_t)); + *action_client = rcl_action_get_zero_initialized_client(); + rcl_action_client_options_t action_client_ops = + rcl_action_client_get_default_options(); + + rcl_ret_t ret = + rcl_action_client_init(action_client, node, ts, action_name, &action_client_ops); + + *action_client_handle = (void *)action_client; + + return ret; +} + +int32_t native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle) { + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_action_client_fini(action_client, node); + free(action_client); + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index 43523984..707cd149 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -51,4 +51,13 @@ int32_t RCLDOTNET_CDECL native_rcl_create_client_handle(void **client_handle, RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_destroy_client_handle(void *client_handle, void *node_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_create_client_handle(void **action_client_handle, + void *node_handle, + const char *action_name, + void *typesupport); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle); + #endif // RCLDOTNET_NODE_H From e1b38dae7f17a6a7d75166646848bdf7a30e02b3 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 29 Jun 2022 14:13:43 +0200 Subject: [PATCH 49/73] actions: add public action server API without implementation --- rcldotnet/ActionServer.cs | 88 ++++++++++++++++++++++++++++ rcldotnet/ActionServerGoalHandle.cs | 90 +++++++++++++++++++++++++++++ rcldotnet/CMakeLists.txt | 2 + rcldotnet/Node.cs | 19 ++++++ 4 files changed, 199 insertions(+) create mode 100644 rcldotnet/ActionServer.cs create mode 100644 rcldotnet/ActionServerGoalHandle.cs diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs new file mode 100644 index 00000000..c6179589 --- /dev/null +++ b/rcldotnet/ActionServer.cs @@ -0,0 +1,88 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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. + */ + +namespace ROS2 +{ + /// + /// A response returned by an action server callback when a goal is requested. + /// + public enum GoalResponse + { + /// + /// Invalid default value. + /// + Default = 0, + + /// + /// The goal is rejected and will not be executed. + /// + Reject = 1, + + // `rclpy` doesn't provide the option to defer the the execution (unless you override the `acceptCallback`) + // Accept = 2, + // Alternative from `rclcpp`: + + /// + /// The server accepts the goal, and is going to begin execution immediately. + /// + AcceptAndExecute = 2, + + /// + /// The server accepts the goal, and is going to execute it later. + /// + AcceptAndDefer = 3, + } + + /// + /// A response returned by an action server callback when a goal has been asked to be canceled. + /// + public enum CancelResponse + { + /// + /// Invalid default value. + /// + Default = 0, + + /// + /// The server will not try to cancel the goal. + /// + Reject = 1, + + /// + /// The server has agreed to try to cancel the goal. + /// + Accept = 2, + } + + public abstract class ActionServer + { + // Only allow internal subclasses. + internal ActionServer() + { + } + } + + public sealed class ActionServer : ActionServer + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionServer() + { + } + } +} diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs new file mode 100644 index 00000000..6daca496 --- /dev/null +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -0,0 +1,90 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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; + +namespace ROS2 +{ + public abstract class ActionServerGoalHandle + { + // Only allow internal subclasses. + internal ActionServerGoalHandle() + { + } + + public abstract Guid GoalId { get; } + + public abstract bool IsActive { get; } + + public abstract bool IsCancelRequested { get; } + + public abstract ActionGoalStatus Status { get; } + + // In `rclpy` this calls the `executeCallback` after setting the state to executing. + // In `rclcpp` this does not call the `executeCallback` (as there is none) but sets the state for the explicit `AcceptAndDefer` `GoalResponse`. + public void Execute() + { + throw new NotImplementedException(); + } + } + + public sealed class ActionServerGoalHandle : ActionServerGoalHandle + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionServerGoalHandle() + { + } + + // `rclpy` uses the name `Request`, but the name from `rclcpp` `Goal` fits better. + public TGoal Goal { get; } + + public override Guid GoalId => throw new NotImplementedException(); + + public override bool IsActive => throw new NotImplementedException(); + + public override bool IsCancelRequested => throw new NotImplementedException(); + + public override ActionGoalStatus Status => throw new NotImplementedException(); + + public void PublishFeedback(TFeedback feedback) => throw new NotImplementedException(); + + + // TODO: (sh) Decide which (or both?) of these methods should be exposed. + // // "rclpy style" + // public void Succeed() => throw new NotImplementedException(); + // public void Abort() => throw new NotImplementedException(); + // public void Canceled() => throw new NotImplementedException(); + + // "rclcpp style" + public void Succeed(TResult result) + { + throw new NotImplementedException(); + } + + public void Abort(TResult result) + { + throw new NotImplementedException(); + } + + public void Canceled(TResult result) + { + throw new NotImplementedException(); + } + } +} diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 9ab60df2..957fc5e3 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -36,6 +36,8 @@ set(CS_SOURCES ActionClientGoalHandle.cs ActionDefinitionStaticMemberCache.cs ActionGoalStatus.cs + ActionServer.cs + ActionServerGoalHandle.cs Client.cs GuardCondition.cs GuidExtensions.cs diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 7d30335c..8ca141bd 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -321,5 +321,24 @@ public ActionClient CreateActionClient`) + // TODO: (sh) Make acceptedCallback an async callback which returns the `TResult` (`Func>`)? + // Add as overload? + // In that case this would be more or less an `executeCallback` from `rclpy`. + public ActionServer CreateActionServer( + string actionName, + Action> acceptedCallback, + // This has an additional `goalId` parameter compared to `rclpy`. + Func goalCallback = null, + Func, CancelResponse> cancelCallback = null + ) + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + throw new NotImplementedException(); + } } } From 41cf05bbab8c830b836af3bfd10286f666f9e97a Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 29 Jun 2022 15:18:33 +0200 Subject: [PATCH 50/73] actions: add missing license header --- rcldotnet/SafeActionClientHandle.cs | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/rcldotnet/SafeActionClientHandle.cs b/rcldotnet/SafeActionClientHandle.cs index 92391fc0..ba42db11 100644 --- a/rcldotnet/SafeActionClientHandle.cs +++ b/rcldotnet/SafeActionClientHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; From fdab8944ac37a3be20ea5df33c1965f108e8d1dd Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 12:05:58 +0200 Subject: [PATCH 51/73] guard conditions: Improve exception message in GuardCondition. --- rcldotnet/GuardCondition.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcldotnet/GuardCondition.cs b/rcldotnet/GuardCondition.cs index c444b940..1c6e9aca 100644 --- a/rcldotnet/GuardCondition.cs +++ b/rcldotnet/GuardCondition.cs @@ -60,7 +60,7 @@ internal GuardCondition(SafeGuardConditionHandle handle, Action callback) public void Trigger() { RCLRet ret = GuardConditionDelegates.native_rcl_trigger_guard_condition(Handle); - RCLExceptionHelper.CheckReturnValue(ret); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(GuardConditionDelegates.native_rcl_trigger_guard_condition)}() failed."); } internal void TriggerCallback() From 3b71121f57120487ba0ca1eec9bfd67a70a7f982 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 12:51:15 +0200 Subject: [PATCH 52/73] actions: Change IsCancelRequested to IsCanceling, add IsExecuting --- rcldotnet/ActionServerGoalHandle.cs | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs index 6daca496..3a4198c6 100644 --- a/rcldotnet/ActionServerGoalHandle.cs +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -26,9 +26,23 @@ internal ActionServerGoalHandle() public abstract Guid GoalId { get; } + /// + /// Indicate if goal is pending or executing. + /// + /// False if goal has reached a terminal state. public abstract bool IsActive { get; } - public abstract bool IsCancelRequested { get; } + /// + /// Indicate if client has requested this goal be cancelled. + /// + /// True if a cancellation request has been accepted for this goal. + public abstract bool IsCanceling { get; } + + /// + /// Indicate if goal is executing. + /// + /// True only if the goal is in an executing state. + public abstract bool IsExecuting { get; } public abstract ActionGoalStatus Status { get; } @@ -58,7 +72,9 @@ internal ActionServerGoalHandle() public override bool IsActive => throw new NotImplementedException(); - public override bool IsCancelRequested => throw new NotImplementedException(); + public override bool IsCanceling => throw new NotImplementedException(); + + public override bool IsExecuting => throw new NotImplementedException(); public override ActionGoalStatus Status => throw new NotImplementedException(); From b2e46d9178479d40964d1132059fde22f87788d6 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 13:41:42 +0200 Subject: [PATCH 53/73] actions: Add ReadFromMessageHandle/WriteToMessageHandle helper mehtods --- rcldotnet/ActionClient.cs | 57 +--------- rcldotnet/Client.cs | 19 +--- rcldotnet/Publisher.cs | 19 +--- rcldotnet/RCLdotnet.cs | 232 ++++++++++---------------------------- 4 files changed, 63 insertions(+), 264 deletions(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index 0e058918..a8f52c08 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -183,24 +183,7 @@ private Task> SendGoa using (var goalRequestHandle = ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - goalRequestHandle.DangerousAddRef(ref mustRelease); - goalRequest.__WriteToHandle(goalRequestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - goalRequestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(goalRequest, goalRequestHandle); RCLRet ret = ActionClientDelegates.native_rcl_action_send_goal_request(Handle, goalRequestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_goal_request)}() failed."); @@ -230,24 +213,7 @@ internal Task CancelGoalAsync(ActionClientGoalHandle goalHa using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - cancelRequestHandle.DangerousAddRef(ref mustRelease); - cancelRequest.__WriteToHandle(cancelRequestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - cancelRequestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(cancelRequest, cancelRequestHandle); RCLRet ret = ActionClientDelegates.native_rcl_action_send_cancel_request(Handle, cancelRequestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_cancel_request)}() failed."); @@ -276,24 +242,7 @@ internal Task GetResultAsync( using (var resultRequestHandle = ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - resultRequestHandle.DangerousAddRef(ref mustRelease); - resultRequest.__WriteToHandle(resultRequestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - resultRequestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(resultRequest, resultRequestHandle); RCLRet ret = ActionClientDelegates.native_rcl_action_send_result_request(Handle, resultRequestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_result_request)}() failed."); diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 97bda29e..9e0b1e66 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -108,24 +108,7 @@ public Task SendRequestAsync(TRequest request) using (var requestHandle = MessageStaticMemberCache.CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - requestHandle.DangerousAddRef(ref mustRelease); - request.__WriteToHandle(requestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - requestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(request, requestHandle); RCLRet ret = ClientDelegates.native_rcl_send_request(Handle, requestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_send_request)}() failed."); diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index ef99966d..02198057 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -73,24 +73,7 @@ public void Publish(T message) { using (var messageHandle = MessageStaticMemberCache.CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__WriteToHandle(messageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(message, messageHandle); RCLRet ret = PublisherDelegates.native_rcl_publish(Handle, messageHandle); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(PublisherDelegates.native_rcl_publish)}() failed."); diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 8b61ffd6..2f4ac0de 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -583,25 +583,7 @@ private static bool Take(Subscription subscription, IRosMessage message) switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__ReadFromHandle(messageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(message, messageHandle); return true; case RCLRet.SubscriptionTakeFailed: @@ -621,25 +603,7 @@ private static bool TakeRequest(Service service, SafeRequestIdHandle requestHead switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - requestHandle.DangerousAddRef(ref mustRelease); - request.__ReadFromHandle(requestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - requestHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(request, requestHandle); return true; case RCLRet.ServiceTakeFailed: @@ -659,25 +623,7 @@ private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeade switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__ReadFromHandle(responseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(response, responseHandle); return true; case RCLRet.ClientTakeFailed: @@ -697,25 +643,7 @@ private static bool TakeFeedbackMessage(ActionClient actionClient, IRosMessage f switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - feedbackMessageHandle.DangerousAddRef(ref mustRelease); - feedbackMessage.__ReadFromHandle(feedbackMessageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - feedbackMessageHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(feedbackMessage, feedbackMessageHandle); return true; case RCLRet.SubscriptionTakeFailed: @@ -735,25 +663,7 @@ private static bool TakeStatusMessage(ActionClient actionClient, IRosMessage sta switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - statusMessageHandle.DangerousAddRef(ref mustRelease); - statusMessage.__ReadFromHandle(statusMessageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - statusMessageHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(statusMessage, statusMessageHandle); return true; case RCLRet.SubscriptionTakeFailed: @@ -773,25 +683,7 @@ private static bool TakeGoalResponse(ActionClient actionClient, SafeRequestIdHan switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - goalResponseHandle.DangerousAddRef(ref mustRelease); - goalResponse.__ReadFromHandle(goalResponseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - goalResponseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(goalResponse, goalResponseHandle); return true; case RCLRet.ClientTakeFailed: @@ -811,25 +703,7 @@ private static bool TakeCancelResponse(ActionClient actionClient, SafeRequestIdH switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - cancelResponseHandle.DangerousAddRef(ref mustRelease); - cancelResponse.__ReadFromHandle(cancelResponseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - cancelResponseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(cancelResponse, cancelResponseHandle); return true; case RCLRet.ClientTakeFailed: @@ -849,25 +723,7 @@ private static bool TakeResultResponse(ActionClient actionClient, SafeRequestIdH switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - resultResponseHandle.DangerousAddRef(ref mustRelease); - resultResponse.__ReadFromHandle(resultResponseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - resultResponseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(resultResponse, resultResponseHandle); return true; case RCLRet.ClientTakeFailed: @@ -883,30 +739,10 @@ private static void SendResponse(Service service, SafeRequestIdHandle requestHea { using (var responseHandle = service.CreateResponseHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__WriteToHandle(responseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } - } + WriteToMessageHandle(response, responseHandle); RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); - if (ret != RCLRet.Ok) - { - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); - } + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); } } @@ -1156,5 +992,53 @@ public static string GetRMWIdentifier() string rmw_identifier = Marshal.PtrToStringAnsi(ptr); return rmw_identifier; } + + internal static void ReadFromMessageHandle(IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as + // this needs to be handled in generated code across multiple + // assemblies. Array and collection indexing would need to + // create SafeHandles everywhere. It's not worth it, especially + // considering the extra allocations for SafeHandles in arrays + // or collections that don't really represent their own native + // resource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__ReadFromHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } + + internal static void WriteToMessageHandle(IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as + // this needs to be handled in generated code across multiple + // assemblies. Array and collection indexing would need to + // create SafeHandles everywhere. It's not worth it, especially + // considering the extra allocations for SafeHandles in arrays + // or collections that don't really represent their own native + // resource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__WriteToHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } } } From e6639a6e7aed5921f3fb875be2dee86abe41d3e6 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 14:14:50 +0200 Subject: [PATCH 54/73] actions: implement basic action server --- rcldotnet/ActionServer.cs | 392 +++++++++++++++++++++++++++- rcldotnet/ActionServerGoalHandle.cs | 104 +++++++- rcldotnet/CMakeLists.txt | 2 + rcldotnet/Node.cs | 70 ++++- rcldotnet/RCLdotnet.cs | 360 +++++++++++++++++++++++++ rcldotnet/SafeActionGoalHandle.cs | 45 ++++ rcldotnet/SafeActionServerHandle.cs | 90 +++++++ rcldotnet/rcldotnet.c | 269 +++++++++++++++++++ rcldotnet/rcldotnet.h | 71 +++++ rcldotnet/rcldotnet_node.c | 36 +++ rcldotnet/rcldotnet_node.h | 9 + 11 files changed, 1432 insertions(+), 16 deletions(-) create mode 100644 rcldotnet/SafeActionGoalHandle.cs create mode 100644 rcldotnet/SafeActionServerHandle.cs diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs index c6179589..6db438b5 100644 --- a/rcldotnet/ActionServer.cs +++ b/rcldotnet/ActionServer.cs @@ -13,6 +13,16 @@ * limitations under the License. */ +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.InteropServices; +using System.Threading.Tasks; +using action_msgs.msg; +using action_msgs.srv; +using unique_identifier_msgs.msg; + namespace ROS2 { /// @@ -66,12 +76,44 @@ public enum CancelResponse Accept = 2, } + internal enum ActionGoalEvent + { + // These values need to mirror rcl_action_goal_event_t. + + Execute = 0, + CancelGoal, + Succeed, + Abort, + Canceled, + } + public abstract class ActionServer { // Only allow internal subclasses. internal ActionServer() { } + + // ActionServer does intentionally (for now) not implement IDisposable + // as this needs some extra consideration how the type works after its + // internal handle is disposed. By relying on the GC/Finalizer of + // SafeHandle the handle only gets Disposed if the action client is not + // live anymore. + internal abstract SafeActionServerHandle Handle { get; } + + internal abstract IRosMessage CreateSendGoalRequest(); + internal abstract SafeHandle CreateSendGoalRequestHandle(); + + internal abstract IRosActionGetResultRequest CreateGetResultRequest(); + internal abstract SafeHandle CreateGetResultRequestHandle(); + + internal abstract void HandleGoalRequest(SafeRequestIdHandle requestIdHandle, IRosMessage goalRequest); + + internal abstract void HandleCancelRequest(SafeRequestIdHandle requestIdHandle, CancelGoal_Request cancelRequest); + + internal abstract void HandleResultRequest(SafeRequestIdHandle requestIdHandle, IRosActionGetResultRequest resultRequest); + + internal abstract void HandleGoalExpired(); } public sealed class ActionServer : ActionServer @@ -80,9 +122,357 @@ public sealed class ActionServer : ActionSer where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + private readonly Action> _acceptedCallback; + + private readonly Func _goalCallback; + + private readonly Func, CancelResponse> _cancelCallback; + + private readonly ConcurrentDictionary> _goalHandles + = new ConcurrentDictionary>(); + // No public constructor. - internal ActionServer() + internal ActionServer( + SafeActionServerHandle handle, + Action> acceptedCallback, + Func goalCallback, + Func, CancelResponse> cancelCallback) + { + Handle = handle; + _acceptedCallback = acceptedCallback; + _goalCallback = goalCallback; + _cancelCallback = cancelCallback; + } + + internal override SafeActionServerHandle Handle { get; } + + internal override IRosMessage CreateSendGoalRequest() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalRequest(); + } + + internal override SafeHandle CreateSendGoalRequestHandle() { + return ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle(); + } + + internal override IRosActionGetResultRequest CreateGetResultRequest() + { + return ActionDefinitionStaticMemberCache.CreateGetResultRequest(); + } + + internal override SafeHandle CreateGetResultRequestHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle(); + } + + internal IRosActionGetResultResponse CreateGetResultResponse() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponse(); + } + + private SafeHandle CreateGetResultResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponseHandle(); + } + + private IRosActionSendGoalResponse CreateSendGoalResponse() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponse(); + } + + private SafeHandle CreateSendGoalResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponseHandle(); + } + + private SafeHandle CreateFeedbackMessageHandle() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessageHandle(); + } + + internal override void HandleGoalRequest(SafeRequestIdHandle requestIdHandle, IRosMessage goalRequest) + { + var goalRequestCasted = (IRosActionSendGoalRequest)goalRequest; + + var goal = goalRequestCasted.Goal; + var uuid = (UUID)goalRequestCasted.GoalIdAsRosMessage; + var goalId = uuid.ToGuid(); + + // TODO: check if a goalHandle for the goalId already exists. + // + // rclpy does this before calling the goalCallback, rclcpp doesn't + // do this and relies on rcl_action_accept_new_goal to return NULL. + // In the rclpy case goalCallback doesn't get called, in rclcpp it + // does. + // rclpy logs this, sends a not accepted response. + // rclcpp throws an error without logging and sending an response. + // Currently this behaves like rclcpp. + + var goalResponse = _goalCallback(goalId, goal); + + var accepted = + goalResponse == GoalResponse.AcceptAndExecute || + goalResponse == GoalResponse.AcceptAndDefer; + + ActionServerGoalHandle goalHandle = null; + + // goalInfo.Stamp will be filled in by native_rcl_action_accept_new_goal(). + GoalInfo goalInfo = null; + + if (accepted) + { + goalInfo = new GoalInfo(); + + goalInfo.GoalId.Uuid = goalId.ToUuidByteArray(); + + using (var goalInfoHandle = GoalInfo.__CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(goalInfo, goalInfoHandle); + + var actionGoalHandle = new SafeActionGoalHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_action_accept_new_goal(ref actionGoalHandle, Handle, goalInfoHandle); + if (ret != RCLRet.Ok) + { + actionGoalHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_accept_new_goal)}() failed."); + } + + // Read back goalInfo.Stamp. + RCLdotnet.ReadFromMessageHandle(goalInfo, goalInfoHandle); + + goalHandle = new ActionServerGoalHandle(actionGoalHandle, this, goalId, goal); + + if (!_goalHandles.TryAdd(goalId, goalHandle)) + { + throw new Exception($"Two goals were accepted with the same Id '{goalId}'."); + }; + + if (goalResponse == GoalResponse.AcceptAndExecute) + { + ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(goalHandle.Handle, ActionGoalEvent.Execute); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_update_goal_state)}() failed."); + } + } + } + + var response = CreateSendGoalResponse(); + response.Accepted = accepted; + + if (accepted) + { + Debug.Assert(goalInfo != null); + + // TODO: (sh) File Issues for different handling of this in rclcpp or rclpy? + // rclpy fills in the response.Stamp but with a different timestamp than rcl_action_accept_new_goal() restamps. + // rclcpp doesn't fill in the stamp at all. + + // Return the stamp that native_rcl_action_accept_new_goal filled in. + response.StampAsRosMessage = goalInfo.Stamp; + } + + SendGoalResponse(requestIdHandle, response); + + if (accepted) + { + Debug.Assert(goalHandle != null); + + PublishStatus(); + + _acceptedCallback(goalHandle); + } + } + + private void SendGoalResponse(SafeRequestIdHandle requestHeaderHandle, IRosMessage goalResponse) + { + using (var goalResponseHandle = CreateSendGoalResponseHandle()) + { + RCLdotnet.WriteToMessageHandle(goalResponse, goalResponseHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_send_goal_response(Handle, requestHeaderHandle, goalResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_goal_response)}() failed."); + } + } + + internal override void HandleCancelRequest(SafeRequestIdHandle requestIdHandle, CancelGoal_Request cancelRequest) + { + var cancelResponse = new CancelGoal_Response(); + + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + using (var cancelResponseHandle = CancelGoal_Response.__CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(cancelRequest, cancelRequestHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_process_cancel_request(Handle, cancelRequestHandle, cancelResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_process_cancel_request)}() failed."); + + RCLdotnet.ReadFromMessageHandle(cancelResponse, cancelResponseHandle); + + var goalsCancelingFromRcl = cancelResponse.GoalsCanceling; + + // Create and fill in new list for the goals the callback accepts to cancel. + cancelResponse.GoalsCanceling = new List(); + + foreach (var goalInfo in goalsCancelingFromRcl) + { + var goalId = goalInfo.GoalId.ToGuid(); + + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + var response = _cancelCallback(goalHandle); + + if (response == CancelResponse.Accept) + { + ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(goalHandle.Handle, ActionGoalEvent.CancelGoal); + if (ret == RCLRet.Ok) + { + cancelResponse.GoalsCanceling.Add(goalInfo); + } + else + { + // If the goal's just succeeded after user cancel callback + // that will generate an exception from invalid transition. + // -> Remove from response since goal has been succeeded. + + // TODO: (sh) Logging. + + // Don't add to goalsForResponse. + } + } + } + else + { + // Possibly the user doesn't care to track the goal handle + // Remove from response. + // or: + // We can't cancel a goal for a goalHandle we don't have. + + // Don't add to goalsForResponse. + } + } + + // TODO: (sh) rclpy doesn't set this? -> Create issue there? + // If the user rejects all individual requests to cancel goals, + // then we consider the top-level cancel request as rejected. + if (goalsCancelingFromRcl.Count > 0 && cancelResponse.GoalsCanceling.Count == 0) + { + cancelResponse.ReturnCode = CancelGoal_Response.ERROR_REJECTED; + } + + if (cancelResponse.GoalsCanceling.Count > 0) + { + // At least one goal state changed, publish a new status message. + PublishStatus(); + } + + RCLdotnet.WriteToMessageHandle(cancelResponse, cancelResponseHandle); + + ret = RCLdotnetDelegates.native_rcl_action_send_cancel_response(Handle, requestIdHandle, cancelResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_cancel_response)}() failed."); + } + } + + internal override void HandleResultRequest(SafeRequestIdHandle requestIdHandle, IRosActionGetResultRequest resultRequest) + { + var uuid = (UUID)resultRequest.GoalIdAsRosMessage; + var goalId = uuid.ToGuid(); + + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + // Using Task.ContinueWith here should deal with all the multi-threaded race conditions. + goalHandle.ResultTask.ContinueWith(ResultTaskContinuationAction, requestIdHandle, TaskContinuationOptions.ExecuteSynchronously); + } + else + { + try + { + var resultResponse = CreateGetResultResponse(); + resultResponse.Status = GoalStatus.STATUS_UNKNOWN; + SendResultResponse(requestIdHandle, resultResponse); + } + finally + { + requestIdHandle.Dispose(); + } + } + } + + private void ResultTaskContinuationAction(Task> resultTask, object state) + { + var requestIdHandle = (SafeRequestIdHandle)state; + + try + { + SendResultResponse(requestIdHandle, resultTask.Result); + } + finally + { + requestIdHandle.Dispose(); + } + } + + private void SendResultResponse(SafeRequestIdHandle requestHeaderHandle, IRosActionGetResultResponse resultResponse) + { + using (var resultResponseHandle = CreateGetResultResponseHandle()) + { + RCLdotnet.WriteToMessageHandle(resultResponse, resultResponseHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_send_result_response(Handle, requestHeaderHandle, resultResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_result_response)}() failed."); + } + } + + internal override void HandleGoalExpired() + { + // This expects only one goal to expire at a time (rclcpp does the same). + // Loop until no new goals are expired. + var goalInfo = new GoalInfo(); + int numExpired; + + using (var goalInfoHandle = GoalInfo.__CreateMessageHandle()) + { + do + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_expire_goals(Handle, goalInfoHandle, out numExpired); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_expire_goals)}() failed."); + + if (numExpired > 0) + { + RCLdotnet.ReadFromMessageHandle(goalInfo, goalInfoHandle); + Guid goalId = goalInfo.GoalId.ToGuid(); + + _goalHandles.TryRemove(goalId, out _); + } + } while (numExpired > 0); + } + } + + internal void HandleTerminalState( + ActionServerGoalHandle actionServerGoalHandle, + IRosActionGetResultResponse response) + { + actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); + PublishStatus(); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_notify_goal_done(Handle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_notify_goal_done)}() failed."); + } + + internal void PublishStatus() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_publish_status(Handle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_publish_status)}() failed."); + } + + internal void PublishFeedbackMessage(IRosMessage feedbackMessage) + { + using (var feedbackMessageHandle = CreateFeedbackMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(feedbackMessage, feedbackMessageHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_publish_feedback(Handle, feedbackMessageHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_publish_feedback)}() failed."); + } } } } diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs index 3a4198c6..f5d3e626 100644 --- a/rcldotnet/ActionServerGoalHandle.cs +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -14,6 +14,9 @@ */ using System; +using System.Threading.Tasks; +using action_msgs.msg; +using unique_identifier_msgs.msg; namespace ROS2 { @@ -46,12 +49,11 @@ internal ActionServerGoalHandle() public abstract ActionGoalStatus Status { get; } + internal abstract SafeActionGoalHandle Handle { get; } + // In `rclpy` this calls the `executeCallback` after setting the state to executing. // In `rclcpp` this does not call the `executeCallback` (as there is none) but sets the state for the explicit `AcceptAndDefer` `GoalResponse`. - public void Execute() - { - throw new NotImplementedException(); - } + public abstract void Execute(); } public sealed class ActionServerGoalHandle : ActionServerGoalHandle @@ -60,26 +62,79 @@ public sealed class ActionServerGoalHandle : where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + private readonly ActionServer _actionServer; + // No public constructor. - internal ActionServerGoalHandle() + internal ActionServerGoalHandle( + SafeActionGoalHandle handle, + ActionServer actionServer, + Guid goalId, + TGoal goal) { + Handle = handle; + _actionServer = actionServer; + Goal = goal; + GoalId = goalId; + ResultTaskCompletionSource = new TaskCompletionSource>(); } // `rclpy` uses the name `Request`, but the name from `rclcpp` `Goal` fits better. public TGoal Goal { get; } - public override Guid GoalId => throw new NotImplementedException(); + public override Guid GoalId { get; } + + public override bool IsActive + { + get + { + bool isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); + return isActive; + } + } + + public override bool IsCanceling => Status == ActionGoalStatus.Canceling; + + public override bool IsExecuting => Status == ActionGoalStatus.Executing; + + public override ActionGoalStatus Status + { + get + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_goal_handle_get_status(Handle, out byte status); + + // In .NET properties should not throw exceptions. Should this + // be converted to a method for this reason? -> No as the rcl + // methods only do argument null checks for values which + // rcldotnet should ensure that they are not null. + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_goal_handle_get_status)}() failed."); + + return (ActionGoalStatus)status; + } + } + + internal override SafeActionGoalHandle Handle { get; } - public override bool IsActive => throw new NotImplementedException(); + internal TaskCompletionSource> ResultTaskCompletionSource { get; } - public override bool IsCanceling => throw new NotImplementedException(); + internal Task> ResultTask => ResultTaskCompletionSource.Task; - public override bool IsExecuting => throw new NotImplementedException(); + public override void Execute() + { + UpdateGoalState(ActionGoalEvent.Execute); - public override ActionGoalStatus Status => throw new NotImplementedException(); + _actionServer.PublishStatus(); + } - public void PublishFeedback(TFeedback feedback) => throw new NotImplementedException(); + public void PublishFeedback(TFeedback feedback) + { + IRosActionFeedbackMessage feedbackMessage = + ActionDefinitionStaticMemberCache.CreateFeedbackMessage(); + var goalId = (UUID)feedbackMessage.GoalIdAsRosMessage; + goalId.Uuid = GoalId.ToUuidByteArray(); + feedbackMessage.Feedback = feedback; + _actionServer.PublishFeedbackMessage(feedbackMessage); + } // TODO: (sh) Decide which (or both?) of these methods should be exposed. // // "rclpy style" @@ -90,17 +145,38 @@ internal ActionServerGoalHandle() // "rclcpp style" public void Succeed(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Succeed); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_SUCCEEDED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); } public void Abort(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Abort); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_ABORTED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); } public void Canceled(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Canceled); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_CANCELED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); + } + + private void UpdateGoalState(ActionGoalEvent actionGoalEvent) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(Handle, actionGoalEvent); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_update_goal_state)}() failed."); } } } diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 957fc5e3..c6c3dd9d 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -48,6 +48,8 @@ set(CS_SOURCES RCLExceptionHelper.cs RCLRet.cs SafeActionClientHandle.cs + SafeActionGoalHandle.cs + SafeActionServerHandle.cs SafeClientHandle.cs SafeGuardConditionHandle.cs SafeNodeHandle.cs diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 8ca141bd..ce877a13 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -84,6 +84,18 @@ internal delegate RCLRet NativeRCLActionDestroyClientHandleType( internal static NativeRCLActionDestroyClientHandleType native_rcl_action_destroy_client_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionCreateServerHandleType( + ref SafeActionServerHandle actionServerHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); + + internal static NativeRCLActionCreateServerHandleType native_rcl_action_create_server_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyServerHandleType( + IntPtr actionServerHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLActionDestroyServerHandleType native_rcl_action_destroy_server_handle = null; + static NodeDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -158,6 +170,18 @@ static NodeDelegates() NodeDelegates.native_rcl_action_destroy_client_handle = (NativeRCLActionDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_action_destroy_client_handle_ptr, typeof(NativeRCLActionDestroyClientHandleType)); + + IntPtr native_rcl_action_create_server_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_create_server_handle"); + NodeDelegates.native_rcl_action_create_server_handle = + (NativeRCLActionCreateServerHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_create_server_handle_ptr, typeof(NativeRCLActionCreateServerHandleType)); + + IntPtr native_rcl_action_destroy_server_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_destroy_server_handle"); + NodeDelegates.native_rcl_action_destroy_server_handle = + (NativeRCLActionDestroyServerHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_destroy_server_handle_ptr, typeof(NativeRCLActionDestroyServerHandleType)); } } @@ -173,6 +197,8 @@ public sealed class Node private readonly IList _actionClients; + private readonly IList _actionServers; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -181,6 +207,7 @@ internal Node(SafeNodeHandle handle) _clients = new List(); _guardConditions = new List(); _actionClients = new List(); + _actionServers = new List(); } public IList Subscriptions => _subscriptions; @@ -196,6 +223,8 @@ internal Node(SafeNodeHandle handle) public IList ActionClients => _actionClients; + public IList ActionServers => _actionServers; + // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its // internal handle is disposed. @@ -338,7 +367,46 @@ public ActionServer CreateActionServer.GetTypeSupport(); + + var actionServerHandle = new SafeActionServerHandle(); + RCLRet ret = NodeDelegates.native_rcl_action_create_server_handle(ref actionServerHandle, Handle, actionName, typeSupport); + actionServerHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + actionServerHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_action_create_server_handle)}() failed."); + } + + // TODO: (sh) Add actionName to ActionServer. + var actionServer = new ActionServer( + actionServerHandle, + acceptedCallback, + goalCallback, + cancelCallback); + + _actionServers.Add(actionServer); + return actionServer; + } + + private static GoalResponse DefaultGoalCallback(Guid goalId, TGoal goal) + { + return GoalResponse.AcceptAndExecute; + } + + private static CancelResponse DefaultCancelCallback(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Reject; } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 2f4ac0de..694c9846 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -192,6 +192,24 @@ internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( internal static NativeRCLActionClientWaitSetGetEntitiesReadyType native_rcl_action_client_wait_set_get_entities_ready = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerWaitSetGetNumEntriesType( + SafeActionServerHandle actionServerHandle, out int numSubscriptions, out int numGuardConditions, out int numTimers, out int numClients, out int numServices); + + internal static NativeRCLActionServerWaitSetGetNumEntriesType native_rcl_action_server_wait_set_get_num_entries = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionWaitSetAddActionServerType( + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle); + + internal static NativeRCLActionWaitSetAddActionServerType native_rcl_action_wait_set_add_action_server = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerWaitSetGetEntitiesReadyType( + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle, out bool isGoalRequestReady, out bool isCancelRequestReady, out bool isResultRequestReady, out bool isGoalExpired); + + internal static NativeRCLActionServerWaitSetGetEntitiesReadyType native_rcl_action_server_wait_set_get_entities_ready = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionTakeFeedbackType( SafeActionClientHandle actionClientHandle, SafeHandle feedbackMessageHandle); @@ -223,6 +241,90 @@ internal delegate RCLRet NativeRCLActionTakeResultResponseType( internal static NativeRCLActionTakeResultResponseType native_rcl_action_take_result_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeGoalRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle goalRequestHandle); + + internal static NativeRCLActionTakeGoalRequestType native_rcl_action_take_goal_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendGoalResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle gaolResponseHandle); + + internal static NativeRCLActionSendGoalResponseType native_rcl_action_send_goal_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionAcceptNewGoalType( + ref SafeActionGoalHandle actionGoalHandleHandle, SafeActionServerHandle actionServerHandle, SafeHandle goalInfoHandle); + + internal static NativeRCLActionAcceptNewGoalType native_rcl_action_accept_new_goal = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyGoalHandleType(IntPtr actionGoalHandleHandle); + + internal static NativeRCLActionDestroyGoalHandleType native_rcl_action_destroy_goal_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionUpdateGoalStateType( + SafeActionGoalHandle actionGoalHandleHandle, ActionGoalEvent goalEvent); + + internal static NativeRCLActionUpdateGoalStateType native_rcl_action_update_goal_state = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionPublishStatusType(SafeActionServerHandle actionServerHandle); + internal static NativeRCLActionPublishStatusType native_rcl_action_publish_status = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionPublishFeedbackType(SafeActionServerHandle actionServerHandle, SafeHandle feedbackMessageHandle); + internal static NativeRCLActionPublishFeedbackType native_rcl_action_publish_feedback = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeCancelRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelRequestHandle); + + internal static NativeRCLActionTakeCancelRequestType native_rcl_action_take_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionProcessCancelRequestType( + SafeActionServerHandle actionServerHandle, SafeHandle cancelRequestHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionProcessCancelRequestType native_rcl_action_process_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendCancelResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionSendCancelResponseType native_rcl_action_send_cancel_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeResultRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultRequestHandle); + + internal static NativeRCLActionTakeResultRequestType native_rcl_action_take_result_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendResultResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultResponseHandle); + + internal static NativeRCLActionSendResultResponseType native_rcl_action_send_result_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionNotifyGoalDoneType(SafeActionServerHandle actionServerHandle); + internal static NativeRCLActionNotifyGoalDoneType native_rcl_action_notify_goal_done = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionExpireGoalsType( + SafeActionServerHandle actionServerHandle, SafeHandle goalInfoHandle, out int numExpired); + + internal static NativeRCLActionExpireGoalsType native_rcl_action_expire_goals = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate bool NativeRCLActionGoalHandleIsActiveType(SafeActionGoalHandle actionGoalHandleHandle); + internal static NativeRCLActionGoalHandleIsActiveType native_rcl_action_goal_handle_is_active = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionGoalHandleGetStatusType(SafeActionGoalHandle actionGoalHandleHandle, out byte status); + internal static NativeRCLActionGoalHandleGetStatusType native_rcl_action_goal_handle_get_status = null; static RCLdotnetDelegates() { @@ -415,6 +517,24 @@ static RCLdotnetDelegates() (NativeRCLActionClientWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( native_rcl_action_client_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionClientWaitSetGetEntitiesReadyType)); + IntPtr native_rcl_action_server_wait_set_get_num_entries_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_wait_set_get_num_entries"); + RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries = + (NativeRCLActionServerWaitSetGetNumEntriesType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_wait_set_get_num_entries_ptr, typeof(NativeRCLActionServerWaitSetGetNumEntriesType)); + + IntPtr native_rcl_action_wait_set_add_action_server_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_wait_set_add_action_server"); + RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server = + (NativeRCLActionWaitSetAddActionServerType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_wait_set_add_action_server_ptr, typeof(NativeRCLActionWaitSetAddActionServerType)); + + IntPtr native_rcl_action_server_wait_set_get_entities_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_wait_set_get_entities_ready"); + RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready = + (NativeRCLActionServerWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionServerWaitSetGetEntitiesReadyType)); + IntPtr native_rcl_action_take_feedback_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_feedback"); RCLdotnetDelegates.native_rcl_action_take_feedback @@ -445,6 +565,101 @@ static RCLdotnetDelegates() (NativeRCLActionTakeResultResponseType)Marshal.GetDelegateForFunctionPointer( native_rcl_action_take_result_response_ptr, typeof(NativeRCLActionTakeResultResponseType)); + IntPtr native_rcl_action_take_goal_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_goal_request"); + RCLdotnetDelegates.native_rcl_action_take_goal_request = + (NativeRCLActionTakeGoalRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_goal_request_ptr, typeof(NativeRCLActionTakeGoalRequestType)); + + IntPtr native_rcl_action_send_goal_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_goal_response"); + RCLdotnetDelegates.native_rcl_action_send_goal_response = + (NativeRCLActionSendGoalResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_goal_response_ptr, typeof(NativeRCLActionSendGoalResponseType)); + + IntPtr native_rcl_action_accept_new_goal_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_accept_new_goal"); + RCLdotnetDelegates.native_rcl_action_accept_new_goal = + (NativeRCLActionAcceptNewGoalType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_accept_new_goal_ptr, typeof(NativeRCLActionAcceptNewGoalType)); + + IntPtr native_rcl_action_destroy_goal_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_destroy_goal_handle"); + RCLdotnetDelegates.native_rcl_action_destroy_goal_handle = + (NativeRCLActionDestroyGoalHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_destroy_goal_handle_ptr, typeof(NativeRCLActionDestroyGoalHandleType)); + + IntPtr native_rcl_action_update_goal_state_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_update_goal_state"); + RCLdotnetDelegates.native_rcl_action_update_goal_state = + (NativeRCLActionUpdateGoalStateType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_update_goal_state_ptr, typeof(NativeRCLActionUpdateGoalStateType)); + + IntPtr native_rcl_action_publish_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_publish_status"); + RCLdotnetDelegates.native_rcl_action_publish_status = + (NativeRCLActionPublishStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_publish_status_ptr, typeof(NativeRCLActionPublishStatusType)); + + IntPtr native_rcl_action_publish_feedback_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_publish_feedback"); + RCLdotnetDelegates.native_rcl_action_publish_feedback = + (NativeRCLActionPublishFeedbackType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_publish_feedback_ptr, typeof(NativeRCLActionPublishFeedbackType)); + + IntPtr native_rcl_action_take_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_cancel_request"); + RCLdotnetDelegates.native_rcl_action_take_cancel_request = + (NativeRCLActionTakeCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_cancel_request_ptr, typeof(NativeRCLActionTakeCancelRequestType)); + + IntPtr native_rcl_action_process_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_process_cancel_request"); + RCLdotnetDelegates.native_rcl_action_process_cancel_request = + (NativeRCLActionProcessCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_process_cancel_request_ptr, typeof(NativeRCLActionProcessCancelRequestType)); + + IntPtr native_rcl_action_send_cancel_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_cancel_response"); + RCLdotnetDelegates.native_rcl_action_send_cancel_response = + (NativeRCLActionSendCancelResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_cancel_response_ptr, typeof(NativeRCLActionSendCancelResponseType)); + + IntPtr native_rcl_action_take_result_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_result_request"); + RCLdotnetDelegates.native_rcl_action_take_result_request = + (NativeRCLActionTakeResultRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_result_request_ptr, typeof(NativeRCLActionTakeResultRequestType)); + + IntPtr native_rcl_action_send_result_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_result_response"); + RCLdotnetDelegates.native_rcl_action_send_result_response = + (NativeRCLActionSendResultResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_result_response_ptr, typeof(NativeRCLActionSendResultResponseType)); + + IntPtr native_rcl_action_notify_goal_done_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_notify_goal_done"); + RCLdotnetDelegates.native_rcl_action_notify_goal_done = + (NativeRCLActionNotifyGoalDoneType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_notify_goal_done_ptr, typeof(NativeRCLActionNotifyGoalDoneType)); + + IntPtr native_rcl_action_expire_goals_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_expire_goals"); + RCLdotnetDelegates.native_rcl_action_expire_goals = + (NativeRCLActionExpireGoalsType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_expire_goals_ptr, typeof(NativeRCLActionExpireGoalsType)); + + IntPtr native_rcl_action_goal_handle_is_active_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_goal_handle_is_active"); + RCLdotnetDelegates.native_rcl_action_goal_handle_is_active = + (NativeRCLActionGoalHandleIsActiveType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_goal_handle_is_active_ptr, typeof(NativeRCLActionGoalHandleIsActiveType)); + + IntPtr native_rcl_action_goal_handle_get_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_goal_handle_get_status"); + RCLdotnetDelegates.native_rcl_action_goal_handle_get_status = + (NativeRCLActionGoalHandleGetStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_goal_handle_get_status_ptr, typeof(NativeRCLActionGoalHandleGetStatusType)); } } @@ -542,6 +757,12 @@ private static void WaitSetAddActionClient(SafeWaitSetHandle waitSetHandle, Safe RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client)}() failed."); } + private static void WaitSetAddActionServer(SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server(waitSetHandle, actionServerHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server)}() failed."); + } + /// /// Block until the wait set is ready or until the timeout has been exceeded. /// @@ -746,6 +967,66 @@ private static void SendResponse(Service service, SafeRequestIdHandle requestHea } } + private static bool TakeGoalRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage goalRequest) + { + using (var goalRequestHandle = actionServer.CreateSendGoalRequestHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_goal_request(actionServer.Handle, requestHeaderHandle, goalRequestHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(goalRequest, goalRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_goal_request)}() failed."); + } + } + } + + private static bool TakeCancelRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage cancelRequest) + { + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_cancel_request(actionServer.Handle, requestHeaderHandle, cancelRequestHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(cancelRequest, cancelRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_cancel_request)}() failed."); + } + } + } + + private static bool TakeResultRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage resultRequest) + { + using (var resultRequestHandle = actionServer.CreateGetResultRequestHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_result_request(actionServer.Handle, requestHeaderHandle, resultRequestHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(resultRequest, resultRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_result_request)}() failed."); + } + } + } + public static void SpinOnce(Node node, long timeout) { int numberOfSubscriptions = node.Subscriptions.Count; @@ -774,6 +1055,25 @@ public static void SpinOnce(Node node, long timeout) numberOfServices += acNumberOfServices; } + foreach (var actionServer in node.ActionServers) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries( + actionServer.Handle, + out int asNumberOfSubscriptions, + out int asNumberOfGuardConditions, + out int asNumberOfTimers, + out int asNumberOfClients, + out int asNumberOfServices); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries)}() failed."); + + numberOfSubscriptions += asNumberOfSubscriptions; + numberOfGuardConditions += asNumberOfGuardConditions; + numberOfTimers += asNumberOfTimers; + numberOfClients += asNumberOfClients; + numberOfServices += asNumberOfServices; + } + bool waitSetEmpty = numberOfSubscriptions == 0 && numberOfGuardConditions == 0 && numberOfTimers == 0 @@ -824,6 +1124,11 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddActionClient(waitSetHandle, actionClient.Handle); } + foreach (var actionServer in node.ActionServers) + { + WaitSetAddActionServer(waitSetHandle, actionServer.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { @@ -958,6 +1263,61 @@ public static void SpinOnce(Node node, long timeout) } } } + + foreach (var actionServer in node.ActionServers) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready( + waitSetHandle, + actionServer.Handle, + out bool isGoalRequestReady, + out bool isCancelRequestReady, + out bool isResultRequestReady, + out bool isGoalExpired); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready)}() failed."); + + if (isGoalRequestReady) + { + var goalRequest = actionServer.CreateSendGoalRequest(); + + var result = TakeGoalRequest(actionServer, requestIdHandle, goalRequest); + if (result) + { + actionServer.HandleGoalRequest(requestIdHandle, goalRequest); + } + } + + if (isCancelRequestReady) + { + var cancelRequest = new CancelGoal_Request(); + + var result = TakeCancelRequest(actionServer, requestIdHandle, cancelRequest); + if (result) + { + actionServer.HandleCancelRequest(requestIdHandle, cancelRequest); + } + } + + if (isResultRequestReady) + { + var resultRequest = actionServer.CreateGetResultRequest(); + + // We can't reuse requestIdHandle here as it is needed later when the result is ready. + // Create a new one for each GetResultRequest. + SafeRequestIdHandle resultRequestIdHandle = CreateRequestId(); + + var result = TakeResultRequest(actionServer, resultRequestIdHandle, resultRequest); + if (result) + { + actionServer.HandleResultRequest(resultRequestIdHandle, resultRequest); + } + } + + if (isGoalExpired) + { + actionServer.HandleGoalExpired(); + } + } } int guardConditionIndex = 0; diff --git a/rcldotnet/SafeActionGoalHandle.cs b/rcldotnet/SafeActionGoalHandle.cs new file mode 100644 index 00000000..838083c5 --- /dev/null +++ b/rcldotnet/SafeActionGoalHandle.cs @@ -0,0 +1,45 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_action_goal_handle_t + /// This is only used to implement the action server. + /// It is not used for the action client. + /// + // Could be called SafeActionGoalHandleHandle as well: + // Safe${c_type}Handle where c_type = ActionGoalHandle ;) + internal sealed class SafeActionGoalHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeActionGoalHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_destroy_goal_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/SafeActionServerHandle.cs b/rcldotnet/SafeActionServerHandle.cs new file mode 100644 index 00000000..36831444 --- /dev/null +++ b/rcldotnet/SafeActionServerHandle.cs @@ -0,0 +1,90 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_action_server_t + /// + /// + /// Since we need to delete the action client handle before the node is released we need to actually hold a + /// pointer to a rcl_action_server_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the client handle and node handle. + /// + internal sealed class SafeActionServerHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_client_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeActionServerHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_action_destroy_server_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 0e172d3a..cb47b3d9 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -26,6 +26,7 @@ #include "rcldotnet.h" static rcl_context_t context; +static rcl_clock_t clock; int32_t native_rcl_init() { // TODO(esteve): parse args @@ -39,9 +40,18 @@ int32_t native_rcl_init() { } const char ** arg_values = NULL; ret = rcl_init(num_args, arg_values, &init_options, &context); + if (ret != RCL_RET_OK) { + return ret; + } + + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); return ret; } +rcl_clock_t *native_rcl_get_default_clock() { + return &clock; +} + const char *native_rcl_get_rmw_identifier() { return rmw_get_implementation_identifier(); } @@ -213,6 +223,47 @@ int32_t native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void return ret; } +int32_t native_rcl_action_server_wait_set_get_num_entries( + void *action_server_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services) +{ + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + + size_t num_subscriptions_as_size_t; + size_t num_guard_conditions_as_size_t; + size_t num_timers_as_size_t; + size_t num_clients_as_size_t; + size_t num_services_as_size_t; + + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + action_server, + &num_subscriptions_as_size_t, + &num_guard_conditions_as_size_t, + &num_timers_as_size_t, + &num_clients_as_size_t, + &num_services_as_size_t); + + *num_subscriptions = (int32_t)num_subscriptions_as_size_t; + *num_guard_conditions = (int32_t)num_guard_conditions_as_size_t; + *num_timers = (int32_t)num_timers_as_size_t; + *num_clients = (int32_t)num_clients_as_size_t; + *num_services = (int32_t)num_services_as_size_t; + + return ret; +} + +int32_t native_rcl_action_wait_set_add_action_server(void *wait_set_handle, void *action_server_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); + + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); @@ -288,6 +339,28 @@ int32_t native_rcl_action_client_wait_set_get_entities_ready( return ret; } +int32_t native_rcl_action_server_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_server_handle, + bool *is_goal_request_ready, + bool *is_cancel_request_ready, + bool *is_result_request_ready, + bool *is_goal_expired) +{ + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + action_server, + is_goal_request_ready, + is_cancel_request_ready, + is_result_request_ready, + is_goal_expired); + + return ret; +} + int32_t native_rcl_take(void *subscription_handle, void *message_handle) { rcl_subscription_t * subscription = (rcl_subscription_t *)subscription_handle; @@ -374,3 +447,199 @@ int32_t native_rcl_action_take_result_response(void *action_client_handle, void rcl_ret_t ret = rcl_action_take_result_response(action_client, request_header, result_response_handle); return ret; } + +int32_t native_rcl_action_take_goal_request(void *action_server_handle, void *request_header_handle, void *goal_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_goal_request(action_server, request_header, goal_request_handle); + return ret; +} + +int32_t native_rcl_action_send_goal_response(void *action_server_handle, void *request_header_handle, void *goal_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_goal_response(action_server, request_header, goal_response_handle); + return ret; +} + +int32_t native_rcl_action_accept_new_goal(void **action_goal_handle_handle, void *action_server_handle, void *goal_info_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rcl_action_goal_info_t * goal_info = (rcl_action_goal_info_t *)goal_info_handle; + + rcl_action_goal_handle_t *action_goal_handle = + (rcl_action_goal_handle_t *)malloc(sizeof(rcl_action_goal_handle_t)); + + *action_goal_handle = rcl_action_get_zero_initialized_goal_handle(); + + rcl_action_goal_handle_t *rcl_action_goal_handle = rcl_action_accept_new_goal(action_server, goal_info); + rcl_ret_t ret; + if (rcl_action_goal_handle == NULL) + { + ret = RCL_RET_ERROR; + } + else + { + // Copy out goal handle since action server storage disappears when it is fini'd. + *action_goal_handle = *rcl_action_goal_handle; + + // Get the goal_info from the goal_handle to return the stamp back to native code. + ret = rcl_action_goal_handle_get_info(action_goal_handle, goal_info); + } + + *action_goal_handle_handle = (void *)action_goal_handle; + + return ret; +} + +int32_t native_rcl_action_destroy_goal_handle(void *action_goal_handle) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle; + + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + free(goal_handle); + + return ret; +} + +int32_t native_rcl_action_update_goal_state(void *action_goal_handle_handle, int32_t goal_event) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, (rcl_action_goal_event_t)goal_event); + return ret; +} + +int32_t native_rcl_action_publish_status(void *action_server_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + + rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = rcl_action_publish_status(action_server, &status_message); + + rcl_ret_t cleanup_ret; + cleanup_ret = rcl_action_goal_status_array_fini(&status_message); + if (cleanup_ret != RCL_RET_OK) + { + // If we got two unexpected errors, return the earlier error. + if (ret == RCL_RET_OK) { + // Error message already set. + ret = cleanup_ret; + } + } + + return ret; +} + +int32_t native_rcl_action_publish_feedback(void *action_server_handle, void *feedback_message_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_publish_feedback(action_server, feedback_message_handle); + return ret; +} + +int32_t native_rcl_action_take_cancel_request(void *action_server_handle, void *request_header_handle, void *cancel_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_cancel_request(action_server, request_header, cancel_request_handle); + return ret; +} + +int32_t native_rcl_action_process_cancel_request(void *action_server_handle, void *cancel_request_handle, void *cancel_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + // the rcl_action_cancel_request_t is a direct typedef to + // action_msgs__srv__CancelGoal_Request + + // rcl_action_cancel_response_t is a wrapper struct around + // action_msgs__srv__CancelGoal_Response with an additional allocator field. + // -> Don't cast in this case! + + rcl_action_cancel_response_t tmp_cancel_response = rcl_action_get_zero_initialized_cancel_response(); + + rcl_ret_t ret = rcl_action_process_cancel_request(action_server, cancel_request_handle, &tmp_cancel_response); + + // TODO: (sh) would be better to copy the list over element by element? + + // HACK: Don't deallocate but instead move reference to data into incoming cancel_response_handle. + // rcl_action_cancel_response_fini(&cancel_response); + action_msgs__srv__CancelGoal_Response * cancel_response = (action_msgs__srv__CancelGoal_Response *)cancel_response_handle; + *cancel_response = tmp_cancel_response.msg; + + // HACK: as rcl_action_cancel_response_init doesn't fill in capacity... + cancel_response->goals_canceling.capacity = cancel_response->goals_canceling.size; + + return ret; +} + +int32_t native_rcl_action_send_cancel_response(void *action_server_handle, void *request_header_handle, void *cancel_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_cancel_response(action_server, request_header, cancel_response_handle); + return ret; +} + +int32_t native_rcl_action_take_result_request(void *action_server_handle, void *request_header_handle, void *result_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_result_request(action_server, request_header, result_request_handle); + return ret; +} + +int32_t native_rcl_action_send_result_response(void *action_server_handle, void *request_header_handle, void *result_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_result_response(action_server, request_header, result_response_handle); + return ret; +} + +int32_t native_rcl_action_notify_goal_done(void *action_server_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_notify_goal_done(action_server); + return ret; +} + +int32_t native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired) +{ + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + size_t num_expired_as_size_t; + + // Only provide one goal_info to the underlying function. + // rclcpp does the same as only one goal is expected to expire at the same time. + // The out parameter num_expired can be used with a loop to expire all goals. + // This does also help to avoid implementing a new SafeHandle and accessor methods for a list of `GoalHandle`s. + rcl_ret_t ret = rcl_action_expire_goals(action_server, goal_info_handle, 1, &num_expired_as_size_t); + + *num_expired = (int32_t)num_expired_as_size_t; + + return ret; +} + +bool native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + return rcl_action_goal_handle_is_active(goal_handle); +} + +int32_t native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + rcl_action_goal_state_t status_as_rcl_type; + + rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status_as_rcl_type); + + *status = (int8_t)status_as_rcl_type; + + return ret; +} diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index ff06497c..c4504440 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -20,6 +20,8 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_init(); +rcl_clock_t *native_rcl_get_default_clock(); + RCLDOTNET_EXPORT const char * RCLDOTNET_CDECL native_rcl_get_rmw_identifier(); @@ -84,6 +86,18 @@ int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_num_entries( RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_num_entries( + void *action_server_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_server(void *wait_set_handle, void *action_server_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); @@ -109,6 +123,15 @@ int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( bool *is_cancel_response_ready, bool *is_result_response_ready); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_server_handle, + bool *is_goal_request_ready, + bool *is_cancel_request_ready, + bool *is_result_request_ready, + bool *is_goal_expired); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -145,4 +168,52 @@ int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_response(void *action_clie RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_goal_request(void *action_server_handle, void *request_header_handle, void *goal_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_goal_response(void *action_server_handle, void *request_header_handle, void *goal_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_accept_new_goal(void **action_goal_handle_handle, void *action_server_handle, void *goal_info_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_goal_handle(void *action_goal_handle_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_update_goal_state(void *action_goal_handle_handle, int32_t goal_event); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_publish_status(void *action_server_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_publish_feedback(void *action_server_handle, void *feedback_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_request(void *action_server_handle, void *request_header_handle, void *cancel_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_process_cancel_request(void *action_server_handle, void *cancel_request_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_response(void *action_server_handle, void *request_header_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_result_request(void *action_server_handle, void *request_header_handle, void *result_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_result_response(void *action_server_handle, void *request_header_handle, void *result_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_notify_goal_done(void *action_server_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired); + +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status); + #endif // RCLDOTNET_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index 1844c033..aa7ca1a2 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -24,6 +24,7 @@ #include "rosidl_runtime_c/message_type_support_struct.h" +#include "rcldotnet.h" #include "rcldotnet_node.h" int32_t native_rcl_create_publisher_handle(void **publisher_handle, @@ -188,3 +189,38 @@ int32_t native_rcl_action_destroy_client_handle(void *action_client_handle, void return ret; } + +int32_t native_rcl_action_create_server_handle(void **action_server_handle, + void *node_handle, + const char *action_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_action_type_support_t *ts = + (rosidl_action_type_support_t *)typesupport; + + rcl_action_server_t *action_server = + (rcl_action_server_t *)malloc(sizeof(rcl_action_server_t)); + *action_server = rcl_action_get_zero_initialized_server(); + rcl_action_server_options_t action_server_ops = + rcl_action_server_get_default_options(); + + rcl_clock_t *clock = native_rcl_get_default_clock(); + + rcl_ret_t ret = + rcl_action_server_init(action_server, node, clock, ts, action_name, &action_server_ops); + + *action_server_handle = (void *)action_server; + + return ret; +} + +int32_t native_rcl_action_destroy_server_handle(void *action_server_handle, void *node_handle) { + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_action_server_fini(action_server, node); + free(action_server); + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index 707cd149..99c20cde 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -60,4 +60,13 @@ int32_t RCLDOTNET_CDECL native_rcl_action_create_client_handle(void **action_cli RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_create_server_handle(void **action_server_handle, + void *node_handle, + const char *action_name, + void *typesupport); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_server_handle(void *action_server_handle, void *node_handle); + #endif // RCLDOTNET_NODE_H From 3ffbd2daa2408cf9e4fe739bb80c268a01ce475e Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 15:58:29 +0200 Subject: [PATCH 55/73] actions: add example action client and server --- rcldotnet_examples/CMakeLists.txt | 18 +++++ rcldotnet_examples/RCLDotnetActionClient.cs | 87 +++++++++++++++++++++ rcldotnet_examples/RCLDotnetActionServer.cs | 70 +++++++++++++++++ rcldotnet_examples/package.xml | 4 + 4 files changed, 179 insertions(+) create mode 100644 rcldotnet_examples/RCLDotnetActionClient.cs create mode 100644 rcldotnet_examples/RCLDotnetActionServer.cs diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index 64d64f89..a4fb217e 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -7,8 +7,10 @@ find_package(rcldotnet REQUIRED) find_package(rcldotnet_common REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(test_msgs REQUIRED) find_package(rcldotnet REQUIRED) find_package(dotnet_cmake_module REQUIRED) @@ -19,8 +21,10 @@ find_package(DotNETExtra REQUIRED) set(_assemblies_dep_dlls ${rcldotnet_common_ASSEMBLIES_DLL} ${rcldotnet_ASSEMBLIES_DLL} + ${builtin_interfaces_ASSEMBLIES_DLL} ${std_msgs_ASSEMBLIES_DLL} ${std_srvs_ASSEMBLIES_DLL} + ${test_msgs_ASSEMBLIES_DLL} ) add_dotnet_executable(rcldotnet_talker @@ -45,9 +49,23 @@ add_dotnet_executable(rcldotnet_example_client ${_assemblies_dep_dlls} ) +add_dotnet_executable(rcldotnet_example_action_server + RCLDotnetActionServer.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + +add_dotnet_executable(rcldotnet_example_action_client + RCLDotnetActionClient.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + install_dotnet(rcldotnet_talker DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_listener DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_example_service DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_example_client DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_action_server DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_action_client DESTINATION lib/${PROJECT_NAME}/dotnet) ament_package() diff --git a/rcldotnet_examples/RCLDotnetActionClient.cs b/rcldotnet_examples/RCLDotnetActionClient.cs new file mode 100644 index 00000000..e90475f3 --- /dev/null +++ b/rcldotnet_examples/RCLDotnetActionClient.cs @@ -0,0 +1,87 @@ +using System; +using System.Threading; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; + +namespace ConsoleApplication +{ + public static class RCLDotnetActionClient + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("action_client"); + + var actionClient = node.CreateActionClient("fibonacci"); + + var cts = new CancellationTokenSource(); + var task = DoWorkAsync(actionClient, cts.Token); + + while (RCLdotnet.Ok()) + { + RCLdotnet.SpinOnce(node, 500); + + if (task.IsCompletedSuccessfully) + { + break; + } + else if (task.IsFaulted) + { + Console.WriteLine($"Task faulted. Exception {task.Exception}"); + break; + } + else if (task.IsCanceled) + { + Console.WriteLine("Task canceled."); + break; + } + } + + cts.Cancel(); + task.GetAwaiter().GetResult(); + } + + private static async Task DoWorkAsync( + ActionClient actionClient, + CancellationToken cancellationToken) + { + while (!actionClient.ServerIsReady()) + { + cancellationToken.ThrowIfCancellationRequested(); + + // NOTE: This causes the code to resume in an background worker Thread. + // Consider this when copying code from the example if additional synchronization is needed. + await Task.Delay(1000, cancellationToken); + } + + var goal = new Fibonacci_Goal(); + goal.Order = 10; + + Console.WriteLine("SendGoal"); + + ActionClientGoalHandle goalHandleForCallback = null; + + var goalHandle = await actionClient.SendGoalAsync(goal, (Fibonacci_Feedback feedback) => + { + Console.WriteLine($"Feedback: {string.Join(", ", feedback.Sequence)}"); + Console.WriteLine($"Status after Feedback: {goalHandleForCallback.Status}"); + }); + + goalHandleForCallback = goalHandle; + + if (goalHandle.Accepted) + { + Console.WriteLine("Goal accepted."); + + var result = await goalHandle.GetResultAsync(); + Console.WriteLine($"Result: {string.Join(", ", result.Sequence)}"); + Console.WriteLine($"Status: {goalHandle.Status}"); + } + else + { + Console.WriteLine("Goal not accepted."); + } + } + } +} diff --git a/rcldotnet_examples/RCLDotnetActionServer.cs b/rcldotnet_examples/RCLDotnetActionServer.cs new file mode 100644 index 00000000..5cb9f576 --- /dev/null +++ b/rcldotnet_examples/RCLDotnetActionServer.cs @@ -0,0 +1,70 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; + +namespace ConsoleApplication +{ + public static class RCLDotnetActionServer + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("action_server"); + + var actionServer = node.CreateActionServer("fibonacci", HandleAccepted, cancelCallback: HandleCancel); + + RCLdotnet.Spin(node); + } + + private static void HandleAccepted(ActionServerGoalHandle goalHandle) + { + // Don't block in the callback. + // -> Don't wait for the returned Task. + _ = DoWorkWithGoal(goalHandle); + } + + private static CancelResponse HandleCancel(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Accept; + } + + private static async Task DoWorkWithGoal(ActionServerGoalHandle goalHandle) + { + Console.WriteLine("Executing goal..."); + var feedback = new Fibonacci_Feedback(); + + feedback.Sequence = new List { 0, 1 }; + + for (int i = 1; i < goalHandle.Goal.Order; i++) + { + if (goalHandle.IsCanceling) + { + var cancelResult = new Fibonacci_Result(); + cancelResult.Sequence = feedback.Sequence; + + Console.WriteLine($"Canceled Result: {string.Join(", ", cancelResult.Sequence)}"); + goalHandle.Canceled(cancelResult); + return; + } + + feedback.Sequence.Add(feedback.Sequence[i] + feedback.Sequence[i - 1]); + + Console.WriteLine($"Feedback: {string.Join(", ", feedback.Sequence)}"); + goalHandle.PublishFeedback(feedback); + + // NOTE: This causes the code to resume in an background worker Thread. + // Consider this when copying code from the example if additional synchronization is needed. + await Task.Delay(1000); + } + + var result = new Fibonacci_Result(); + result.Sequence = feedback.Sequence; + + Console.WriteLine($"Result: {string.Join(", ", result.Sequence)}"); + goalHandle.Succeed(result); + } + } +} diff --git a/rcldotnet_examples/package.xml b/rcldotnet_examples/package.xml index 1a4ef781..602e7a01 100644 --- a/rcldotnet_examples/package.xml +++ b/rcldotnet_examples/package.xml @@ -13,18 +13,22 @@ rcldotnet_common rosidl_cmake + builtin_interfaces example_interfaces rcldotnet std_msgs std_srvs sensor_msgs + test_msgs rcldotnet_common + builtin_interfaces example_interfaces rcldotnet std_msgs std_srvs sensor_msgs + test_msgs ament_cmake From 04b2019f83b73ac5b928dfa9d1e92669785071cd Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 22 Jul 2022 09:36:11 +0200 Subject: [PATCH 56/73] actions: make local variable name more obvious --- rcldotnet/ActionClient.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index a8f52c08..f7982795 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -313,12 +313,12 @@ internal override void HandleFeedbackMessage(IRosMessage feedbackMessage) internal override void HandleStatusMessage(GoalStatusArray statusMessage) { - foreach (var statusMessage1 in statusMessage.StatusList) + foreach (var statusMessageEntry in statusMessage.StatusList) { - var goalId = statusMessage1.GoalInfo.GoalId.ToGuid(); + var goalId = statusMessageEntry.GoalInfo.GoalId.ToGuid(); if (_goalHandles.TryGetValue(goalId, out var goalHandle)) { - var status = statusMessage1.Status; + var status = statusMessageEntry.Status; goalHandle.Status = ConvertGoalStatus(status); // rclpy does this, rclcpp does not. From 02cf5a5d08a23d11b49b8be568923faac5d0f9e6 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 22 Jul 2022 09:43:06 +0200 Subject: [PATCH 57/73] actions: use return value from Interlocked.CompareExchange The field access after Interlocked.CompareExchange might not reflect the updated value. Or does it? Anyways this makes it more clear. --- rcldotnet/ActionClientGoalHandle.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs index bdae9f8b..34ca9baa 100644 --- a/rcldotnet/ActionClientGoalHandle.cs +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -87,10 +87,11 @@ public Task GetResultAsync() var resultTaskCompletionSource = new TaskCompletionSource(); - if (Interlocked.CompareExchange(ref _resultTaskCompletionSource, resultTaskCompletionSource, null) != null) + var oldResultTcs = Interlocked.CompareExchange(ref _resultTaskCompletionSource, resultTaskCompletionSource, null); + if (oldResultTcs != null) { // Some other thread was first. - return _resultTaskCompletionSource.Task; + return oldResultTcs.Task; } return _actionClient.GetResultAsync(this, resultTaskCompletionSource); From 459c229d788feecc3cf6dff8c01b97f29a5c7132 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 28 Jul 2022 09:00:06 +0200 Subject: [PATCH 58/73] actions: reorder HandleGoalResponse, HandleStatusMessage and HandleFeedbackMessage --- rcldotnet/RCLdotnet.cs | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 694c9846..5f8731be 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -1205,17 +1205,24 @@ public static void SpinOnce(Node node, long timeout) RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready)}() failed."); - if (isFeedbackReady) + // Check isGoalResponseReady so that a new goalHandle is + // already created if the status or feedback for the new + // goal is received. + if (isGoalResponseReady) { - var feedbackMessage = actionClient.CreateFeedbackMessage(); + var goalResponse = actionClient.CreateSendGoalResponse(); - var result = TakeFeedbackMessage(actionClient, feedbackMessage); + var result = TakeGoalResponse(actionClient, requestIdHandle, goalResponse); if (result) { - actionClient.HandleFeedbackMessage(feedbackMessage); + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleGoalResponse(sequenceNumber, goalResponse); } } + // Check isStatusReady before isFeedbackReady so that + // the feedback callback already has the newest status + // information if updates are received at the same time. if (isStatusReady) { var statusMessage = new GoalStatusArray(); @@ -1227,15 +1234,14 @@ public static void SpinOnce(Node node, long timeout) } } - if (isGoalResponseReady) + if (isFeedbackReady) { - var goalResponse = actionClient.CreateSendGoalResponse(); + var feedbackMessage = actionClient.CreateFeedbackMessage(); - var result = TakeGoalResponse(actionClient, requestIdHandle, goalResponse); + var result = TakeFeedbackMessage(actionClient, feedbackMessage); if (result) { - var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); - actionClient.HandleGoalResponse(sequenceNumber, goalResponse); + actionClient.HandleFeedbackMessage(feedbackMessage); } } From 15156b0710998ce73d6b0a7e7ab1ae3413a41e69 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 28 Jul 2022 09:28:40 +0200 Subject: [PATCH 59/73] actions: handle action specific take failed error codes --- rcldotnet/RCLRet.cs | 4 +++- rcldotnet/RCLdotnet.cs | 8 ++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/rcldotnet/RCLRet.cs b/rcldotnet/RCLRet.cs index 95c2f6de..2f529d2a 100644 --- a/rcldotnet/RCLRet.cs +++ b/rcldotnet/RCLRet.cs @@ -50,6 +50,8 @@ internal enum RCLRet InvalidParamRule = 1010, InvalidLogLevelRule = 1020, EventInvalid = 2000, - EventTakeFailed = 2001 + EventTakeFailed = 2001, + ActionClientTakeFailed = 2103, + ActionServerTakeFailed = 2201, } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 5f8731be..6f1d1eba 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -868,6 +868,7 @@ private static bool TakeFeedbackMessage(ActionClient actionClient, IRosMessage f return true; case RCLRet.SubscriptionTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -888,6 +889,7 @@ private static bool TakeStatusMessage(ActionClient actionClient, IRosMessage sta return true; case RCLRet.SubscriptionTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -908,6 +910,7 @@ private static bool TakeGoalResponse(ActionClient actionClient, SafeRequestIdHan return true; case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -928,6 +931,7 @@ private static bool TakeCancelResponse(ActionClient actionClient, SafeRequestIdH return true; case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -948,6 +952,7 @@ private static bool TakeResultResponse(ActionClient actionClient, SafeRequestIdH return true; case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -979,6 +984,7 @@ private static bool TakeGoalRequest(ActionServer actionServer, SafeRequestIdHand return true; case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: return false; default: @@ -999,6 +1005,7 @@ private static bool TakeCancelRequest(ActionServer actionServer, SafeRequestIdHa return true; case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: return false; default: @@ -1019,6 +1026,7 @@ private static bool TakeResultRequest(ActionServer actionServer, SafeRequestIdHa return true; case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: return false; default: From 866834a3521c44f4b8042391ab93cfab8b946fa5 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 28 Jul 2022 09:33:42 +0200 Subject: [PATCH 60/73] actions: publish status before sending the result response --- rcldotnet/ActionServer.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs index 6db438b5..18a1caca 100644 --- a/rcldotnet/ActionServer.cs +++ b/rcldotnet/ActionServer.cs @@ -451,8 +451,8 @@ internal void HandleTerminalState( ActionServerGoalHandle actionServerGoalHandle, IRosActionGetResultResponse response) { - actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); PublishStatus(); + actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); RCLRet ret = RCLdotnetDelegates.native_rcl_action_notify_goal_done(Handle); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_notify_goal_done)}() failed."); From 5fda852935a1513ad27b77ebe15e84c198304420 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 17 Aug 2022 14:25:28 +0200 Subject: [PATCH 61/73] actions: fix typo in dependency --- rcldotnet/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index db01ebe6..88be5f0f 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -19,14 +19,14 @@ rmw_implementation_cmake rcl - rcl_actions + rcl_action rmw action_msgs builtin_interfaces unique_identifier_msgs rcl - rcl_actions + rcl_action action_msgs builtin_interfaces unique_identifier_msgs From 0019358f9b7a094300b8185645aab5db3a35cdcf Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 13 Sep 2022 15:38:14 +0200 Subject: [PATCH 62/73] bool pInvoke: Fix bool pInvokes in rcldotnet --- rcldotnet/ActionClient.cs | 6 +- rcldotnet/ActionServerGoalHandle.cs | 4 +- rcldotnet/Client.cs | 6 +- rcldotnet/RCLdotnet.cs | 62 ++++++++++---------- rcldotnet/rcldotnet.c | 88 +++++++++++++++++++---------- rcldotnet/rcldotnet.h | 30 +++++----- rcldotnet/rcldotnet_action_client.c | 7 ++- rcldotnet/rcldotnet_action_client.h | 2 +- rcldotnet/rcldotnet_client.c | 7 ++- rcldotnet/rcldotnet_client.h | 2 +- 10 files changed, 125 insertions(+), 89 deletions(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index f7982795..9f9e362e 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -49,7 +49,7 @@ internal delegate RCLRet NativeRCLActionSendCancelRequestType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionServerIsAvailableType( - SafeNodeHandle nodeHandle, SafeActionClientHandle clientHandle, out bool isAvailable); + SafeNodeHandle nodeHandle, SafeActionClientHandle clientHandle, out int isAvailable); internal static NativeRCLActionServerIsAvailableType native_rcl_action_server_is_available = null; @@ -156,10 +156,10 @@ internal ActionClient(SafeActionClientHandle handle, Node node) public override bool ServerIsReady() { - RCLRet ret = ActionClientDelegates.native_rcl_action_server_is_available(_node.Handle, Handle, out var serverIsReady); + RCLRet ret = ActionClientDelegates.native_rcl_action_server_is_available(_node.Handle, Handle, out int serverIsReady); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_server_is_available)}() failed."); - return serverIsReady; + return serverIsReady != 0; } public Task> SendGoalAsync(TGoal goal) diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs index f5d3e626..cd87d68c 100644 --- a/rcldotnet/ActionServerGoalHandle.cs +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -87,8 +87,8 @@ public override bool IsActive { get { - bool isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); - return isActive; + int isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); + return isActive != 0; } } diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 9e0b1e66..f0ff7ad0 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -33,7 +33,7 @@ internal delegate RCLRet NativeRCLSendRequestType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLServiceServerIsAvailableType( - SafeNodeHandle nodeHandle, SafeClientHandle clientHandle, out bool isAvailable); + SafeNodeHandle nodeHandle, SafeClientHandle clientHandle, out int isAvailable); internal static NativeRCLServiceServerIsAvailableType native_rcl_service_server_is_available = null; @@ -95,10 +95,10 @@ internal Client(SafeClientHandle handle, Node node) public bool ServiceIsReady() { - RCLRet ret = ClientDelegates.native_rcl_service_server_is_available(_node.Handle, Handle, out var serviceIsReady); + RCLRet ret = ClientDelegates.native_rcl_service_server_is_available(_node.Handle, Handle, out int serviceIsReady); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_service_server_is_available)}() failed."); - return serviceIsReady; + return serviceIsReady != 0; } public Task SendRequestAsync(TRequest request) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 6f1d1eba..3407a4fd 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -42,7 +42,7 @@ internal delegate void NativeRCLGetErrorStringType( internal static NativeRCLResetErrorType native_rcl_reset_error = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NativeRCLOkType(); + internal delegate int NativeRCLOkType(); internal static NativeRCLOkType native_rcl_ok = null; @@ -119,22 +119,22 @@ internal delegate RCLRet NativeRCLCreateWaitSetHandleType( internal static NativeRCLWaitType native_rcl_wait = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NativeRCLWaitSetSubscriptionReady(SafeWaitSetHandle waitSetHandle, int index); + internal delegate int NativeRCLWaitSetSubscriptionReady(SafeWaitSetHandle waitSetHandle, int index); internal static NativeRCLWaitSetSubscriptionReady native_rcl_wait_set_subscription_ready = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NativeRCLWaitSetClientReady(SafeWaitSetHandle waitSetHandle, int index); + internal delegate int NativeRCLWaitSetClientReady(SafeWaitSetHandle waitSetHandle, int index); internal static NativeRCLWaitSetClientReady native_rcl_wait_set_client_ready = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NativeRCLWaitSetServiceReady(SafeWaitSetHandle waitSetHandle, int index); + internal delegate int NativeRCLWaitSetServiceReady(SafeWaitSetHandle waitSetHandle, int index); internal static NativeRCLWaitSetServiceReady native_rcl_wait_set_service_ready = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NativeRCLWaitSetGuardConditionReady(SafeWaitSetHandle waitSetHandle, int index); + internal delegate int NativeRCLWaitSetGuardConditionReady(SafeWaitSetHandle waitSetHandle, int index); internal static NativeRCLWaitSetGuardConditionReady native_rcl_wait_set_guard_condition_ready = null; @@ -188,7 +188,7 @@ internal delegate RCLRet NativeRCLActionWaitSetAddActionClientType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( - SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle, out bool isFeedbackReady, out bool isStatusReady, out bool isGoalResponseReady, out bool isCancelResponseReady, out bool isResultResponseReady); + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle, out int isFeedbackReady, out int isStatusReady, out int isGoalResponseReady, out int isCancelResponseReady, out int isResultResponseReady); internal static NativeRCLActionClientWaitSetGetEntitiesReadyType native_rcl_action_client_wait_set_get_entities_ready = null; @@ -206,7 +206,7 @@ internal delegate RCLRet NativeRCLActionWaitSetAddActionServerType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionServerWaitSetGetEntitiesReadyType( - SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle, out bool isGoalRequestReady, out bool isCancelRequestReady, out bool isResultRequestReady, out bool isGoalExpired); + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle, out int isGoalRequestReady, out int isCancelRequestReady, out int isResultRequestReady, out int isGoalExpired); internal static NativeRCLActionServerWaitSetGetEntitiesReadyType native_rcl_action_server_wait_set_get_entities_ready = null; @@ -319,7 +319,7 @@ internal delegate RCLRet NativeRCLActionExpireGoalsType( internal static NativeRCLActionExpireGoalsType native_rcl_action_expire_goals = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NativeRCLActionGoalHandleIsActiveType(SafeActionGoalHandle actionGoalHandleHandle); + internal delegate int NativeRCLActionGoalHandleIsActiveType(SafeActionGoalHandle actionGoalHandleHandle); internal static NativeRCLActionGoalHandleIsActiveType native_rcl_action_goal_handle_is_active = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] @@ -670,7 +670,7 @@ public static class RCLdotnet public static bool Ok() { - return RCLdotnetDelegates.native_rcl_ok(); + return RCLdotnetDelegates.native_rcl_ok() != 0; } public static Node CreateNode(string nodeName) @@ -1146,7 +1146,7 @@ public static void SpinOnce(Node node, long timeout) int subscriptionIndex = 0; foreach (Subscription subscription in node.Subscriptions) { - if (RCLdotnetDelegates.native_rcl_wait_set_subscription_ready(waitSetHandle, subscriptionIndex)) + if (RCLdotnetDelegates.native_rcl_wait_set_subscription_ready(waitSetHandle, subscriptionIndex) != 0) { IRosMessage message = subscription.CreateMessage(); bool result = Take(subscription, message); @@ -1165,7 +1165,7 @@ public static void SpinOnce(Node node, long timeout) int serviceIndex = 0; foreach (var service in node.Services) { - if (RCLdotnetDelegates.native_rcl_wait_set_service_ready(waitSetHandle, serviceIndex)) + if (RCLdotnetDelegates.native_rcl_wait_set_service_ready(waitSetHandle, serviceIndex) != 0) { var request = service.CreateRequest(); var response = service.CreateResponse(); @@ -1185,7 +1185,7 @@ public static void SpinOnce(Node node, long timeout) int clientIndex = 0; foreach (var client in node.Clients) { - if (RCLdotnetDelegates.native_rcl_wait_set_client_ready(waitSetHandle, clientIndex)) + if (RCLdotnetDelegates.native_rcl_wait_set_client_ready(waitSetHandle, clientIndex) != 0) { var response = client.CreateResponse(); @@ -1205,18 +1205,18 @@ public static void SpinOnce(Node node, long timeout) RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready( waitSetHandle, actionClient.Handle, - out bool isFeedbackReady, - out bool isStatusReady, - out bool isGoalResponseReady, - out bool isCancelResponseReady, - out bool isResultResponseReady); + out int isFeedbackReady, + out int isStatusReady, + out int isGoalResponseReady, + out int isCancelResponseReady, + out int isResultResponseReady); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready)}() failed."); // Check isGoalResponseReady so that a new goalHandle is // already created if the status or feedback for the new // goal is received. - if (isGoalResponseReady) + if (isGoalResponseReady != 0) { var goalResponse = actionClient.CreateSendGoalResponse(); @@ -1231,7 +1231,7 @@ public static void SpinOnce(Node node, long timeout) // Check isStatusReady before isFeedbackReady so that // the feedback callback already has the newest status // information if updates are received at the same time. - if (isStatusReady) + if (isStatusReady != 0) { var statusMessage = new GoalStatusArray(); @@ -1242,7 +1242,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isFeedbackReady) + if (isFeedbackReady != 0) { var feedbackMessage = actionClient.CreateFeedbackMessage(); @@ -1253,7 +1253,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isCancelResponseReady) + if (isCancelResponseReady != 0) { var cancelResponse = new CancelGoal_Response(); @@ -1265,7 +1265,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isResultResponseReady) + if (isResultResponseReady != 0) { var resultResponse = actionClient.CreateGetResultResponse(); @@ -1283,14 +1283,14 @@ public static void SpinOnce(Node node, long timeout) RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready( waitSetHandle, actionServer.Handle, - out bool isGoalRequestReady, - out bool isCancelRequestReady, - out bool isResultRequestReady, - out bool isGoalExpired); + out int isGoalRequestReady, + out int isCancelRequestReady, + out int isResultRequestReady, + out int isGoalExpired); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready)}() failed."); - if (isGoalRequestReady) + if (isGoalRequestReady != 0) { var goalRequest = actionServer.CreateSendGoalRequest(); @@ -1301,7 +1301,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isCancelRequestReady) + if (isCancelRequestReady != 0) { var cancelRequest = new CancelGoal_Request(); @@ -1312,7 +1312,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isResultRequestReady) + if (isResultRequestReady != 0) { var resultRequest = actionServer.CreateGetResultRequest(); @@ -1327,7 +1327,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isGoalExpired) + if (isGoalExpired != 0) { actionServer.HandleGoalExpired(); } @@ -1337,7 +1337,7 @@ public static void SpinOnce(Node node, long timeout) int guardConditionIndex = 0; foreach (GuardCondition guardCondition in node.GuardConditions) { - if (RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready(waitSetHandle, guardConditionIndex)) + if (RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready(waitSetHandle, guardConditionIndex) != 0) { guardCondition.TriggerCallback(); } diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index cb47b3d9..00eefe64 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -68,7 +68,10 @@ void native_rcl_reset_error(void) { rcl_reset_error(); } -bool native_rcl_ok() { return rcl_context_is_valid(&context); } +int32_t native_rcl_ok() { + bool result = rcl_context_is_valid(&context); + return result ? 1 : 0; +} int32_t native_rcl_create_node_handle(void **node_handle, const char *name, const char *namespace) { rcl_node_t *node = (rcl_node_t *)malloc(sizeof(rcl_node_t)); @@ -271,7 +274,7 @@ int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { return ret; } -bool native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index) { +int32_t native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; if (index >= wait_set->size_of_subscriptions) @@ -279,10 +282,11 @@ bool native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index return false; } - return wait_set->subscriptions[index] != NULL; + bool result = wait_set->subscriptions[index] != NULL; + return result ? 1 : 0; } -bool native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index) { +int32_t native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; if (index >= wait_set->size_of_clients) @@ -290,10 +294,11 @@ bool native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index) { return false; } - return wait_set->clients[index] != NULL; + bool result = wait_set->clients[index] != NULL; + return result ? 1 : 0; } -bool native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index) { +int32_t native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; if (index >= wait_set->size_of_services) @@ -301,10 +306,11 @@ bool native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index) { return false; } - return wait_set->services[index] != NULL; + bool result = wait_set->services[index] != NULL; + return result ? 1 : 0; } -bool native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index) { +int32_t native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; if (index >= wait_set->size_of_guard_conditions) @@ -312,29 +318,42 @@ bool native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t in return false; } - return wait_set->guard_conditions[index] != NULL; + bool result = wait_set->guard_conditions[index] != NULL; + return result ? 1 : 0; } int32_t native_rcl_action_client_wait_set_get_entities_ready( void *wait_set_handle, void *action_client_handle, - bool *is_feedback_ready, - bool *is_status_ready, - bool *is_goal_response_ready, - bool *is_cancel_response_ready, - bool *is_result_response_ready) + int32_t *is_feedback_ready, + int32_t *is_status_ready, + int32_t *is_goal_response_ready, + int32_t *is_cancel_response_ready, + int32_t *is_result_response_ready) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + bool is_feedback_ready_as_bool; + bool is_status_ready_as_bool; + bool is_goal_response_ready_as_bool; + bool is_cancel_response_ready_as_bool; + bool is_result_response_ready_as_bool; + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( wait_set, action_client, - is_feedback_ready, - is_status_ready, - is_goal_response_ready, - is_cancel_response_ready, - is_result_response_ready); + &is_feedback_ready_as_bool, + &is_status_ready_as_bool, + &is_goal_response_ready_as_bool, + &is_cancel_response_ready_as_bool, + &is_result_response_ready_as_bool); + + *is_feedback_ready = is_feedback_ready_as_bool ? 1 : 0; + *is_status_ready = is_status_ready_as_bool ? 1 : 0; + *is_goal_response_ready = is_goal_response_ready_as_bool ? 1 : 0; + *is_cancel_response_ready = is_cancel_response_ready_as_bool ? 1 : 0; + *is_result_response_ready = is_result_response_ready_as_bool ? 1 : 0; return ret; } @@ -342,21 +361,31 @@ int32_t native_rcl_action_client_wait_set_get_entities_ready( int32_t native_rcl_action_server_wait_set_get_entities_ready( void *wait_set_handle, void *action_server_handle, - bool *is_goal_request_ready, - bool *is_cancel_request_ready, - bool *is_result_request_ready, - bool *is_goal_expired) + int32_t *is_goal_request_ready, + int32_t *is_cancel_request_ready, + int32_t *is_result_request_ready, + int32_t *is_goal_expired) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + bool is_goal_request_ready_as_bool; + bool is_cancel_request_ready_as_bool; + bool is_result_request_ready_as_bool; + bool is_goal_expired_as_bool; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( wait_set, action_server, - is_goal_request_ready, - is_cancel_request_ready, - is_result_request_ready, - is_goal_expired); + &is_goal_request_ready_as_bool, + &is_cancel_request_ready_as_bool, + &is_result_request_ready_as_bool, + &is_goal_expired_as_bool); + + *is_goal_request_ready = is_goal_request_ready_as_bool ? 1 : 0; + *is_cancel_request_ready = is_cancel_request_ready_as_bool ? 1 : 0; + *is_result_request_ready = is_result_request_ready_as_bool ? 1 : 0; + *is_goal_expired = is_goal_expired_as_bool ? 1 : 0; return ret; } @@ -626,10 +655,11 @@ int32_t native_rcl_action_expire_goals(void *action_server_handle, void *goal_in return ret; } -bool native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle) { +int32_t native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle) { rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; - return rcl_action_goal_handle_is_active(goal_handle); + bool result = rcl_action_goal_handle_is_active(goal_handle); + return result ? 1 : 0; } int32_t native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status) { diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index c4504440..e0280cea 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -32,7 +32,7 @@ RCLDOTNET_EXPORT void RCLDOTNET_CDECL native_rcl_reset_error(void); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_ok(); +int32_t RCLDOTNET_CDECL native_rcl_ok(); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_node_handle(void **, const char *, const char *); @@ -102,35 +102,35 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index); +int32_t RCLDOTNET_CDECL native_rcl_wait_set_subscription_ready(void *wait_set_handle, int32_t index); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index); +int32_t RCLDOTNET_CDECL native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index); +int32_t RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index); +int32_t RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( void *wait_set_handle, void *action_client_handle, - bool *is_feedback_ready, - bool *is_status_ready, - bool *is_goal_response_ready, - bool *is_cancel_response_ready, - bool *is_result_response_ready); + int32_t *is_feedback_ready, + int32_t *is_status_ready, + int32_t *is_goal_response_ready, + int32_t *is_cancel_response_ready, + int32_t *is_result_response_ready); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_entities_ready( void *wait_set_handle, void *action_server_handle, - bool *is_goal_request_ready, - bool *is_cancel_request_ready, - bool *is_result_request_ready, - bool *is_goal_expired); + int32_t *is_goal_request_ready, + int32_t *is_cancel_request_ready, + int32_t *is_result_request_ready, + int32_t *is_goal_expired); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -211,7 +211,7 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle); +int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status); diff --git a/rcldotnet/rcldotnet_action_client.c b/rcldotnet/rcldotnet_action_client.c index a03e6491..25d4ea52 100644 --- a/rcldotnet/rcldotnet_action_client.c +++ b/rcldotnet/rcldotnet_action_client.c @@ -49,10 +49,13 @@ int32_t native_rcl_action_send_cancel_request(void *action_client_handle, void * return ret; } -int32_t native_rcl_action_server_is_available(void *node_handle, void *client_handle, bool *is_available) { +int32_t native_rcl_action_server_is_available(void *node_handle, void *client_handle, int32_t *is_available) { rcl_node_t * node = (rcl_node_t *)node_handle; rcl_action_client_t * client = (rcl_action_client_t *)client_handle; - rcl_ret_t ret = rcl_action_server_is_available(node, client, is_available); + bool is_available_as_bool; + rcl_ret_t ret = rcl_action_server_is_available(node, client, &is_available_as_bool); + *is_available = is_available_as_bool ? 1 : 0; + return ret; } diff --git a/rcldotnet/rcldotnet_action_client.h b/rcldotnet/rcldotnet_action_client.h index 93d06d4c..4fbd873f 100644 --- a/rcldotnet/rcldotnet_action_client.h +++ b/rcldotnet/rcldotnet_action_client.h @@ -27,6 +27,6 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number); RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_action_server_is_available(void *node_handle, void *action_client_handle, bool *is_available); +int32_t RCLDOTNET_CDECL native_rcl_action_server_is_available(void *node_handle, void *action_client_handle, int32_t *is_available); #endif // RCLDOTNET_CLIENT_H diff --git a/rcldotnet/rcldotnet_client.c b/rcldotnet/rcldotnet_client.c index 5529689d..8b1637cd 100644 --- a/rcldotnet/rcldotnet_client.c +++ b/rcldotnet/rcldotnet_client.c @@ -34,10 +34,13 @@ int32_t native_rcl_send_request(void *client_handle, void *request_handle, int64 return ret; } -int32_t native_rcl_service_server_is_available(void *node_handle, void *client_handle, bool *is_available) { +int32_t native_rcl_service_server_is_available(void *node_handle, void *client_handle, int32_t *is_available) { rcl_node_t * node = (rcl_node_t *)node_handle; rcl_client_t * client = (rcl_client_t *)client_handle; - rcl_ret_t ret = rcl_service_server_is_available(node, client, is_available); + bool is_available_as_bool; + rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_available_as_bool); + *is_available = is_available_as_bool ? 1 : 0; + return ret; } diff --git a/rcldotnet/rcldotnet_client.h b/rcldotnet/rcldotnet_client.h index 8e29773a..9844e760 100644 --- a/rcldotnet/rcldotnet_client.h +++ b/rcldotnet/rcldotnet_client.h @@ -21,6 +21,6 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_send_request(void *client_handle, void *request_handle, int64_t *sequence_number); RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_service_server_is_available(void *node_handle, void *client_handle, bool *is_available); +int32_t RCLDOTNET_CDECL native_rcl_service_server_is_available(void *node_handle, void *client_handle, int32_t *is_available); #endif // RCLDOTNET_CLIENT_H From 275dc2bc7f990b5a3114926dca679b76161aaffe Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 11 Oct 2022 17:14:08 +0200 Subject: [PATCH 63/73] bool pInvoke: Fix bool pInvokes in generated messages This does not change the C# side of the generated messages to avoid even more complexity in the templates. As bools in C# pInvokes get marshalled as 32 bit integer by default there is no strictly need to change the code for bool fields. --- rosidl_generator_dotnet/resource/msg.c.em | 26 ++++++++++++++++++++++- rosidl_generator_dotnet/resource/msg.h.em | 16 +++++++++++++- 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/rosidl_generator_dotnet/resource/msg.c.em b/rosidl_generator_dotnet/resource/msg.c.em index aeb451f9..d836ffe6 100644 --- a/rosidl_generator_dotnet/resource/msg.c.em +++ b/rosidl_generator_dotnet/resource/msg.c.em @@ -11,6 +11,7 @@ from rosidl_parser.definition import AbstractSequence from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import NamespacedType +from rosidl_parser.definition import BOOLEAN_TYPE from rosidl_cmake import convert_camel_case_to_lower_case_underscore @@ -72,7 +73,20 @@ bool @(msg_typename)__init_sequence_field_@(member.name)_message(void *message_h } @[ end if]@ -@[ if isinstance(member.type.value_type, BasicType)]@ +@[ if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == BOOLEAN_TYPE]@ +@# Special handling for marshaling bool as int32_t +void @(msg_typename)__write_field_@(member.name)(void *message_handle, int32_t /* bool */ value) +{ + bool * ros_message = (bool *)message_handle; + *ros_message = value != 0; +} + +int32_t /* bool */ @(msg_typename)__read_field_@(member.name)(void *message_handle) +{ + bool * ros_message = (bool *)message_handle; + return (*ros_message) ? 1 : 0; +} +@[ elif isinstance(member.type.value_type, BasicType)]@ void @(msg_typename)__write_field_@(member.name)(void *message_handle, @(msg_type_to_c(member.type.value_type)) value) { @(msg_type_to_c(member.type.value_type)) * ros_message = (@(msg_type_to_c(member.type.value_type)) *)message_handle; @@ -100,6 +114,16 @@ void @(msg_typename)__write_field_@(member.name)(void *message_handle, @(msg_typ @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported +@[ elif isinstance(member.type, BasicType) and member.type.typename == BOOLEAN_TYPE]@ +@# Special handling for marshaling bool as int32_t +int32_t /* bool */ @(msg_typename)__read_field_@(member.name)(void * message_handle) { + @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; + return (ros_message->@(member.name)) ? 1 : 0; +} +void @(msg_typename)__write_field_@(member.name)(void * message_handle, int32_t /* bool */ value) { + @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; + ros_message->@(member.name) = value != 0; +} @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @(msg_type_to_c(member.type)) @(msg_typename)__read_field_@(member.name)(void * message_handle) { @(msg_typename) * ros_message = (@(msg_typename) *)message_handle; diff --git a/rosidl_generator_dotnet/resource/msg.h.em b/rosidl_generator_dotnet/resource/msg.h.em index a81516f3..4bb123b4 100644 --- a/rosidl_generator_dotnet/resource/msg.h.em +++ b/rosidl_generator_dotnet/resource/msg.h.em @@ -7,6 +7,7 @@ from rosidl_parser.definition import AbstractSequence from rosidl_parser.definition import Array from rosidl_parser.definition import BasicType from rosidl_parser.definition import NamespacedType +from rosidl_parser.definition import BOOLEAN_TYPE from rosidl_generator_dotnet import msg_type_to_c type_name = message.structure.namespaced_type.name @@ -62,7 +63,13 @@ int32_t @(msg_prefix)_CDECL @(msg_typename)__getsize_field_@(member.name)_messag bool @(msg_prefix)_CDECL @(msg_typename)__init_sequence_field_@(member.name)_message(void *, int32_t); @[ end if]@ -@[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ +@[ if isinstance(member.type.value_type, BasicType) and member.type.value_type.typename == BOOLEAN_TYPE]@ +@# Special handling for marshaling bool as int32_t +@(msg_prefix)_EXPORT +void @(msg_typename)__write_field_@(member.name)(void *, int32_t /* bool */); +@(msg_prefix)_EXPORT +int32_t /* bool */ @(msg_prefix)_CDECL @(msg_typename)__read_field_@(member.name)(void *); +@[ elif isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ @(msg_prefix)_EXPORT void @(msg_typename)__write_field_@(member.name)(void *, @(msg_type_to_c(member.type.value_type))); @(msg_prefix)_EXPORT @@ -71,6 +78,13 @@ void @(msg_typename)__write_field_@(member.name)(void *, @(msg_type_to_c(member. @[ elif isinstance(member.type, AbstractWString)]@ // TODO: Unicode types are not supported +@[ elif isinstance(member.type, BasicType) and member.type.typename == BOOLEAN_TYPE]@ +@# Special handling for marshaling bool as int32_t +@(msg_prefix)_EXPORT +int32_t /* bool */ @(msg_prefix)_CDECL @(msg_typename)__read_field_@(member.name)(void *); + +@(msg_prefix)_EXPORT +void @(msg_typename)__write_field_@(member.name)(void *, int32_t /* bool */); @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @(msg_prefix)_EXPORT @(msg_type_to_c(member.type)) @(msg_prefix)_CDECL @(msg_typename)__read_field_@(member.name)(void *); From 2f857a347b094c281ff284e2c561dedafdadcd09 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 13 Oct 2022 14:40:46 +0200 Subject: [PATCH 64/73] 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 --- rcldotnet/CMakeLists.txt | 2 + rcldotnet/Node.cs | 44 ++- rcldotnet/QosProfile.cs | 566 ++++++++++++++++++++++++++++++ rcldotnet/RCLdotnet.cs | 44 +++ rcldotnet/SafeQosProfileHandle.cs | 43 +++ rcldotnet/rcldotnet.c | 50 +++ rcldotnet/rcldotnet.h | 22 ++ rcldotnet/rcldotnet_node.c | 20 +- rcldotnet/rcldotnet_node.h | 2 + 9 files changed, 785 insertions(+), 8 deletions(-) create mode 100644 rcldotnet/QosProfile.cs create mode 100644 rcldotnet/SafeQosProfileHandle.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index c6c3dd9d..f1476e57 100644 --- 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 index ce877a13..1439d56b 100644 --- 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 CreatePublisher(string topic) where T : IRosMessage + public Publisher CreatePublisher(string topic, QosProfile qosProfile = null) where T : IRosMessage + { + if (qosProfile != null) + { + using (SafeQosProfileHandle qosProfileHandle = QosProfile.CreateQosProfileHandle()) + { + QosProfile.WriteToQosProfileHandle(qosProfile, qosProfileHandle); + return CreatePublisherInner(topic, qosProfileHandle); + } + } + else + { + return CreatePublisherInner(topic, SafeQosProfileHandle.Null); + } + } + + private Publisher CreatePublisherInner(string topic, SafeQosProfileHandle qosProfileHandle) where T : IRosMessage { IntPtr typeSupport = MessageStaticMemberCache.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 CreatePublisher(string topic) where T : IRosMessage return publisher; } - public Subscription CreateSubscription(string topic, Action callback) where T : IRosMessage, new() + public Subscription CreateSubscription(string topic, Action 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 CreateSubscriptionInner(string topic, Action callback, SafeQosProfileHandle qosProfileHandle) where T : IRosMessage, new() { IntPtr typeSupport = MessageStaticMemberCache.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) { diff --git a/rcldotnet/QosProfile.cs b/rcldotnet/QosProfile.cs new file mode 100644 index 00000000..d05d9f8d --- /dev/null +++ b/rcldotnet/QosProfile.cs @@ -0,0 +1,566 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading; + +namespace ROS2 +{ + /// + /// The `HISTORY` DDS QoS policy. + /// + /// A subscription internally maintains a queue of messages (called "samples" in DDS) that have not + /// been processed yet by the application, and likewise a publisher internally maintains a queue. + /// + /// If the history policy is `KeepAll`, this queue is unbounded, and if it is `KeepLast`, it is + /// bounded and old values are discarded when the queue is overfull. + /// + /// # Compatibility + /// | Publisher | Subscription | Compatible | + /// | --------- | ------------ | ---------- | + /// | KeepLast | KeepLast | yes | + /// | KeepLast | KeepAll | yes | + /// | KeepAll | KeepLast | yes | + /// | KeepAll | KeepAll | yes | + /// + public enum QosHistoryPolicy + { + /// + /// Implementation default for history policy. + /// + SystemDefault = 0, + + /// + /// Only store up to a maximum number of samples, dropping oldest once max is exceeded. + /// + KeepLast = 1, + + /// + /// Store all samples, subject to resource limits. + /// + KeepAll = 2, + } + + /// + /// The `RELIABILITY` DDS QoS policy. + /// + /// This policy determines whether delivery between a publisher and a subscription will be retried + /// until successful, or whether messages may be lost in a trade off for better performance. + /// + /// # Compatibility + /// | Publisher | Subscription | Compatible | Behavior | + /// | ---------- | ------------ | ---------- | ----------- | + /// | Reliable | Reliable | yes | Reliable | + /// | Reliable | BestEffort | yes | Best effort | + /// | BestEffort | Reliable | no | - | + /// | BestEffort | BestEffort | yes | Best effort | + /// + public enum QosReliabilityPolicy + { + /// + /// Use the default policy of the RMW layer. + /// + SystemDefault = 0, + + /// + /// Guarantee delivery of messages. + /// + Reliable = 1, + + /// + /// Send messages but do not guarantee delivery. + /// + BestEffort = 2, + } + + /// + /// The `DURABILITY` DDS QoS policy. + /// + /// If a subscription is created after some messages have already been published, it is possible + /// for the subscription to receive a number of previously-published messages by using the + /// "transient local" durability kind on both ends. For this, the publisher must still exist when + /// the subscription is created. + /// + /// # Compatibility + /// | Publisher | Subscription | Compatible | Behavior | + /// | -------------- | -------------- | ---------- | ----------------------------------------- | + /// | TransientLocal | TransientLocal | yes | Deliver old messages to new subscriptions | + /// | TransientLocal | Volatile | yes | Deliver only new messages | + /// | Volatile | TransientLocal | no | - | + /// | Volatile | Volatile | yes | Deliver only new messages | + /// + public enum QosDurabilityPolicy + { + /// + /// Use the default policy of the RMW layer. + /// + SystemDefault = 0, + + /// + /// Re-deliver old messages. + /// - For publishers: Retain messages for later delivery. + /// - For subscriptions: Request delivery of old messages. + /// + TransientLocal = 1, + + /// + /// Do not retain/request old messages. + /// + Volatile = 2, + } + + /// + /// The `LIVELINESS` DDS QoS policy. + /// + /// This policy describes a publisher's reporting policy for its alive status. + /// For a subscription, these are its requirements for its topic's publishers. + /// + /// # Compatibility + /// | Publisher | Subscription | Compatible | + /// | ------------- | ------------- | ---------- | + /// | Automatic | Automatic | yes | + /// | Automatic | ManualByTopic | no | + /// | ManualByTopic | Automatic | yes | + /// | ManualByTopic | ManualByTopic | yes | + /// + public enum QosLivelinessPolicy + { + /// + /// Use the default policy of the RMW layer. + /// + SystemDefault = 0, + + /// + /// The signal that establishes that a topic is alive comes from the ROS `rmw` layer. + /// + Automatic = 1, + + /// + /// The signal that establishes that a topic is alive is sent explicitly. Only publishing a message + /// on the topic or an explicit signal from the application to assert liveliness on the topic + /// will mark the topic as being alive. + /// + ManualByTopic = 3, + } + + /// + /// A Quality of Service profile. + /// + /// See [docs.ros.org][1] on Quality of Service settings in general. + /// + /// In the general case, a topic can have multiple publishers and multiple subscriptions, each with + /// an individual QoS profile. For each publisher-subscription pair, messages are only delivered if + /// their QoS profiles are compatible. + /// + /// [1]: https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html + /// + public sealed class QosProfile + { + // This is private to force use of factory methods. + private QosProfile( + QosHistoryPolicy history, + int depth, + QosReliabilityPolicy reliability, + QosDurabilityPolicy durability, + TimeSpan deadline, + TimeSpan lifespan, + QosLivelinessPolicy liveliness, + TimeSpan livelinessLeaseDuration, + bool avoidRosNamespaceConventions) + { + History = history; + Depth = depth; + Reliability = reliability; + Durability = durability; + Deadline = deadline; + Lifespan = lifespan; + Liveliness = liveliness; + LivelinessLeaseDuration = livelinessLeaseDuration; + AvoidRosNamespaceConventions = avoidRosNamespaceConventions; + } + + // RMW_DURATION_INFINITE is defined as INT64_MAX nanoseconds, which does + // not map nicely to TimeSpan as it uses 100ns ticks as representation. + // + // So this uses Timeout.InfiniteTimeSpan as special value. + // Timeout.InfiniteTimeSpan is defined as -1ms. This does not collide + // with the rmw_time_t values as it is defined as unsigned integers for + // seconds. + + /// + /// Special value for TimeSpan to indicate Infinite on all TimeSpan + /// properties fo this class. + /// + public static TimeSpan InfiniteDuration => Timeout.InfiniteTimeSpan; + + /// + /// The default QoS profile. + /// + public static QosProfile DefaultProfile { get; } = CreateDefaultProfile(); + + /// + /// The history policy. + /// + public QosHistoryPolicy History { get; } + + /// + /// Size of the message queue. + /// + public int Depth { get; } + + /// + /// The reliability policy. + /// + public QosReliabilityPolicy Reliability { get; } + + /// + /// The durability policy. + /// + public QosDurabilityPolicy Durability { get; } + + /// + /// The period at which messages are expected to be sent/received. + /// + /// If this is , messages never miss a deadline expectation. + /// + public TimeSpan Deadline { get; } + + /// + /// The age at which messages are considered expired and no longer valid. + /// + /// If this is , messages do not expire. + /// + public TimeSpan Lifespan { get; } + + /// + /// The liveliness policy. + /// + public QosLivelinessPolicy Liveliness { get; } + + /// + /// The time within which the RMW publisher must show that it is alive. + /// + /// If this is , liveliness is not enforced. + /// + public TimeSpan LivelinessLeaseDuration { get; } + + /// + /// If true, any ROS specific namespacing conventions will be circumvented. + /// + /// In the case of DDS and topics, for example, this means the typical + /// ROS specific prefix of `rt` would not be applied as described [here][1]. + /// + /// This might be useful when trying to directly connect a native DDS topic + /// with a ROS 2 topic. + /// + /// [1]: http://design.ros2.org/articles/topic_and_service_names.html#ros-specific-namespace-prefix + /// + public bool AvoidRosNamespaceConventions { get; } + + /// + /// Create a new QosProfile with the history set to KeepLast and the given depth. + /// + /// Size of the message queue. + public static QosProfile KeepLast(int depth) + { + return DefaultProfile.WithKeepLast(depth); + } + + /// + /// Create a new QosProfile with the history set to KeepAll. + /// + public static QosProfile KeepAll() + { + return DefaultProfile.WithKeepAll(); + } + + /// + /// Set the history to KeepLast and the given depth. + /// + /// Size of the message queue. + /// A new QosProfile with the modification. + public QosProfile WithKeepLast(int depth) + { + return WithHistoryAndDepth(QosHistoryPolicy.KeepLast, depth); + } + + /// + /// Set the history to KeepAll. + /// + /// A new QosProfile with the modification. + public QosProfile WithKeepAll() + { + return WithHistoryAndDepth(QosHistoryPolicy.KeepAll, depth: 0); + } + + // This is private to disallow invalid combination off values. + private QosProfile WithHistoryAndDepth(QosHistoryPolicy history, int depth) + { + var result = new QosProfile( + history: history, + depth: depth, + reliability: Reliability, + durability: Durability, + deadline: Deadline, + lifespan: Lifespan, + liveliness: Liveliness, + livelinessLeaseDuration: LivelinessLeaseDuration, + avoidRosNamespaceConventions: AvoidRosNamespaceConventions); + + return result; + } + + /// + /// Set the . + /// + /// A new QosProfile with the modification. + public QosProfile WithReliability(QosReliabilityPolicy reliability) + { + var result = new QosProfile( + history: History, + depth: Depth, + reliability: reliability, + durability: Durability, + deadline: Deadline, + lifespan: Lifespan, + liveliness: Liveliness, + livelinessLeaseDuration: LivelinessLeaseDuration, + avoidRosNamespaceConventions: AvoidRosNamespaceConventions); + + return result; + } + + /// + /// Set the to . + /// + /// A new QosProfile with the modification. + public QosProfile WithReliable() => WithReliability(QosReliabilityPolicy.Reliable); + + /// + /// Set the to . + /// + /// A new QosProfile with the modification. + public QosProfile WithBestEffort() => WithReliability(QosReliabilityPolicy.BestEffort); + + /// + /// Set the . + /// + /// A new QosProfile with the modification. + public QosProfile WithDurability(QosDurabilityPolicy durability) + { + var result = new QosProfile( + history: History, + depth: Depth, + reliability: Reliability, + durability: durability, + deadline: Deadline, + lifespan: Lifespan, + liveliness: Liveliness, + livelinessLeaseDuration: LivelinessLeaseDuration, + avoidRosNamespaceConventions: AvoidRosNamespaceConventions); + + return result; + } + + /// + /// Set the to . + /// + /// A new QosProfile with the modification. + public QosProfile WithTransientLocal() => WithDurability(QosDurabilityPolicy.TransientLocal); + + /// + /// Set the to . + /// + /// A new QosProfile with the modification. + public QosProfile WithVolatile() => WithDurability(QosDurabilityPolicy.Volatile); + + /// + /// Set the . + /// + /// A new QosProfile with the modification. + public QosProfile WithDeadline(TimeSpan deadline) + { + var result = new QosProfile( + history: History, + depth: Depth, + reliability: Reliability, + durability: Durability, + deadline: deadline, + lifespan: Lifespan, + liveliness: Liveliness, + livelinessLeaseDuration: LivelinessLeaseDuration, + avoidRosNamespaceConventions: AvoidRosNamespaceConventions); + + return result; + } + + /// + /// Set the . + /// + /// A new QosProfile with the modification. + public QosProfile WithLifespan(TimeSpan lifespan) + { + var result = new QosProfile( + history: History, + depth: Depth, + reliability: Reliability, + durability: Durability, + deadline: Deadline, + lifespan: lifespan, + liveliness: Liveliness, + livelinessLeaseDuration: LivelinessLeaseDuration, + avoidRosNamespaceConventions: AvoidRosNamespaceConventions); + + return result; + } + + /// + /// Set the . + /// + /// A new QosProfile with the modification. + public QosProfile WithLiveliness(QosLivelinessPolicy liveliness) + { + var result = new QosProfile( + history: History, + depth: Depth, + reliability: Reliability, + durability: Durability, + deadline: Deadline, + lifespan: Lifespan, + liveliness: liveliness, + livelinessLeaseDuration: LivelinessLeaseDuration, + avoidRosNamespaceConventions: AvoidRosNamespaceConventions); + + return result; + } + + /// + /// Set the . + /// + /// A new QosProfile with the modification. + public QosProfile WithLivelinessLeaseDuration(TimeSpan livelinessLeaseDuration) + { + var result = new QosProfile( + history: History, + depth: Depth, + reliability: Reliability, + durability: Durability, + deadline: Deadline, + lifespan: Lifespan, + liveliness: Liveliness, + livelinessLeaseDuration: livelinessLeaseDuration, + avoidRosNamespaceConventions: AvoidRosNamespaceConventions); + + return result; + } + + /// + /// Set the value. + /// + /// A new QosProfile with the modification. + public QosProfile WithAvoidRosNamespaceConventions(bool avoidRosNamespaceConventions) + { + var result = new QosProfile( + history: History, + depth: Depth, + reliability: Reliability, + durability: Durability, + deadline: Deadline, + lifespan: Lifespan, + liveliness: Liveliness, + livelinessLeaseDuration: LivelinessLeaseDuration, + avoidRosNamespaceConventions: avoidRosNamespaceConventions); + + 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; + } + + internal static SafeQosProfileHandle CreateQosProfileHandle() + { + var qosProfileHandle = new SafeQosProfileHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_qos_profile_handle(ref qosProfileHandle); + if (ret != RCLRet.Ok) + { + qosProfileHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_qos_profile_handle)}() failed."); + } + + return qosProfileHandle; + } + + internal static void WriteToQosProfileHandle(QosProfile qosProfile, SafeQosProfileHandle qosProfileHandle) + { + ToRmwTime(qosProfile.Deadline, out ulong deadlineSec, out ulong deadlineNsec); + ToRmwTime(qosProfile.Lifespan, out ulong lifespanSec, out ulong lifespanNsec); + ToRmwTime(qosProfile.LivelinessLeaseDuration, out ulong livelinessLeaseDurationSec, out ulong livelinessLeaseDurationNsec); + + RCLRet ret = RCLdotnetDelegates.native_rcl_write_to_qos_profile_handle(qosProfileHandle, + history: (int)qosProfile.History, + depth: qosProfile.Depth, + reliability: (int)qosProfile.Reliability, + durability: (int)qosProfile.Durability, + deadlineSec: deadlineSec, + deadlineNsec: deadlineNsec, + lifespanSec: lifespanSec, + lifespanNsec: lifespanNsec, + liveliness: (int)qosProfile.Liveliness, + livelinessLeaseDurationSec: livelinessLeaseDurationSec, + livelinessLeaseDurationNsec: livelinessLeaseDurationNsec, + avoidRosNamespaceConventions: qosProfile.AvoidRosNamespaceConventions ? 1 : 0); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_write_to_qos_profile_handle)}() failed."); + } + + private static void ToRmwTime(TimeSpan timeSpan, out ulong sec, out ulong nsec) + { + if (timeSpan == InfiniteDuration) + { + // see RMW_DURATION_INFINITE and comment on QosProfile.InfiniteDuration above. + sec = 9223372036; + nsec = 854775807; + return; + } + + const long NanosecondsPerSecond = 1_000_000_000; + const long NanosecondsPerTick = NanosecondsPerSecond / TimeSpan.TicksPerSecond; + + long ticks = timeSpan.Ticks; + long seconds = ticks / TimeSpan.TicksPerSecond; + long ticksRoundedDownToFullSecond = seconds * TimeSpan.TicksPerSecond; + long ticksInsideSecond = ticks - ticksRoundedDownToFullSecond; + + sec = (ulong)seconds; + nsec = (ulong)(ticksInsideSecond * NanosecondsPerTick); + } + } +} diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 3407a4fd..10e0bf67 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -326,6 +326,32 @@ internal delegate RCLRet NativeRCLActionExpireGoalsType( internal delegate RCLRet NativeRCLActionGoalHandleGetStatusType(SafeActionGoalHandle actionGoalHandleHandle, out byte status); internal static NativeRCLActionGoalHandleGetStatusType native_rcl_action_goal_handle_get_status = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLCreateQosProfileHandleType(ref SafeQosProfileHandle qosProfileHandle); + internal static NativeRCLCreateQosProfileHandleType native_rcl_create_qos_profile_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLDestroyQosProfileHandleType(IntPtr qosProfileHandle); + internal static NativeRCLDestroyQosProfileHandleType native_rcl_destroy_qos_profile_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLWriteToQosProfileHandleType( + SafeQosProfileHandle qosProfileHandle, + int history, + int depth, + int reliability, + int durability, + ulong deadlineSec, + ulong deadlineNsec, + ulong lifespanSec, + ulong lifespanNsec, + int liveliness, + ulong livelinessLeaseDurationSec, + ulong livelinessLeaseDurationNsec, + int avoidRosNamespaceConventions); + + internal static NativeRCLWriteToQosProfileHandleType native_rcl_write_to_qos_profile_handle = null; + static RCLdotnetDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -660,6 +686,24 @@ static RCLdotnetDelegates() RCLdotnetDelegates.native_rcl_action_goal_handle_get_status = (NativeRCLActionGoalHandleGetStatusType)Marshal.GetDelegateForFunctionPointer( native_rcl_action_goal_handle_get_status_ptr, typeof(NativeRCLActionGoalHandleGetStatusType)); + + IntPtr native_rcl_create_qos_profile_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_qos_profile_handle"); + RCLdotnetDelegates.native_rcl_create_qos_profile_handle = + (NativeRCLCreateQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_qos_profile_handle_ptr, typeof(NativeRCLCreateQosProfileHandleType)); + + IntPtr native_rcl_destroy_qos_profile_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_destroy_qos_profile_handle"); + RCLdotnetDelegates.native_rcl_destroy_qos_profile_handle = + (NativeRCLDestroyQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_qos_profile_handle_ptr, typeof(NativeRCLDestroyQosProfileHandleType)); + + IntPtr native_rcl_write_to_qos_profile_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_write_to_qos_profile_handle"); + RCLdotnetDelegates.native_rcl_write_to_qos_profile_handle = + (NativeRCLWriteToQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_write_to_qos_profile_handle_ptr, typeof(NativeRCLWriteToQosProfileHandleType)); } } diff --git a/rcldotnet/SafeQosProfileHandle.cs b/rcldotnet/SafeQosProfileHandle.cs new file mode 100644 index 00000000..f808f452 --- /dev/null +++ b/rcldotnet/SafeQosProfileHandle.cs @@ -0,0 +1,43 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rmw_qos_profile_t + /// + internal sealed class SafeQosProfileHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public static readonly SafeQosProfileHandle Null = new SafeQosProfileHandle(); + + public SafeQosProfileHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_destroy_qos_profile_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 00eefe64..778f011b 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -673,3 +673,53 @@ int32_t native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle return ret; } + +int32_t native_rcl_create_qos_profile_handle(void **qos_profile_handle) +{ + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)malloc(sizeof(rmw_qos_profile_t)); + *qos_profile = rmw_qos_profile_default; + *qos_profile_handle = (void *)qos_profile; + + return RCL_RET_OK; +} + +int32_t native_rcl_destroy_qos_profile_handle(void *qos_profile_handle) +{ + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + free(qos_profile); + + return RCL_RET_OK; +} + +int32_t native_rcl_write_to_qos_profile_handle( + void *qos_profile_handle, + int32_t history, + int32_t depth, + int32_t reliability, + int32_t durability, + uint64_t deadline_sec, + uint64_t deadline_nsec, + uint64_t lifespan_sec, + uint64_t lifespan_nsec, + int32_t liveliness, + uint64_t liveliness_lease_duration_sec, + uint64_t liveliness_lease_duration_nsec, + int32_t /* bool */ avoid_ros_namespace_conventions) +{ + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + + qos_profile->history = (enum rmw_qos_history_policy_t)history; + qos_profile->depth = (size_t)depth; + qos_profile->reliability = (enum rmw_qos_reliability_policy_t)reliability; + qos_profile->durability = (enum rmw_qos_durability_policy_t)durability; + qos_profile->deadline.sec = deadline_sec; + qos_profile->deadline.nsec = deadline_nsec; + qos_profile->lifespan.sec = lifespan_sec; + qos_profile->lifespan.nsec = lifespan_nsec; + qos_profile->liveliness = (enum rmw_qos_liveliness_policy_t)liveliness; + qos_profile->liveliness_lease_duration.sec = liveliness_lease_duration_sec; + qos_profile->liveliness_lease_duration.nsec = liveliness_lease_duration_nsec; + qos_profile->avoid_ros_namespace_conventions = avoid_ros_namespace_conventions != 0; + + return RCL_RET_OK; +} diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index e0280cea..6c4252ff 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -216,4 +216,26 @@ int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goa RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_create_qos_profile_handle(void **qos_profile_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_qos_profile_handle(void *qos_profile_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_write_to_qos_profile_handle( + void *qos_profile_handle, + int32_t history, + int32_t depth, + int32_t reliability, + int32_t durability, + uint64_t deadline_sec, + uint64_t deadline_nsec, + uint64_t lifespan_sec, + uint64_t lifespan_nsec, + int32_t liveliness, + uint64_t liveliness_lease_duration_sec, + uint64_t liveliness_lease_duration_nsec, + int32_t /* bool */ avoid_ros_namespace_conventions); + #endif // RCLDOTNET_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index aa7ca1a2..3dfa900a 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -29,9 +29,12 @@ int32_t native_rcl_create_publisher_handle(void **publisher_handle, void *node_handle, const char *topic, - void *typesupport) { + void *typesupport, + void *qos_profile_handle) { rcl_node_t *node = (rcl_node_t *)node_handle; + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + rosidl_message_type_support_t *ts = (rosidl_message_type_support_t *)typesupport; @@ -40,6 +43,11 @@ int32_t native_rcl_create_publisher_handle(void **publisher_handle, *publisher = rcl_get_zero_initialized_publisher(); rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); + if (qos_profile != NULL) + { + publisher_ops.qos = *qos_profile; + } + rcl_ret_t ret = rcl_publisher_init(publisher, node, ts, topic, &publisher_ops); @@ -61,9 +69,12 @@ int32_t native_rcl_destroy_publisher_handle(void *publisher_handle, void *node_h int32_t native_rcl_create_subscription_handle(void **subscription_handle, void *node_handle, const char *topic, - void *typesupport) { + void *typesupport, + void *qos_profile_handle) { rcl_node_t *node = (rcl_node_t *)node_handle; + rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)qos_profile_handle; + rosidl_message_type_support_t *ts = (rosidl_message_type_support_t *)typesupport; @@ -73,6 +84,11 @@ int32_t native_rcl_create_subscription_handle(void **subscription_handle, rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); + if (qos_profile != NULL) + { + subscription_ops.qos = *qos_profile; + } + rcl_ret_t ret = rcl_subscription_init(subscription, node, ts, topic, &subscription_ops); diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index 99c20cde..f28da07e 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -20,6 +20,7 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_publisher_handle(void **, void *, const char *, + void *, void *); RCLDOTNET_EXPORT @@ -28,6 +29,7 @@ int32_t RCLDOTNET_CDECL native_rcl_destroy_publisher_handle(void *publisher_hand RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_subscription_handle(void **, void *, const char *, + void *, void *); RCLDOTNET_EXPORT From bb168317f59af5ba1c3b109f255389477f1927dc Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 30 Nov 2022 04:58:01 +0100 Subject: [PATCH 65/73] missing --- .../rosidl_generator_dotnet_generate_interfaces.cmake | 8 ++++---- rosidl_generator_dotnet/package.xml | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake index 52e9aadc..316e7608 100644 --- a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake +++ b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake @@ -25,10 +25,10 @@ find_package(DotNETExtra REQUIRED) # Get a list of typesupport implementations from valid rmw implementations. rosidl_generator_dotnet_get_typesupports(_typesupport_impls) -if(_typesupport_impls STREQUAL "") - message(WARNING "No valid typesupport for .NET generator. .NET messages will not be generated.") - return() -endif() +# if(_typesupport_impls STREQUAL "") +# message(WARNING "No valid typesupport for .NET generator. .NET messages will not be generated.") +# return() +# endif() set(_output_path "${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_dotnet/${PROJECT_NAME}") diff --git a/rosidl_generator_dotnet/package.xml b/rosidl_generator_dotnet/package.xml index 40d363b8..493d4482 100644 --- a/rosidl_generator_dotnet/package.xml +++ b/rosidl_generator_dotnet/package.xml @@ -21,7 +21,7 @@ rosidl_runtime_c - rmw_implementation + rmw_implementation_cmake rosidl_runtime_c rosidl_generator_c From 328145e88f58f249ea365df76c4d7df2c385dbd1 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Fri, 9 Jun 2023 13:09:28 +0200 Subject: [PATCH 66/73] first attempt --- rcldotnet/CMakeLists.txt | 4 ++ rcldotnet/RCLdotnet.cs | 63 +++++++++++++++++++++- rcldotnet/SafeSubscriptionHandle.cs | 2 +- rcldotnet/Subscription.cs | 4 +- rcldotnet/package.xml | 4 ++ rosidl_generator_dotnet/resource/msg.cs.em | 56 +++++++++---------- 6 files changed, 101 insertions(+), 32 deletions(-) diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index f1476e57..175bd0ad 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -15,6 +15,8 @@ find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(rosidl_generator_c REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(dotnet_cmake_module REQUIRED) find_package(DotNETExtra REQUIRED) @@ -72,6 +74,8 @@ set(_assemblies_dep_dlls ${builtin_interfaces_ASSEMBLIES_DLL} ${rcldotnet_common_ASSEMBLIES_DLL} ${unique_identifier_msgs_ASSEMBLIES_DLL} + ${sensor_msgs_ASSEMBLIES_DLL} + ${std_msgs_ASSEMBLIES_DLL} ) add_dotnet_library(${PROJECT_NAME}_assemblies diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 10e0bf67..093854d1 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -18,6 +18,7 @@ using action_msgs.msg; using action_msgs.srv; using ROS2.Utils; +using sensor_msgs.msg; namespace ROS2 { @@ -848,7 +849,14 @@ private static bool Take(Subscription subscription, IRosMessage message) switch (ret) { case RCLRet.Ok: - ReadFromMessageHandle(message, messageHandle); + if (!(message is sensor_msgs.msg.Image)) + { + ReadFromMessageHandle(message, messageHandle); + } + else + { + HackedReadFromMessageHandle(message, messageHandle); + } return true; case RCLRet.SubscriptionTakeFailed: @@ -1411,6 +1419,59 @@ public static string GetRMWIdentifier() return rmw_identifier; } + + + public static void HackedImage__ReadFromHandle(global::System.IntPtr messageHandle, sensor_msgs.msg.Image image) + { + image.Header.__ReadFromHandle(sensor_msgs.msg.Image.native_get_field_header_HANDLE(messageHandle)); + image.Height = sensor_msgs.msg.Image.native_read_field_height(messageHandle); + image.Width = sensor_msgs.msg.Image.native_read_field_width(messageHandle); + IntPtr pStr_Encoding = sensor_msgs.msg.Image.native_read_field_encoding(messageHandle); + image.Encoding = Marshal.PtrToStringAnsi(pStr_Encoding); + image.Encoding="HackedEncoding"; + image.IsBigendian = sensor_msgs.msg.Image.native_read_field_is_bigendian(messageHandle); + image.Step = sensor_msgs.msg.Image.native_read_field_step(messageHandle); + { + int size__local_variable = sensor_msgs.msg.Image.native_getsize_field_data_message(messageHandle); + + //nav_msgs__msg__OccupancyGrid__get_field_data_message + + IntPtr first = sensor_msgs.msg.Image.native_get_field_data_message(messageHandle,0); + + image.Data = new System.Collections.Generic.List(size__local_variable); + for (int i__local_variable = 0; i__local_variable < size__local_variable; i__local_variable++) + { + image.Data.Add(sensor_msgs.msg.Image.native_read_field_data(sensor_msgs.msg.Image.native_get_field_data_message(messageHandle, i__local_variable))); + } + } + } + + internal static void HackedReadFromMessageHandle(IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as + // this needs to be handled in generated code across multiple + // assemblies. Array and collection indexing would need to + // create SafeHandles everywhere. It's not worth it, especially + // considering the extra allocations for SafeHandles in arrays + // or collections that don't really represent their own native + // resource. + messageHandle.DangerousAddRef(ref mustRelease); + HackedImage__ReadFromHandle(messageHandle.DangerousGetHandle(),message as sensor_msgs.msg.Image); + //message.__ReadFromHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } + + internal static void ReadFromMessageHandle(IRosMessage message, SafeHandle messageHandle) { bool mustRelease = false; diff --git a/rcldotnet/SafeSubscriptionHandle.cs b/rcldotnet/SafeSubscriptionHandle.cs index a7075035..9dd3f397 100644 --- a/rcldotnet/SafeSubscriptionHandle.cs +++ b/rcldotnet/SafeSubscriptionHandle.cs @@ -28,7 +28,7 @@ namespace ROS2 /// of the subscription handle and node handle. This also applies to publisher, service and client handles, /// which point to a rcl_publisher_t, rcl_service_t or rcl_client_t. /// - internal sealed class SafeSubscriptionHandle : SafeHandleZeroOrMinusOneIsInvalid + public sealed class SafeSubscriptionHandle : SafeHandleZeroOrMinusOneIsInvalid { // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 // Commit from early 2016, but still in current .NET as of september 2021: diff --git a/rcldotnet/Subscription.cs b/rcldotnet/Subscription.cs index cc639f37..754d2698 100644 --- a/rcldotnet/Subscription.cs +++ b/rcldotnet/Subscription.cs @@ -34,7 +34,7 @@ internal Subscription() // internal handle is disposed. // By relying on the GC/Finalizer of SafeHandle the handle only gets // Disposed if the subscription is not live anymore. - internal abstract SafeSubscriptionHandle Handle { get; } + public abstract SafeSubscriptionHandle Handle { get; } internal abstract IRosMessage CreateMessage(); @@ -54,7 +54,7 @@ internal Subscription(SafeSubscriptionHandle handle, Action callback) _callback = callback; } - internal override SafeSubscriptionHandle Handle { get; } + public override SafeSubscriptionHandle Handle { get; } internal override IRosMessage CreateMessage() => (IRosMessage)new T(); diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index 88be5f0f..22e02f80 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -16,6 +16,7 @@ rmw_implementation_cmake rosidl_generator_c rosidl_parser + std_msgs rmw_implementation_cmake rcl @@ -24,12 +25,15 @@ action_msgs builtin_interfaces unique_identifier_msgs + sensor_msgs + std_msgs rcl rcl_action action_msgs builtin_interfaces unique_identifier_msgs + unique_identifier_msgs std_msgs test_msgs diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 5d4ca694..3568152d 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -28,7 +28,7 @@ namespace @('.'.join(message.structure.namespaced_type.namespaces)) { public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) { - private static readonly DllLoadUtils dllLoadUtils; + public static readonly DllLoadUtils dllLoadUtils; @[for member in message.structure.members]@ @[ if isinstance(member.type, Array)]@ @@ -181,54 +181,54 @@ public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) } [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate IntPtr NativeGetTypeSupportType(); + public delegate IntPtr NativeGetTypeSupportType(); - private static NativeGetTypeSupportType native_get_typesupport = null; + public static NativeGetTypeSupportType native_get_typesupport = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate Safe@(type_name)Handle NativeCreateNativeType(); + public delegate Safe@(type_name)Handle NativeCreateNativeType(); - private static NativeCreateNativeType native_create_native_message = null; + public static NativeCreateNativeType native_create_native_message = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate void NativeDestroyNativeType(IntPtr messageHandle); + public delegate void NativeDestroyNativeType(IntPtr messageHandle); - private static NativeDestroyNativeType native_destroy_native_message = null; + public static NativeDestroyNativeType native_destroy_native_message = null; @[for member in message.structure.members]@ @[ if isinstance(member.type, Array) or isinstance(member.type, AbstractSequence)]@ - private static NativeGetField@(get_field_name(type_name, member.name))Type native_get_field_@(member.name)_message = null; + public static NativeGetField@(get_field_name(type_name, member.name))Type native_get_field_@(member.name)_message = null; @[ if isinstance(member.type, AbstractSequence)]@ - private static NativeGetSizeField@(get_field_name(type_name, member.name))Type native_getsize_field_@(member.name)_message = null; - private static NativeInitSequenceField@(get_field_name(type_name, member.name))Type native_init_seqence_field_@(member.name)_message = null; + public static NativeGetSizeField@(get_field_name(type_name, member.name))Type native_getsize_field_@(member.name)_message = null; + public static NativeInitSequenceField@(get_field_name(type_name, member.name))Type native_init_seqence_field_@(member.name)_message = null; @[ end if]@ @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ - private static NativeWriteField@(get_field_name(type_name, member.name))Type native_write_field_@(member.name) = null; - private static NativeReadField@(get_field_name(type_name, member.name))Type native_read_field_@(member.name) = null; + public static NativeWriteField@(get_field_name(type_name, member.name))Type native_write_field_@(member.name) = null; + public static NativeReadField@(get_field_name(type_name, member.name))Type native_read_field_@(member.name) = null; @[ end if]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate IntPtr NativeGetField@(get_field_name(type_name, member.name))Type( + public delegate IntPtr NativeGetField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, int index); @[ if isinstance(member.type, AbstractSequence)]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate int NativeGetSizeField@(get_field_name(type_name, member.name))Type( + public delegate int NativeGetSizeField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate bool NativeInitSequenceField@(get_field_name(type_name, member.name))Type( + public delegate bool NativeInitSequenceField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, int size); @[ end if]@ @[ if isinstance(member.type.value_type, AbstractString)]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( + public delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, [MarshalAs (UnmanagedType.LPStr)] string value); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate IntPtr NativeReadField@(get_field_name(type_name, member.name))Type(IntPtr messageHandle); + public delegate IntPtr NativeReadField@(get_field_name(type_name, member.name))Type(IntPtr messageHandle); @[ else]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate @(get_dotnet_type(member.type.value_type)) NativeReadField@(get_field_name(type_name, member.name))Type( + public delegate @(get_dotnet_type(member.type.value_type)) NativeReadField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( + public delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, @(get_dotnet_type(member.type.value_type)) value); @[ end if]@ @[ elif isinstance(member.type, AbstractWString)]@ @@ -236,28 +236,28 @@ public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) @[ elif isinstance(member.type, BasicType) or isinstance(member.type, AbstractString)]@ @[ if isinstance(member.type, AbstractString)]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate IntPtr NativeReadField@(get_field_name(type_name, member.name))Type(IntPtr messageHandle); + public delegate IntPtr NativeReadField@(get_field_name(type_name, member.name))Type(IntPtr messageHandle); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( + public delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, [MarshalAs (UnmanagedType.LPStr)] string value); @[ else]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate @(get_dotnet_type(member.type)) NativeReadField@(get_field_name(type_name, member.name))Type( + public delegate @(get_dotnet_type(member.type)) NativeReadField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( + public delegate void NativeWriteField@(get_field_name(type_name, member.name))Type( IntPtr messageHandle, @(get_dotnet_type(member.type)) value); @[ end if]@ - private static NativeReadField@(get_field_name(type_name, member.name))Type native_read_field_@(member.name) = null; + public static NativeReadField@(get_field_name(type_name, member.name))Type native_read_field_@(member.name) = null; - private static NativeWriteField@(get_field_name(type_name, member.name))Type native_write_field_@(member.name) = null; + public static NativeWriteField@(get_field_name(type_name, member.name))Type native_write_field_@(member.name) = null; @[ else]@ [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - private delegate IntPtr NativeGetField@(get_field_name(type_name, member.name))Type(IntPtr messageHandle); + public delegate IntPtr NativeGetField@(get_field_name(type_name, member.name))Type(IntPtr messageHandle); - private static NativeGetField@(get_field_name(type_name, member.name))Type native_get_field_@(member.name)_HANDLE = null; + public static NativeGetField@(get_field_name(type_name, member.name))Type native_get_field_@(member.name)_HANDLE = null; @[ end if]@ @[end for]@ @@ -407,7 +407,7 @@ public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) } @[end if]@ - private sealed class Safe@(type_name)Handle : global::System.Runtime.InteropServices.SafeHandle + public sealed class Safe@(type_name)Handle : global::System.Runtime.InteropServices.SafeHandle { public Safe@(type_name)Handle() : base(global::System.IntPtr.Zero, true) { } From fd7b86d9cd6a73681dcf1d196045c8e2f16545e5 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Tue, 13 Jun 2023 11:02:53 +0200 Subject: [PATCH 67/73] image performance --- rcldotnet/RCLdotnet.cs | 85 +++++++++++++++++++++++++++++++++++------- 1 file changed, 71 insertions(+), 14 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 093854d1..6fe2a3cb 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -19,6 +19,8 @@ using action_msgs.srv; using ROS2.Utils; using sensor_msgs.msg; +using System.Collections.Generic; +using System.Diagnostics; namespace ROS2 { @@ -855,7 +857,7 @@ private static bool Take(Subscription subscription, IRosMessage message) } else { - HackedReadFromMessageHandle(message, messageHandle); + HackedReadFromMessageHandle(subscription, message, messageHandle); } return true; @@ -1420,33 +1422,84 @@ public static string GetRMWIdentifier() } + // WORKAROUND IMAGES + // public static Dictionary lastImages =new Dictionary(); + + public static Dictionary imageSubscription = new Dictionary(); + + public static Dictionary lastImagesBytes = new Dictionary(); + + public static object IMG_DATA_LOCK = new object(); + - public static void HackedImage__ReadFromHandle(global::System.IntPtr messageHandle, sensor_msgs.msg.Image image) + public static void HackedImage__ReadFromHandle(Subscription subscription, global::System.IntPtr messageHandle, sensor_msgs.msg.Image image) { + Debug.WriteLine("HackedImage__ReadFromHandle"); + var sw = System.Diagnostics.Stopwatch.StartNew(); + + lock(IMG_DATA_LOCK) + { image.Header.__ReadFromHandle(sensor_msgs.msg.Image.native_get_field_header_HANDLE(messageHandle)); image.Height = sensor_msgs.msg.Image.native_read_field_height(messageHandle); image.Width = sensor_msgs.msg.Image.native_read_field_width(messageHandle); IntPtr pStr_Encoding = sensor_msgs.msg.Image.native_read_field_encoding(messageHandle); image.Encoding = Marshal.PtrToStringAnsi(pStr_Encoding); - image.Encoding="HackedEncoding"; + image.IsBigendian = sensor_msgs.msg.Image.native_read_field_is_bigendian(messageHandle); image.Step = sensor_msgs.msg.Image.native_read_field_step(messageHandle); - { - int size__local_variable = sensor_msgs.msg.Image.native_getsize_field_data_message(messageHandle); + { + int size__local_variable = sensor_msgs.msg.Image.native_getsize_field_data_message(messageHandle); - //nav_msgs__msg__OccupancyGrid__get_field_data_message - - IntPtr first = sensor_msgs.msg.Image.native_get_field_data_message(messageHandle,0); + HackedMemoryCopy(subscription, messageHandle, image, size__local_variable); - image.Data = new System.Collections.Generic.List(size__local_variable); - for (int i__local_variable = 0; i__local_variable < size__local_variable; i__local_variable++) - { - image.Data.Add(sensor_msgs.msg.Image.native_read_field_data(sensor_msgs.msg.Image.native_get_field_data_message(messageHandle, i__local_variable))); + // sw.Stop(); + + // var sw2 = System.Diagnostics.Stopwatch.StartNew(); + + // image.Data = new System.Collections.Generic.List(size__local_variable); + // for (int i__local_variable = 0; i__local_variable < size__local_variable; i__local_variable++) + // { + // image.Data.Add(sensor_msgs.msg.Image.native_read_field_data(sensor_msgs.msg.Image.native_get_field_data_message(messageHandle, i__local_variable))); + // } + + // sw2.Stop(); + // Console.WriteLine($"copying image took: {sw.ElapsedTicks} ticks, {sw2.ElapsedTicks} ticks: {(float)(sw.ElapsedTicks - sw2.ElapsedTicks)/(float)sw.ElapsedTicks}"); } } + sw.Stop(); + Debug.WriteLine($"HackedImage__ReadFromHandle END: {sw.ElapsedMilliseconds} ms"); } - internal static void HackedReadFromMessageHandle(IRosMessage message, SafeHandle messageHandle) + private static void HackedMemoryCopy(Subscription subscription, IntPtr messageHandle, sensor_msgs.msg.Image image, int size__local_variable) + { + IntPtr first = sensor_msgs.msg.Image.native_get_field_data_message(messageHandle, 0); + + // lastImagesHandles[image] = new Tuple(first, (uint)size__local_variable); + + byte[] bytes = null; + if (imageSubscription.ContainsKey(subscription)) + { + var lastImage = imageSubscription[subscription]; + bytes = lastImagesBytes[lastImage]; + lastImagesBytes.Remove(lastImage); + } + + if (bytes == null || bytes.Length != size__local_variable) + { + Debug.WriteLine("HackedImage__ReadFromHandle: new byte buffer created"); + + bytes = new byte[size__local_variable]; + // Console.WriteLine($"makmustReleaseing new size for image subscription: {size__local_variable} subscription: {subscription.GetHashCode()}"); + } + + + Marshal.Copy(first,bytes, 0, (int)(size__local_variable)); + + lastImagesBytes[image] = bytes; + imageSubscription[subscription] = image; + } + + internal static void HackedReadFromMessageHandle(Subscription subscription, IRosMessage message, SafeHandle messageHandle) { bool mustRelease = false; try @@ -1459,9 +1512,13 @@ internal static void HackedReadFromMessageHandle(IRosMessage message, SafeHandle // or collections that don't really represent their own native // resource. messageHandle.DangerousAddRef(ref mustRelease); - HackedImage__ReadFromHandle(messageHandle.DangerousGetHandle(),message as sensor_msgs.msg.Image); + HackedImage__ReadFromHandle(subscription, messageHandle.DangerousGetHandle(),message as sensor_msgs.msg.Image); //message.__ReadFromHandle(messageHandle.DangerousGetHandle()); } + catch(Exception e) + { + Debug.WriteLine("HackedImage__ReadFromHandle: exception"); + } finally { if (mustRelease) From d2b7553b303422d9946d4563875c417df3d7a4ce Mon Sep 17 00:00:00 2001 From: Claudia Date: Tue, 20 Feb 2024 09:20:57 +0100 Subject: [PATCH 68/73] solvin merge errors --- rcldotnet/rcldotnet_node.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index 11644c80..7222d9bd 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -223,8 +223,6 @@ int32_t native_rcl_action_create_server_handle(void **action_server_handle, rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - rcl_clock_t *clock = native_rcl_get_default_clock(); - rcl_ret_t ret = rcl_action_server_init(action_server, node, clock, ts, action_name, &action_server_ops); From 58b970bf91f62583fcfad99acdc98f37af24d02d Mon Sep 17 00:00:00 2001 From: Claudia Date: Wed, 21 Feb 2024 14:19:49 +0100 Subject: [PATCH 69/73] fixing unknown platform error --- rcldotnet_common/DllLoadUtils.cs | 71 ++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/rcldotnet_common/DllLoadUtils.cs b/rcldotnet_common/DllLoadUtils.cs index 556d80a9..f112b5e4 100644 --- a/rcldotnet_common/DllLoadUtils.cs +++ b/rcldotnet_common/DllLoadUtils.cs @@ -42,6 +42,7 @@ public enum Platform MacOSX, WindowsDesktop, UWP, + Android, Unknown } @@ -65,6 +66,12 @@ public class DllLoadUtilsFactory [DllImport("libdl.so.2", EntryPoint = "dlclose")] private static extern int dlclose_unix(IntPtr handle); + [DllImport("libdl.so", EntryPoint = "dlopen")] + private static extern IntPtr dlopen_android(string fileName, int flags); + + [DllImport("libdl.so", EntryPoint = "dlclose")] + private static extern int dlclose_android(IntPtr handle); + [DllImport("libdl.dylib", EntryPoint = "dlopen")] private static extern IntPtr dlopen_macosx(string fileName, int flags); @@ -85,6 +92,8 @@ public static DllLoadUtils GetDllLoadUtils() return new DllLoadUtilsWindowsDesktop(); case Platform.UWP: return new DllLoadUtilsUWP(); + case Platform.Android: + return new DllLoadUtilsAndroid(); case Platform.Unknown: default: throw new UnknownPlatformError(); @@ -133,6 +142,20 @@ private static bool IsUnix() } } + private static bool IsAndroid() + { + try + { + IntPtr ptr = dlopen_android("libdl.so", RTLD_NOW); + dlclose_android(ptr); + return true; + } + catch (TypeLoadException) + { + return false; + } + } + private static bool IsMacOSX() { try @@ -165,6 +188,10 @@ private static Platform CheckPlatform() { return Platform.UWP; } + else if (IsAndroid()) + { + return Platform.Android; + } else { return Platform.Unknown; @@ -253,6 +280,50 @@ public override IntPtr LoadLibrary(string fileName) } } + internal class DllLoadUtilsAndroid : DllLoadUtilsAbs + { + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern IntPtr dlopen(string fileName, int flags); + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern IntPtr dlsym(IntPtr handle, string symbol); + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern int dlclose(IntPtr handle); + + [DllImport("libdl.so", ExactSpelling = true)] + private static extern IntPtr dlerror(); + + private const int RTLD_NOW = 2; + + public override void FreeLibrary(IntPtr handle) => dlclose(handle); + + public override IntPtr GetProcAddress(IntPtr dllHandle, string name) + { + // clear previous errors if any + dlerror(); + var res = dlsym(dllHandle, name); + var errPtr = dlerror(); + if (errPtr != IntPtr.Zero) + { + throw new Exception("dlsym: " + Marshal.PtrToStringAnsi(errPtr)); + } + return res; + } + + public override IntPtr LoadLibrary(string fileName) + { + string libraryName = "lib" + fileName + "_native.so"; + IntPtr ptr = dlopen(libraryName, RTLD_NOW); + if (ptr == IntPtr.Zero) + { + throw new UnsatisfiedLinkError(libraryName); + } + return ptr; + } + } + internal class DllLoadUtilsUnix : DllLoadUtilsAbs { From e328135cc04082da4da7d46dbda4a5fac5248f19 Mon Sep 17 00:00:00 2001 From: claudia Date: Tue, 19 Mar 2024 13:28:07 +0100 Subject: [PATCH 70/73] update net7.0 --- rcldotnet/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 7ae994da..2bd0f0c1 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -145,7 +145,7 @@ if(BUILD_TESTING) if(DEFINED ENV{RCLDOTNET_TEST_TARGET_FRAMEWORK}) set(RCLDOTNET_TEST_TARGET_FRAMEWORK $ENV{RCLDOTNET_TEST_TARGET_FRAMEWORK}) else() - set(RCLDOTNET_TEST_TARGET_FRAMEWORK "net6.0") + set(RCLDOTNET_TEST_TARGET_FRAMEWORK "net7.0") endif() add_dotnet_test(test_messages From 30bdf4fa8ea739ca9e677a278c6436f82e7989d7 Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 9 Nov 2024 01:43:15 +0100 Subject: [PATCH 71/73] pointcloud performance --- rcldotnet/RCLdotnet.cs | 95 +++++++++++++++++++++++++++++++++++++++--- 1 file changed, 90 insertions(+), 5 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 157a162e..6ff0780e 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -901,14 +901,21 @@ private static bool Take(Subscription subscription, IRosMessage message) switch (ret) { case RCLRet.Ok: - if (!(message is sensor_msgs.msg.Image)) + if (message is sensor_msgs.msg.Image) { - ReadFromMessageHandle(message, messageHandle); + HackedReadFromImageMessageHandle(subscription, message, messageHandle); + + } + else if (message is sensor_msgs.msg.PointCloud2) + { + Console.WriteLine("PointCloud2"); + HackedReadFromPointCloudMessageHandle(subscription, message, messageHandle); } else { - HackedReadFromMessageHandle(subscription, message, messageHandle); + ReadFromMessageHandle(message, messageHandle); } + return true; case RCLRet.SubscriptionTakeFailed: @@ -1499,11 +1506,48 @@ public static string GetRMWIdentifier() // WORKAROUND IMAGES // public static Dictionary lastImages =new Dictionary(); + public static Dictionary pointCloudSubscription = new Dictionary(); + public static Dictionary imageSubscription = new Dictionary(); public static Dictionary lastImagesBytes = new Dictionary(); + public static Dictionary lastPointCloudsBytes = new Dictionary(); + public static object IMG_DATA_LOCK = new object(); + public static object POINTCLOUD_DATA_LOCK = new object(); + + public static byte[] GetLastPointCloudBytes(sensor_msgs.msg.PointCloud2 pointCloud2) + { + lock (POINTCLOUD_DATA_LOCK) + { + if (lastPointCloudsBytes.ContainsKey(pointCloud2)) + { + return lastPointCloudsBytes[pointCloud2]; + } + return null; + } + } + + public static void HackedPointCloud__ReadFromHandle(Subscription subscription, global::System.IntPtr messageHandle, sensor_msgs.msg.PointCloud2 pointCloud2) + { + Debug.WriteLine("HackedPointCloud__ReadFromHandle"); + var sw = System.Diagnostics.Stopwatch.StartNew(); + lock(POINTCLOUD_DATA_LOCK) + { + pointCloud2.Header.__ReadFromHandle(sensor_msgs.msg.PointCloud2.native_get_field_header_HANDLE(messageHandle)); + pointCloud2.Height = sensor_msgs.msg.PointCloud2.native_read_field_height(messageHandle); + pointCloud2.Width = sensor_msgs.msg.PointCloud2.native_read_field_width(messageHandle); + pointCloud2.PointStep = sensor_msgs.msg.PointCloud2.native_read_field_point_step(messageHandle); + pointCloud2.RowStep = sensor_msgs.msg.PointCloud2.native_read_field_row_step(messageHandle); + pointCloud2.IsBigendian = sensor_msgs.msg.PointCloud2.native_read_field_is_bigendian(messageHandle); + { + int size = sensor_msgs.msg.PointCloud2.native_getsize_field_data_message(messageHandle); + HackedPointCloudMemoryCopy(subscription, messageHandle, pointCloud2, size); + } + + } + } public static void HackedImage__ReadFromHandle(Subscription subscription, global::System.IntPtr messageHandle, sensor_msgs.msg.Image image) { @@ -1545,7 +1589,29 @@ public static void HackedImage__ReadFromHandle(Subscription subscription, global Debug.WriteLine($"HackedImage__ReadFromHandle END: {sw.ElapsedMilliseconds} ms"); } - private static void HackedMemoryCopy(Subscription subscription, IntPtr messageHandle, sensor_msgs.msg.Image image, int size__local_variable) + private static void HackedPointCloudMemoryCopy(Subscription subscription, IntPtr messageHandle, sensor_msgs.msg.PointCloud2 pointCloud2, int size__local_variable) + { + IntPtr first = sensor_msgs.msg.PointCloud2.native_get_field_data_message(messageHandle, 0); + byte[] bytes = null; + if (pointCloudSubscription.ContainsKey(subscription)) + { + var lastPointCloud = pointCloudSubscription[subscription]; + bytes = lastPointCloudsBytes[lastPointCloud]; + lastPointCloudsBytes.Remove(lastPointCloud); + } + + if (bytes == null || bytes.Length != size__local_variable) + { + bytes = new byte[size__local_variable]; + lastPointCloudsBytes[pointCloud2] = bytes; + } + + Marshal.Copy(first, bytes, 0, (int)(size__local_variable)); + lastPointCloudsBytes[pointCloud2] = bytes; + pointCloudSubscription[subscription] = pointCloud2; + } + + private static void HackedMemoryCopy(Subscription subscription, IntPtr messageHandle, sensor_msgs.msg.Image image, int size__local_variable) { IntPtr first = sensor_msgs.msg.Image.native_get_field_data_message(messageHandle, 0); @@ -1574,7 +1640,26 @@ private static void HackedMemoryCopy(Subscription subscription, IntPtr messageHa imageSubscription[subscription] = image; } - internal static void HackedReadFromMessageHandle(Subscription subscription, IRosMessage message, SafeHandle messageHandle) + internal static void HackedReadFromPointCloudMessageHandle(Subscription subscription, IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + messageHandle.DangerousAddRef(ref mustRelease); + HackedPointCloud__ReadFromHandle(subscription, messageHandle.DangerousGetHandle(),message as sensor_msgs.msg.PointCloud2); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } + + + + internal static void HackedReadFromImageMessageHandle(Subscription subscription, IRosMessage message, SafeHandle messageHandle) { bool mustRelease = false; try From 08ec73a8b0eec5ef74a962c91b4d69a05c5dd62b Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Sat, 9 Nov 2024 02:00:50 +0100 Subject: [PATCH 72/73] safe bytes --- rcldotnet/RCLdotnet.cs | 32 +++++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 6ff0780e..53863fc8 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -1517,18 +1517,44 @@ public static string GetRMWIdentifier() public static object IMG_DATA_LOCK = new object(); public static object POINTCLOUD_DATA_LOCK = new object(); - public static byte[] GetLastPointCloudBytes(sensor_msgs.msg.PointCloud2 pointCloud2) + + public static void GetLastPointCloudCopy(sensor_msgs.msg.PointCloud2 pointCloud2, ref byte[] pointCloudData) { lock (POINTCLOUD_DATA_LOCK) { if (lastPointCloudsBytes.ContainsKey(pointCloud2)) { - return lastPointCloudsBytes[pointCloud2]; + var cloudData = lastPointCloudsBytes[pointCloud2]; + + // cloudData = lastPointCloudsBytes[pointCloud2]; + if(pointCloudData == null || pointCloudData.Length != cloudData.Length) + { + pointCloudData = new byte[lastPointCloudsBytes[pointCloud2].Length]; + } + + Buffer.BlockCopy(cloudData, 0, pointCloudData, 0, cloudData.Length); + // Marshal.Copy(lastPointCloudsBytes[pointCloud2], 0, pointCloudData, cloudData.Length); + + } + else + { + pointCloudData = null; } - return null; } } + // public static byte[] GetLastPointCloudBytes(sensor_msgs.msg.PointCloud2 pointCloud2) + // { + // lock (POINTCLOUD_DATA_LOCK) + // { + // if (lastPointCloudsBytes.ContainsKey(pointCloud2)) + // { + // return lastPointCloudsBytes[pointCloud2]; + // } + // return null; + // } + // } + public static void HackedPointCloud__ReadFromHandle(Subscription subscription, global::System.IntPtr messageHandle, sensor_msgs.msg.PointCloud2 pointCloud2) { Debug.WriteLine("HackedPointCloud__ReadFromHandle"); From db91259b1ab605f8a5d3ccaa657ca773f321ed0a Mon Sep 17 00:00:00 2001 From: pabloinigoblasco Date: Wed, 13 Nov 2024 21:39:08 +0100 Subject: [PATCH 73/73] minor --- rcldotnet/RCLdotnet.cs | 35 +++++++++++++++++------------------ 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 53863fc8..a19a304f 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -908,7 +908,7 @@ private static bool Take(Subscription subscription, IRosMessage message) } else if (message is sensor_msgs.msg.PointCloud2) { - Console.WriteLine("PointCloud2"); + // Console.WriteLine("PointCloud2"); HackedReadFromPointCloudMessageHandle(subscription, message, messageHandle); } else @@ -1561,24 +1561,23 @@ public static void HackedPointCloud__ReadFromHandle(Subscription subscription, g var sw = System.Diagnostics.Stopwatch.StartNew(); lock(POINTCLOUD_DATA_LOCK) { - pointCloud2.Header.__ReadFromHandle(sensor_msgs.msg.PointCloud2.native_get_field_header_HANDLE(messageHandle)); - pointCloud2.Height = sensor_msgs.msg.PointCloud2.native_read_field_height(messageHandle); - pointCloud2.Width = sensor_msgs.msg.PointCloud2.native_read_field_width(messageHandle); - pointCloud2.PointStep = sensor_msgs.msg.PointCloud2.native_read_field_point_step(messageHandle); - pointCloud2.RowStep = sensor_msgs.msg.PointCloud2.native_read_field_row_step(messageHandle); - pointCloud2.IsBigendian = sensor_msgs.msg.PointCloud2.native_read_field_is_bigendian(messageHandle); - { - int size = sensor_msgs.msg.PointCloud2.native_getsize_field_data_message(messageHandle); - HackedPointCloudMemoryCopy(subscription, messageHandle, pointCloud2, size); - } - + pointCloud2.Header.__ReadFromHandle(sensor_msgs.msg.PointCloud2.native_get_field_header_HANDLE(messageHandle)); + pointCloud2.Height = sensor_msgs.msg.PointCloud2.native_read_field_height(messageHandle); + pointCloud2.Width = sensor_msgs.msg.PointCloud2.native_read_field_width(messageHandle); + pointCloud2.PointStep = sensor_msgs.msg.PointCloud2.native_read_field_point_step(messageHandle); + pointCloud2.RowStep = sensor_msgs.msg.PointCloud2.native_read_field_row_step(messageHandle); + pointCloud2.IsBigendian = sensor_msgs.msg.PointCloud2.native_read_field_is_bigendian(messageHandle); + { + int size = sensor_msgs.msg.PointCloud2.native_getsize_field_data_message(messageHandle); + HackedPointCloudMemoryCopy(subscription, messageHandle, pointCloud2, size); + } } } public static void HackedImage__ReadFromHandle(Subscription subscription, global::System.IntPtr messageHandle, sensor_msgs.msg.Image image) { - Debug.WriteLine("HackedImage__ReadFromHandle"); - var sw = System.Diagnostics.Stopwatch.StartNew(); + // Debug.WriteLine("HackedImage__ReadFromHandle"); + // var sw = System.Diagnostics.Stopwatch.StartNew(); lock(IMG_DATA_LOCK) { @@ -1611,8 +1610,8 @@ public static void HackedImage__ReadFromHandle(Subscription subscription, global } } - sw.Stop(); - Debug.WriteLine($"HackedImage__ReadFromHandle END: {sw.ElapsedMilliseconds} ms"); + // sw.Stop(); + // Debug.WriteLine($"HackedImage__ReadFromHandle END: {sw.ElapsedMilliseconds} ms"); } private static void HackedPointCloudMemoryCopy(Subscription subscription, IntPtr messageHandle, sensor_msgs.msg.PointCloud2 pointCloud2, int size__local_variable) @@ -1653,7 +1652,7 @@ private static void HackedMemoryCopy(Subscription subscription, IntPtr messageHa if (bytes == null || bytes.Length != size__local_variable) { - Debug.WriteLine("HackedImage__ReadFromHandle: new byte buffer created"); + // Debug.WriteLine("HackedImage__ReadFromHandle: new byte buffer created"); bytes = new byte[size__local_variable]; // Console.WriteLine($"makmustReleaseing new size for image subscription: {size__local_variable} subscription: {subscription.GetHashCode()}"); @@ -1703,7 +1702,7 @@ internal static void HackedReadFromImageMessageHandle(Subscription subscription, } catch(Exception e) { - Debug.WriteLine("HackedImage__ReadFromHandle: exception"); + // Debug.WriteLine("HackedImage__ReadFromHandle: exception"); } finally {