diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 30381660..4303d2e6 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 @@ -62,29 +64,28 @@ 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_guard_condition.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) @@ -120,6 +121,7 @@ if(BUILD_TESTING) ${RCLDOTNET_TEST_TARGET_FRAMEWORK} SOURCES ${CS_SOURCES} + test/test_guard_conditions.cs test/test_messages.cs test/test_services.cs INCLUDE_DLLS 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/GuardCondition.cs b/rcldotnet/GuardCondition.cs new file mode 100644 index 00000000..1c6e9aca --- /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, $"{nameof(GuardConditionDelegates.native_rcl_trigger_guard_condition)}() failed."); + } + + internal void TriggerCallback() + { + _callback(); + } + } +} diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 8a3c585b..f6177d4e 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"); @@ -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 +616,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 +662,84 @@ 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) + int subscriptionIndex = 0; + foreach (Subscription subscription in node.Subscriptions) { - subscription.TriggerCallback(message); + if (RCLdotnetDelegates.native_rcl_wait_set_subscription_ready(waitSetHandle, subscriptionIndex)) + { + 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()) - { - 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(); + int serviceIndex = 0; + foreach (var service in node.Services) + { + if (RCLdotnetDelegates.native_rcl_wait_set_service_ready(waitSetHandle, serviceIndex)) + { + 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); + } + } + + serviceIndex++; + } + + int clientIndex = 0; + foreach (var client in node.Clients) { - service.TriggerCallback(request, response); + if (RCLdotnetDelegates.native_rcl_wait_set_client_ready(waitSetHandle, clientIndex)) + { + 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); + } + } + + clientIndex++; } } - foreach (var client in node.Clients) + int guardConditionIndex = 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, guardConditionIndex)) { - var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); - client.HandleResponse(sequenceNumber, response); + guardCondition.TriggerCallback(); } + + guardConditionIndex++; } } } 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..cee09a14 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,50 @@ 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; + + 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..6869fffa 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,24 @@ 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_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); + 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(); + } + } + } +} diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index 64d64f89..a5518444 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -45,9 +45,16 @@ add_dotnet_executable(rcldotnet_example_client ${_assemblies_dep_dlls} ) +add_dotnet_executable(rcldotnet_guard_condition + RCLDotnetGuardCondition.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_guard_condition DESTINATION lib/${PROJECT_NAME}/dotnet) ament_package() diff --git a/rcldotnet_examples/RCLDotnetGuardCondition.cs b/rcldotnet_examples/RCLDotnetGuardCondition.cs new file mode 100644 index 00000000..04d2cc1a --- /dev/null +++ b/rcldotnet_examples/RCLDotnetGuardCondition.cs @@ -0,0 +1,50 @@ +using System; +using System.Threading; +using System.Threading.Tasks; + +using ROS2; + +namespace ConsoleApplication +{ + public static class RCLDotnetGuardCondition + { + static GuardCondition _guardCondition = null; + + public static void Main(string[] args) + { + RCLdotnet.Init(); + + var node = RCLdotnet.CreateNode("guard_condition_example"); + + _guardCondition = node.CreateGuardCondition(GuardConditionTriggered); + + Console.WriteLine($"RMWIdentifier: {RCLdotnet.GetRMWIdentifier()}"); + + _guardCondition.Trigger(); + Console.WriteLine($"{DateTimeOffset.Now:O} ThreadId: {Environment.CurrentManagedThreadId} - Initial trigger of GuardCondition."); + + RCLdotnet.Spin(node); + } + + static void GuardConditionTriggered() + { + Console.WriteLine($"{DateTimeOffset.Now:O} ThreadId: {Environment.CurrentManagedThreadId} - GuardCondition was triggered."); + _ = TriggerAfterSomeTime(); + } + + static async Task TriggerAfterSomeTime() + { + // This is a multiple of the 500ms used in the loop in + // RCLdotnet.Spin(). So the guard condition will be triggered nearly + // at the same time as the wait times out. This is to test/reproduce + // if the combination of rcldotnet and the rmw implementation have + // an race-conditions in that case. + await Task.Delay(TimeSpan.FromSeconds(1)); + + // This will be called in an background worker thread as console + // applications don't have an SynchronizationContext by default. + _guardCondition.Trigger(); + Console.WriteLine($"{DateTimeOffset.Now:O} ThreadId: {Environment.CurrentManagedThreadId} - Triggered GuardCondition."); + } + } +}