diff --git a/src/ros2cs/ros2cs_common/interfaces/IExtendedDisposable.cs b/src/ros2cs/ros2cs_common/interfaces/IExtendedDisposable.cs index ad4c0ff6..60440e57 100644 --- a/src/ros2cs/ros2cs_common/interfaces/IExtendedDisposable.cs +++ b/src/ros2cs/ros2cs_common/interfaces/IExtendedDisposable.cs @@ -20,6 +20,11 @@ namespace ROS2 /// Use instead of IDisposable public interface IExtendedDisposable : IDisposable { + /// If the object is in a disposed state. + /// + /// Being in a disposed state does not mean that an object has ben disposed successfully. + /// Call to assert that an object has been disposed successfully. + /// bool IsDisposed { get; } } diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index f6a2f929..f08ab886 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright 2019-2021 Robotec.ai +# Copyright 2019-2023 Robotec.ai # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -79,6 +79,9 @@ set(CS_INTERFACES interfaces/IService.cs interfaces/IPublisher.cs interfaces/ISubscription.cs + interfaces/IContext.cs + interfaces/IExecutor.cs + interfaces/IWaitable.cs ) set(CS_NATIVE @@ -91,6 +94,9 @@ set(CS_NATIVE set(CS_UTILS utils/Utils.cs + utils/LockedDictionary.cs + utils/LockedCollection.cs + utils/MappedValueDictionary.cs ) set(CS_SOURCES @@ -103,9 +109,12 @@ set(CS_SOURCES Node.cs Publisher.cs QualityOfServiceProfile.cs - Ros2cs.cs Subscription.cs WaitSet.cs + Context.cs + GuardCondition.cs + executors/ManualExecutor.cs + executors/TaskExecutor.cs properties/AssemblyInfo.cs ) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index fd39ab16..f2b64756 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,7 +13,6 @@ // limitations under the License. using System; -using System.Collections; using System.Collections.Generic; using System.Diagnostics; using System.Linq; @@ -23,393 +22,371 @@ namespace ROS2 { - /// Client with a topic and Types for Messages - /// Instances are created by - /// Message Type to be send - /// Message Type to be received - public class Client: IClient - where I : Message, new() - where O : Message, new() - { - /// - public string Topic { get { return topic; } } - - public rcl_client_t Handle { get { return serviceHandle; } } - - /// - public IReadOnlyDictionary> PendingRequests {get; private set;} - - /// - IReadOnlyDictionary IClientBase.PendingRequests {get { return (IReadOnlyDictionary)this.PendingRequests; }} - - private string topic; - - /// - public object Mutex { get { return mutex; } } - - private object mutex = new object(); - /// - /// Mapping from request id without Response to . + /// Client with a topic and types for messages wrapping a rcl client. /// /// - /// The is stored separately to allow - /// to work even if the source returns multiple tasks. + /// This is the implementation produced by , + /// use this method to create new instances. /// - private Dictionary, Task)> Requests; - - private Ros2csLogger logger = Ros2csLogger.GetInstance(); - - rcl_client_t serviceHandle; - - IntPtr serviceOptions = IntPtr.Zero; - - rcl_node_t nodeHandle; - - /// - public bool IsDisposed { get { return disposed; } } - private bool disposed = false; - - /// - /// Internal constructor for Client - /// - /// Use to construct new Instances - public Client(string pubTopic, Node node, QualityOfServiceProfile qos = null) - { - topic = pubTopic; - nodeHandle = node.nodeHandle; - - QualityOfServiceProfile qualityOfServiceProfile = qos; - if (qualityOfServiceProfile == null) - qualityOfServiceProfile = new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT); - - Requests = new Dictionary, Task)>(); - PendingRequests = new PendingTasksView(Requests); - - serviceOptions = NativeRclInterface.rclcs_client_create_options(qualityOfServiceProfile.handle); - - IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); - - serviceHandle = NativeRcl.rcl_get_zero_initialized_client(); - Utils.CheckReturnEnum(NativeRcl.rcl_client_init( - ref serviceHandle, - ref nodeHandle, - typeSupportHandle, - topic, - serviceOptions)); - } - - ~Client() - { - Dispose(); - } - - public void Dispose() - { - DestroyClient(); - } - - /// "Destructor" supporting disposable model - private void DestroyClient() + /// + /// + public sealed class Client : IClient, IRawClient + where I : Message, new() + where O : Message, new() { - lock (mutex) - { - if (!disposed) + /// + public string Topic { get; private set; } + + /// + /// This dictionary is thread safe. + /// + /// + public IReadOnlyDictionary> PendingRequests { get; private set; } + + /// + /// This dictionary is thread safe. + /// + /// + IReadOnlyDictionary IClientBase.PendingRequests { get { return this.UntypedPendingRequests; } } + + /// + /// Wrapper for . + /// + private readonly IReadOnlyDictionary UntypedPendingRequests; + + /// + public bool IsDisposed { - lock (Requests) - { - foreach (var source in Requests.Values) + get { - bool success = source.Item1.TrySetException(new ObjectDisposedException("client has been disposed")); - Debug.Assert(success); + bool ok = NativeRclInterface.rclcs_client_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; } - Requests.Clear(); - } - Utils.CheckReturnEnum(NativeRcl.rcl_client_fini(ref serviceHandle, ref nodeHandle)); - NativeRclInterface.rclcs_client_dispose_options(serviceOptions); - logger.LogInfo("Client destroyed"); - disposed = true; } - } - } - - /// - public bool IsServiceAvailable() - { - bool available = false; - Utils.CheckReturnEnum(NativeRcl.rcl_service_server_is_available( - ref nodeHandle, - ref serviceHandle, - ref available - )); - return available; - } - /// - public void TakeMessage() - { - MessageInternals msg = new O() as MessageInternals; - rcl_rmw_request_id_t request_header = default(rcl_rmw_request_id_t); - int ret; - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + /// Handle to the rcl client. + /// + public IntPtr Handle { get; private set; } = IntPtr.Zero; + + /// + /// Handle to the rcl client options. + /// + private IntPtr Options = IntPtr.Zero; + + /// + /// Node associated with this instance. + /// + private readonly Node Node; + + /// + /// Mapping from request id without Response to . + /// + /// + /// The is stored separately to allow + /// to work even if the source returns multiple tasks. + /// Furthermore, this object is used for locking. + /// + private readonly Dictionary, Task)> Requests = new Dictionary, Task)>(); + + /// + /// Create a new instance. + /// + /// + /// The caller is responsible for adding the instance to . + /// This action is not thread safe. + /// + /// Topic to subscribe to. + /// Node to associate with. + /// QOS setting for this subscription. + /// If was disposed. + internal Client(string topic, Node node, QualityOfServiceProfile qos = null) { - return; + this.Topic = topic; + this.Node = node; + + var lockedRequests = new LockedDictionary, Task)>(this.Requests); + this.PendingRequests = new MappedValueDictionary, Task), Task>( + lockedRequests, + tuple => tuple.Item2 + ); + this.UntypedPendingRequests = new MappedValueDictionary, Task), Task>( + lockedRequests, + tuple => tuple.Item2 + ); + + QualityOfServiceProfile qualityOfServiceProfile = qos ?? new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT); + + this.Options = NativeRclInterface.rclcs_client_create_options(qualityOfServiceProfile.handle); + + IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); + + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_client(); + int ret = NativeRcl.rcl_client_init( + this.Handle, + this.Node.Handle, + typeSupportHandle, + this.Topic, + this.Options + ); + + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } } - ret = NativeRcl.rcl_take_response( - ref serviceHandle, - ref request_header, - msg.Handle - ); - } - if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_CLIENT_TAKE_FAILED) - { - Utils.CheckReturnEnum(ret); - ProcessResponse(request_header.sequence_number, msg); - } - } - /// - /// Populates managed fields with native values and finishes the corresponding - /// - /// Message that will be populated and used as the task result - /// sequence number received when sending the Request - private void ProcessResponse(long sequence_number, MessageInternals msg) - { - bool exists = false; - (TaskCompletionSource, Task) source = default((TaskCompletionSource, Task)); - lock (Requests) - { - if (Requests.TryGetValue(sequence_number, out source)) + /// + /// This method is not thread safe. + /// + /// If the instance was disposed. + /// + public bool IsServiceAvailable() { - exists = true; - Requests.Remove(sequence_number); + bool available = false; + Utils.CheckReturnEnum(NativeRcl.rcl_service_server_is_available( + this.Node.Handle, + this.Handle, + out available + )); + GC.KeepAlive(this); + return available; } - } - if (exists) - { - msg.ReadNativeMessage(); - source.Item1.SetResult((O)msg); - } - else - { - Debug.Print("received unknown sequence number or got disposed"); - } - } - - /// - /// Send a Request to the Service - /// - /// Message to be send - /// sequence number of the Request - private long SendRequest(I msg) - { - long sequence_number = default(long); - MessageInternals msgInternals = msg as MessageInternals; - msgInternals.WriteNativeMessage(); - Utils.CheckReturnEnum( - NativeRcl.rcl_send_request( - ref serviceHandle, - msgInternals.Handle, - ref sequence_number - ) - ); - return sequence_number; - } - /// - /// Associate a task with a sequence number - /// - /// source used to controll the - /// sequence number received when sending the Request - /// The associated task. - private Task RegisterSource(TaskCompletionSource source, long sequence_number) - { - Task task = source.Task; - lock (Requests) - { - Requests.Add(sequence_number, (source, task)); - } - return task; - } - - /// - public O Call(I msg) - { - var task = CallAsync(msg); - task.Wait(); - return task.Result; - } - - /// - public Task CallAsync(I msg) - { - return CallAsync(msg, TaskCreationOptions.None); - } + /// + /// This method is thread safe. + /// + /// + public bool TryProcess() + { + rcl_rmw_request_id_t header = default(rcl_rmw_request_id_t); + O message = new O(); + (TaskCompletionSource, Task) source; + bool exists = false; - /// - public Task CallAsync(I msg, TaskCreationOptions options) - { - TaskCompletionSource source; - lock (mutex) - { - if (!Ros2cs.Ok() || disposed) - { - throw new InvalidOperationException("Cannot service as the class is already disposed or shutdown was called"); - } - // prevent TakeMessage from receiving Responses before we called RegisterSource - long sequence_number = SendRequest(msg); - source = new TaskCompletionSource(options); - return RegisterSource(source, sequence_number); - } - } + lock (this.Requests) + { + // prevent taking responses before RegisterSource was called + int ret = NativeRcl.rcl_take_response( + this.Handle, + ref header, + (message as MessageInternals).Handle + ); + GC.KeepAlive(this); + + switch ((RCLReturnEnum)ret) + { + case RCLReturnEnum.RCL_RET_CLIENT_TAKE_FAILED: + case RCLReturnEnum.RCL_RET_CLIENT_INVALID: + return false; + default: + Utils.CheckReturnEnum(ret); + break; + } + + if (this.Requests.TryGetValue(header.sequence_number, out source)) + { + exists = true; + this.Requests.Remove(header.sequence_number); + } + } + if (exists) + { + (message as MessageInternals).ReadNativeMessage(); + source.Item1.SetResult(message); + } + else + { + Debug.Print("received request which was not pending, maybe canceled"); + } + return true; + } - /// - public bool Cancel(Task task) - { - var pair = default(KeyValuePair, Task)>); - try - { - lock(this.Requests) + /// + /// The provided message can be modified or disposed after this call. + /// Furthermore, this method is thread safe. + /// + /// If the instance was disposed. + /// + public O Call(I msg) { - pair = this.Requests.First(entry => entry.Value.Item2 == task); - // has to be true - this.Requests.Remove(pair.Key); + var task = CallAsync(msg); + task.Wait(); + return task.Result; } - } - catch (InvalidOperationException) - { - return false; - } - pair.Value.Item1.SetCanceled(); - return true; - } - /// - /// Wrapper to avoid exposing to users. - /// - /// - /// The locking used is required because the user may access the view while is running. - /// - private class PendingTasksView : IReadOnlyDictionary>, IReadOnlyDictionary - { - public Task this[long key] - { - get + /// + /// This method is thread safe. + /// + /// If the instance was disposed. + /// + public Task CallAsync(I msg) { - lock (this.Requests) - { - return this.Requests[key].Item2; - } + return CallAsync(msg, TaskCreationOptions.None); } - } - Task IReadOnlyDictionary.this[long key] - { - get { return this[key]; } - } + /// + /// This method is thread safe. + /// + /// If the instance was disposed. + /// + public Task CallAsync(I msg, TaskCreationOptions options) + { + var source = new TaskCompletionSource(options); + lock (this.Requests) + { + // prevents TryProcess from receiving Responses before we called RegisterSource + long sequence_number = SendRequest(msg); + return RegisterSource(source, sequence_number); + } + } - public IEnumerable Keys - { - get + /// + /// Send a Request to the Service + /// + /// Message to be send + /// sequence number of the Request + private long SendRequest(I msg) { - lock (this.Requests) - { - return this.Requests.Keys.ToArray(); - } + long sequence_number = default(long); + MessageInternals msgInternals = msg as MessageInternals; + msgInternals.WriteNativeMessage(); + Utils.CheckReturnEnum( + NativeRcl.rcl_send_request( + this.Handle, + msgInternals.Handle, + out sequence_number + ) + ); + GC.KeepAlive(this); + return sequence_number; } - } - public IEnumerable> Values - { - get + /// + /// Associate a task with a sequence number + /// + /// source used to controll the + /// sequence number received when sending the Request + /// The associated task. + private Task RegisterSource(TaskCompletionSource source, long sequence_number) { - lock (this.Requests) - { - return this.Requests.Values.Select(value => value.Item2).ToArray(); - } + // handle Task not being a singleton + Task task = source.Task; + Requests.Add(sequence_number, (source, task)); + return task; } - } - IEnumerable IReadOnlyDictionary.Values - { - get { return this.Values; } - } + /// + /// Tasks are automatically removed on completion and have to be removed only when canceled. + /// Furthermore, this method is thread safe. + /// + /// + public bool Cancel(Task task) + { + var pair = default(KeyValuePair, Task)>); + lock (this.Requests) + { + try + { + pair = this.Requests.First(entry => entry.Value.Item2 == task); + } + catch (InvalidOperationException) + { + return false; + } + // has to be true + bool success = this.Requests.Remove(pair.Key); + Debug.Assert(success, "failed to remove matching request"); + } + pair.Value.Item1.SetCanceled(); + return true; + } - public int Count - { - get + /// + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously or while the client is in use. + /// Disposal is automatically performed on finalization by the GC. + /// Any pending tasks are removed and set to have faulted with + /// . + /// + /// + public void Dispose() { - lock (this.Requests) - { - return this.Requests.Count; - } + this.Dispose(true); + // finalizer not needed when we disposed successfully + GC.SuppressFinalize(this); } - } - private readonly IReadOnlyDictionary, Task)> Requests; + /// Disposal logic. + /// If this method is not called in a finalizer. + private void Dispose(bool disposing) + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + // only do if Node.CurrentClients and this.Requests have not been finalized + // save since if we are being finalized we are not in a wait set anymore + if (disposing) + { + bool success = this.Node.RemoveClient(this); + Debug.Assert(success, "failed to remove client"); + this.DisposeAllTasks(); + } - public PendingTasksView(IReadOnlyDictionary, Task)> requests) - { - this.Requests = requests; - } + Utils.CheckReturnEnum(NativeRcl.rcl_client_fini(this.Handle, this.Node.Handle)); + this.FreeHandles(); + } - public bool ContainsKey(long key) - { - lock (this.Requests) + /// + void IRawClient.DisposeFromNode() { - return this.Requests.ContainsKey(key); + if (this.Handle == IntPtr.Zero) + { + return; + } + + this.DisposeAllTasks(); + Utils.CheckReturnEnum(NativeRcl.rcl_client_fini(this.Handle, this.Node.Handle)); + this.FreeHandles(); } - } - public bool TryGetValue(long key, out Task value) - { - bool success = false; - (TaskCompletionSource, Task) source = default((TaskCompletionSource, Task)); - lock (this.Requests) + /// + /// Dispose all tasks currently pending. + /// + private void DisposeAllTasks() { - success = this.Requests.TryGetValue(key, out source); + lock (this.Requests) + { + foreach (var source in this.Requests.Values) + { + source.Item1.TrySetException(new ObjectDisposedException($"client for topic '{this.Topic}'")); + } + this.Requests.Clear(); + } } - value = source.Item2; - return success; - } - - bool IReadOnlyDictionary.TryGetValue(long key, out Task value) - { - bool success = this.TryGetValue(key, out var task); - value = task; - return success; - } - - public IEnumerator>> GetEnumerator() - { - lock (this.Requests) + + /// + /// Free the rcl handles and replace them with null pointers. + /// + /// + /// The handles are not finalised by this method. + /// + private void FreeHandles() { - return this.Requests - .Select(pair => new KeyValuePair>(pair.Key, pair.Value.Item2)) - .ToArray() - .AsEnumerable() - .GetEnumerator(); + NativeRclInterface.rclcs_free_client(this.Handle); + this.Handle = IntPtr.Zero; + NativeRclInterface.rclcs_client_dispose_options(this.Options); + this.Options = IntPtr.Zero; } - } - - IEnumerator IEnumerable.GetEnumerator() - { - return this.GetEnumerator(); - } - IEnumerator> IEnumerable>.GetEnumerator() - { - lock (this.Requests) + ~Client() { - return this.Requests - .Select(pair => new KeyValuePair(pair.Key, pair.Value.Item2)) - .ToArray() - .AsEnumerable() - .GetEnumerator(); + this.Dispose(false); } - } } - } } diff --git a/src/ros2cs/ros2cs_core/Clock.cs b/src/ros2cs/ros2cs_core/Clock.cs index 0afc5ddc..bc89aefc 100644 --- a/src/ros2cs/ros2cs_core/Clock.cs +++ b/src/ros2cs/ros2cs_core/Clock.cs @@ -48,7 +48,7 @@ public RosTime Now { RosTime time = new RosTime(); long queryNowNanoseconds = 0; - NativeRcl.rcl_clock_get_now(handle, ref queryNowNanoseconds); + NativeRcl.rcl_clock_get_now(handle, out queryNowNanoseconds); time.sec = (int)(queryNowNanoseconds / (long)1e9); time.nanosec = (uint)(queryNowNanoseconds - time.sec*((long)1e9)); return time; diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs new file mode 100644 index 00000000..07ba9a4f --- /dev/null +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -0,0 +1,282 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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; + +namespace ROS2 +{ + /// + /// ROS Context encapsulating the non-global state of an init/shutdown cycle. + /// + /// + /// If the instance is not disposed it will be shut down by the garbage collector. + /// Since the collection tracking the nodes might be finalized at this point + /// the handle will be leaked. + /// + public sealed class Context : IContext + { + /// + /// Will be disposed on disposal of this instance. + /// Furthermore, access to the collection is thread safe. + /// + public IReadOnlyDictionary Nodes { get; private set; } + + /// + public bool IsDisposed { get { return !this.Ok(); } } + + /// + public event Action OnShutdown; + + /// + /// Handle to the rcl_context_t + /// + internal IntPtr Handle { get; private set; } = IntPtr.Zero; + + /// + /// Collection nodes active in this context. + /// + /// + /// Also used for synchronisation when creating / removing nodes. + /// + private Dictionary ROSNodes = new Dictionary(); + + /// + /// Collection of guard conditions active in this context. + /// + /// + /// Also used for synchronisation when creating / removing guard conditions. + /// + private HashSet GuardConditions = new HashSet(); + + /// + /// Collection of wait sets active in this context; + /// + /// + /// Also used for synchronisation when creating / removing guard conditions. + /// + private HashSet WaitSets = new HashSet(); + + /// + /// Get the current RMW implementation. + /// + /// The current implementation as string. + public static string GetRMWImplementation() + { + return Utils.PtrToString(NativeRmwInterface.rmw_native_interface_get_implementation_identifier()); + } + + /// + /// Create a new ROS Context. + /// + public Context() + { + this.Nodes = new MappedValueDictionary( + new LockedDictionary(this.ROSNodes), + node => node + ); + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_context(); + int ret = NativeRclInterface.rclcs_init(this.Handle, NativeRcl.rcutils_get_default_allocator()); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } + } + + /// + /// + /// This method is thread safe. + /// + public bool Ok() + { + return NativeRclInterface.rclcs_context_is_valid(this.Handle); + } + + /// + /// + /// This method is thread safe. + /// + public bool TryCreateNode(string name, out INode node) + { + lock (this.ROSNodes) + { + if (this.ROSNodes.ContainsKey(name)) + { + node = default(INode); + return false; + } + else + { + Node ROSNode = new Node(name, this); + this.ROSNodes.Add(name, ROSNode); + node = ROSNode; + return true; + } + } + } + + /// + /// Remove a Node. + /// + /// + /// This method is intended to be used by and does not dispose the node. + /// Furthermore, it is thread safe. + /// + /// Name of the node. + /// If the node existed in this context and has been removed. + internal bool RemoveNode(string name) + { + lock (this.ROSNodes) + { + return this.ROSNodes.Remove(name); + } + } + + /// + /// Create a guard condition. + /// + /// + /// This method is thread safe. + /// + /// Callback executed by the executor when the guard condition is triggered. + /// A new guard condition instance. + internal GuardCondition CreateGuardCondition(Action callback) + { + lock (this.GuardConditions) + { + GuardCondition guardCondition = new GuardCondition(this, callback); + this.GuardConditions.Add(guardCondition); + return guardCondition; + } + } + + /// + /// Remove a guard condition. + /// + /// + /// This method is intended to be used by and does not dispose the guard condition. + /// Furthermore, it is thread safe. + /// + /// Guard condition to remove. + /// If the guard condition existed in this context and has been removed. + internal bool RemoveGuardCondition(GuardCondition guardCondition) + { + lock (this.GuardConditions) + { + return this.GuardConditions.Remove(guardCondition); + } + } + + /// + /// Create a wait set. + /// + /// + /// This method is thread safe. + /// + /// A new wait set instance. + internal WaitSet CreateWaitSet() + { + lock (this.WaitSets) + { + WaitSet waitSet = new WaitSet(this); + this.WaitSets.Add(waitSet); + return waitSet; + } + } + + /// + /// Remove a wait set. + /// + /// + /// This method is intended to be used by and does not dispose the wait set. + /// Furthermore, it is thread safe. + /// + /// Wait set to remove. + /// If the wait set existed in this context and has been removed. + internal bool RemoveWaitSet(WaitSet waitSet) + { + lock (this.WaitSets) + { + return this.WaitSets.Remove(waitSet); + } + } + + /// + /// This method is not thread safe. + /// Do not call while the context or any entities + /// associated with it are in use. + /// + /// + public void Dispose() + { + this.Dispose(true); + // finalizer not needed when we disposed successfully + GC.SuppressFinalize(this); + } + + /// Disposal logic. + /// If this method is not called in a finalizer. + private void Dispose(bool disposing) + { + if (this.Handle == IntPtr.Zero) + { + return; + } + int ret = NativeRcl.rcl_shutdown(this.Handle); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_ALREADY_SHUTDOWN) + { + Utils.CheckReturnEnum(ret); + } + // only continue if the collections of the active primitives have not been finalized + if (disposing) + { + this.OnShutdown?.Invoke(); + foreach (var node in this.ROSNodes.Values) + { + node.DisposeFromContext(); + } + this.ROSNodes.Clear(); + foreach (var guardCondition in this.GuardConditions) + { + guardCondition.DisposeFromContext(); + } + this.GuardConditions.Clear(); + foreach (var waitSet in this.WaitSets) + { + waitSet.DisposeFromContext(); + } + this.WaitSets.Clear(); + // only safe when all primitives are gone, not calling Dispose() will leak the Handle + Utils.CheckReturnEnum(NativeRcl.rcl_context_fini(this.Handle)); + this.FreeHandles(); + } + } + + /// + /// Free the handles of this instance. + /// + private void FreeHandles() + { + NativeRclInterface.rclcs_free_context(this.Handle); + // to allow .IsDisposed to work + this.Handle = IntPtr.Zero; + } + + ~Context() + { + this.Dispose(false); + } + } +} diff --git a/src/ros2cs/ros2cs_core/GuardCondition.cs b/src/ros2cs/ros2cs_core/GuardCondition.cs new file mode 100644 index 00000000..556b92f2 --- /dev/null +++ b/src/ros2cs/ros2cs_core/GuardCondition.cs @@ -0,0 +1,165 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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.Diagnostics; + +namespace ROS2 +{ + /// + /// Guard condition used to interrupt waits wrapping a rcl guard condition. + /// + internal sealed class GuardCondition : IWaitable, IExtendedDisposable { + + /// + /// Handle to the rcl guard condition. + /// + public IntPtr Handle { get; private set; } = IntPtr.Zero; + + /// + public bool IsDisposed + { + get + { + bool ok = NativeRclInterface.rclcs_guard_condition_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } + } + + /// + /// Context associated with this instance. + /// + private readonly Context Context; + + /// + /// Callback invoked when the guard condition + /// is processed. + /// + private readonly Action Callback; + + /// + /// Create a new instance. + /// + /// Context to associate with. + /// Callback to invoke when processed. + /// If is disposed. + internal GuardCondition(Context context, Action callback) + { + this.Context = context; + this.Callback = callback; + int ret = NativeRclInterface.rclcs_get_guard_condition( + context.Handle, + out IntPtr handle + ); + if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_INVALID_ARGUMENT) + { + throw new ObjectDisposedException("rcl context"); + } + Utils.CheckReturnEnum(ret); + this.Handle = handle; + } + + /// + /// Trigger the guard condition to make it become ready. + /// + /// + /// It seems that the guard condition stays ready until waited on. + /// This method is thread safe. + /// + /// If the guard condition was disposed. + public void Trigger() + { + int ret = NativeRcl.rcl_trigger_guard_condition(this.Handle); + GC.KeepAlive(this); + + if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_INVALID_ARGUMENT) + { + throw new ObjectDisposedException("rcl guard condition"); + } + Utils.CheckReturnEnum(ret); + } + + /// + /// This method is thread safe + /// is the callback is thread safe. + /// + /// + public bool TryProcess() + { + this.Callback(); + return true; + } + + /// + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously or while the guard condition is in use. + /// Disposal is automatically performed on finalization by the GC. + /// + /// + public void Dispose() + { + this.Dispose(true); + GC.SuppressFinalize(this); + } + + /// Disposal logic. + /// If this method is not called in a finalizer + private void Dispose(bool disposing) + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + // only do if Context.GuardConditions has not been finalized + if (disposing) + { + bool success = this.Context.RemoveGuardCondition(this); + Debug.Assert(success, message: "failed to remove guard condition"); + } + + this.DisposeFromContext(); + } + + /// Dispose without modifying the context. + internal void DisposeFromContext() + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + Utils.CheckReturnEnum(NativeRcl.rcl_guard_condition_fini(this.Handle)); + this.FreeHandles(); + } + + /// + /// Free the rcl handles and replace them with null pointers. + /// + /// + /// The handles are not finalised by this method. + /// + private void FreeHandles() + { + NativeRclInterface.rclcs_free_guard_condition(this.Handle); + this.Handle = IntPtr.Zero; + } + + ~GuardCondition() + { + this.Dispose(false); + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 6911c2d1..40bbfca4 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,271 +13,373 @@ // limitations under the License. using System; -using System.Linq; using System.Collections.Generic; +using System.Diagnostics; namespace ROS2 { - /// Represents a managed ros2 (rcl) node - /// - public class Node: INode - { - public string Name { get { return name; } } - private string name; - private Ros2csLogger logger = Ros2csLogger.GetInstance(); - - internal List Subscriptions + /// + /// Node wrapping a rcl node. + /// + /// + /// This is the implementation produced by , + /// use this method to create instances. + /// + /// + /// + public sealed class Node : INode { - get - { - lock (mutex) - { - return subscriptions.ToList(); - } - } - } + /// + public string Name { get; private set; } - internal List Clients - { - get - { - lock (mutex) + /// + public IContext Context { get { return this.ROSContext; } } + + /// + /// Users have to guarantee that a node is associated with at most one executor at any given time + /// to prevent undefined behaviour when multithreading is used. + /// It is recommended to not set this property directly and leave this task to the executor. + /// Setting this property is thread safe. + /// + /// + public IExecutor Executor { - return clients.ToList(); + get { return this._Executor; } + set + { + // prevent a executor switch while + // a primitive is being removed + lock (this.Lock) + { + this._Executor = value; + } + } } - } - } - internal List Services - { - get - { - lock (mutex) + private IExecutor _Executor = null; + + /// + public bool IsDisposed { - return services.ToList(); + get { return !NativeRclInterface.rclcs_node_is_valid(this.Handle); } } - } - } - internal rcl_node_t nodeHandle; - private IntPtr defaultNodeOptions; - private HashSet subscriptions; - private HashSet publishers; - private HashSet clients; - private HashSet services; - private readonly object mutex = new object(); - private bool disposed = false; - - public bool IsDisposed { get { return disposed; } } - - /// Node constructor - /// Nodes are created through CreateNode method of Ros2cs class - /// unique, non-namespaced node name - /// (rcl) context for the node. Global context is passed to this method - internal Node(string nodeName, ref rcl_context_t context) - { - name = nodeName; - string nodeNamespace = "/"; - subscriptions = new HashSet(); - publishers = new HashSet(); - clients = new HashSet(); - services = new HashSet(); - - nodeHandle = NativeRcl.rcl_get_zero_initialized_node(); - defaultNodeOptions = NativeRclInterface.rclcs_node_create_default_options(); - Utils.CheckReturnEnum(NativeRcl.rcl_node_init(ref nodeHandle, nodeName, nodeNamespace, ref context, defaultNodeOptions)); - logger.LogInfo("Node initialized"); - } + /// + /// Handle to the rcl node + /// + internal IntPtr Handle { get; private set; } = IntPtr.Zero; - /// Finalizer supporting IDisposable model - ~Node() - { - DestroyNode(); - } + /// + /// Handle to the rcl node options + /// + private IntPtr Options = IntPtr.Zero; - /// Release managed and native resources. IDisposable implementation - public void Dispose() - { - DestroyNode(); - } + /// + /// Context associated with this instance. + /// + private readonly Context ROSContext; - /// "Destructor" supporting IDisposable model - /// Disposes all subscriptions and publishers and clients before finilizing node - internal void DestroyNode() - { - lock (mutex) - { - if (!disposed) + /// + /// Lock used to allow thread safe access to node primitives. + /// + private readonly object Lock = new object(); + + /// + /// This collection is thread safe. + /// + /// + public IReadOnlyCollection Publishers { get; private set; } + + private readonly HashSet CurrentPublishers = new HashSet(); + + /// + /// This collection is thread safe. + /// + /// + public IReadOnlyCollection Subscriptions { get; private set; } + + private readonly HashSet CurrentSubscriptions = new HashSet(); + + /// + /// This collection is thread safe. + /// + /// + public IReadOnlyCollection Services { get; private set; } + + private readonly HashSet CurrentServices = new HashSet(); + + /// + /// This collection is thread safe. + /// + /// + public IReadOnlyCollection Clients { get; private set; } + + private readonly HashSet CurrentClients = new HashSet(); + + /// + /// Create a new instance. + /// + /// + /// The caller is responsible for adding the instance to . + /// This action is not thread safe. + /// + /// Name of the node. + /// Context to associate with. + /// If is disposed. + internal Node(string name, Context context) { - foreach(ISubscriptionBase subscription in subscriptions) - { - subscription.Dispose(); - } - subscriptions.Clear(); - - foreach(IPublisherBase publisher in publishers) - { - publisher.Dispose(); - } - publishers.Clear(); - - foreach(IClientBase client in clients) - { - client.Dispose(); - } - clients.Clear(); - - foreach(IServiceBase service in services) - { - service.Dispose(); - } - services.Clear(); - - Utils.CheckReturnEnum(NativeRcl.rcl_node_fini(ref nodeHandle)); - NativeRclInterface.rclcs_node_dispose_options(defaultNodeOptions); - disposed = true; - logger.LogInfo("Node " + name + " destroyed"); + this.Name = name; + this.ROSContext = context; + this.Publishers = new LockedCollection(this.CurrentPublishers, this.Lock); + this.Subscriptions = new LockedCollection(this.CurrentSubscriptions, this.Lock); + this.Services = new LockedCollection(this.CurrentServices, this.Lock); + this.Clients = new LockedCollection(this.CurrentClients, this.Lock); + + this.Options = NativeRclInterface.rclcs_node_create_default_options(); + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_node(); + int ret = NativeRcl.rcl_node_init( + this.Handle, + this.Name, + "/", + this.ROSContext.Handle, + this.Options + + ); + switch ((RCLReturnEnum)ret) + { + case RCLReturnEnum.RCL_RET_OK: + break; + // does not return RCL_RET_NOT_INIT if the context is NULL + case RCLReturnEnum.RCL_RET_INVALID_ARGUMENT: + this.FreeHandles(); + throw new ObjectDisposedException("RCL Context"); + default: + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + break; + } } - } - } - /// Create a client for this node for a given topic, qos and message type - /// - public Client CreateClient(string topic, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() - { - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + /// This method is thread safe. + /// + /// If the instance was disposed. + /// + /// + public IPublisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new() { - logger.LogWarning("Cannot create client as the class is already disposed or shutdown was called"); - return null; + lock (this.Lock) + { + Publisher publisher = new Publisher(topic, this, qos); + bool success = this.CurrentPublishers.Add(publisher); + Debug.Assert(success, "publisher already exists"); + return publisher; + } } - Client client = new Client(topic, this, qos); - clients.Add(client); - logger.LogInfo("Created Client for topic " + topic); - return client; - } - } - /// Remove a client - /// - public bool RemoveClient(IClientBase client) - { - lock (mutex) - { - if (clients.Contains(client)) + /// + /// Remove a publisher. + /// + /// + /// This method is intended to be used by and does not dispose the publisher. + /// Furthermore, it is thread safe. + /// + /// Publisher to be removed. + /// If the publisher existed on this node and has been removed. + internal bool RemovePublisher(IRawPublisher publisher) { - logger.LogInfo("Removing client for topic " + client.Topic); - client.Dispose(); - return clients.Remove(client); + lock (this.Lock) + { + return this.CurrentPublishers.Remove(publisher); + } } - return false; - } - } - /// Create a service for this node for a given topic, callback, qos and message type - /// - public Service CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() - { - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + /// This method schedules a rescan on the current executor and is thread safe + /// if of the current executor is thread safe. + /// + /// If the instance was disposed. + /// + /// + public ISubscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new() { - logger.LogWarning("Cannot create service as the class is already disposed or shutdown was called"); - return null; + Subscription subscription; + lock (this.Lock) + { + subscription = new Subscription(topic, this, callback, qos); + bool success = this.CurrentSubscriptions.Add(subscription); + Debug.Assert(success, "subscription already exists"); + } + this.Executor?.TryScheduleRescan(this); + return subscription; } - Service service = new Service(topic, this, callback, qos); - services.Add(service); - logger.LogInfo("Created service for topic " + topic); - return service; - } - } + /// + internal bool RemoveSubscription(IRawSubscription primitive) + { + return this.RemovePrimitive(primitive, this.CurrentSubscriptions); + } - /// Remove a service - /// - public bool RemoveService(IServiceBase service) - { - lock (mutex) - { - if (services.Contains(service)) + /// + /// This method schedules a rescan on the current executor and is thread safe + /// if of the current executor is thread safe. + /// + /// If the instance was disposed. + /// + /// + public IClient CreateClient(string topic, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() { - logger.LogInfo("Removing service for topic " + service.Topic); - service.Dispose(); - return services.Remove(service); + Client client; + lock (this.Lock) + { + client = new Client(topic, this, qos); + bool success = this.CurrentClients.Add(client); + Debug.Assert(success, "client already exists"); + } + this.Executor?.TryScheduleRescan(this); + return client; } - return false; - } - } - /// Create a publisher for this node for a given topic, qos and message type - /// - public Publisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new() - { - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + internal bool RemoveClient(IRawClient primitive) { - logger.LogWarning("Cannot create publisher as the class is already disposed or shutdown was called"); - return null; + return this.RemovePrimitive(primitive, this.CurrentClients); } - Publisher publisher = new Publisher(topic, this, qos); - publishers.Add(publisher); - logger.LogInfo("Created Publisher for topic " + topic); - return publisher; - } - } + /// + /// This method schedules a rescan on the current executor and is thread safe + /// if of the current executor is thread safe. + /// + /// If the instance was disposed. + /// + /// + public IService CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() + { + Service service; + lock (this.Lock) + { + service = new Service(topic, this, callback, qos); + bool success = this.CurrentServices.Add(service); + Debug.Assert(success, "service already exists"); + } + this.Executor?.TryScheduleRescan(this); + return service; + } - /// Create a subscription for this node for a given topic, callback, qos and message type - /// - public Subscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new() - { - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + internal bool RemoveService(IRawService primitive) { - logger.LogWarning("Cannot create subscription as the class is already disposed or shutdown was called"); - return null; + return this.RemovePrimitive(primitive, this.CurrentServices); } - Subscription subscription = new Subscription(topic, this, callback, qos); - subscriptions.Add(subscription); - logger.LogInfo("Created subscription for topic " + topic); - return subscription; - } - } + /// + /// Remove a primitive and wait for it to be disposable. + /// + /// + /// This method is intended to be used by + /// of the primitive and does not dispose it. + /// Furthermore, it is thread safe if and + /// of the current executor are thread safe. + /// + /// Type of the primitive. + /// Primitive to remove. + /// Collection of the current primitives. + /// Whether the primitive existed. + private bool RemovePrimitive(T primitive, ICollection collection) + { + bool removed; + IExecutor currentExecutor; + lock (this.Lock) + { + removed = collection.Remove(primitive); + // use the executor in use when removing the primitive, + // new executors will only see the updated collection + currentExecutor = this.Executor; + } + if (removed && !(currentExecutor is null)) + { + currentExecutor.TryScheduleRescan(this); + currentExecutor.Wait(); + } + return removed; + } - /// Remove a publisher - /// - public bool RemovePublisher(IPublisherBase publisher) - { - lock (mutex) - { - if (publishers.Contains(publisher)) + /// + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously or while the node or any of its primitives are in use. + /// Furthermore, it is NOT performed on finalization by the GC. + /// + /// + public void Dispose() { - logger.LogInfo("Removing publisher for topic " + publisher.Topic); - publisher.Dispose(); - return publishers.Remove(publisher); + if (this.Handle == IntPtr.Zero) + { + return; + } + + bool success = this.ROSContext.RemoveNode(this.Name); + Debug.Assert(success, "failed to remove node"); + + // no finalizer since the hash sets may have been finalized + this.DisposeFromContext(); } - return false; - } - } - /// Remove a subscription - /// - public bool RemoveSubscription(ISubscriptionBase subscription) - { - lock (mutex) - { - if (subscriptions.Contains(subscription)) + /// + /// Dispose this node without modifying the context. + /// + internal void DisposeFromContext() + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + if (!(this.Executor is null)) + { + bool success = this.Executor.Remove(this); + Debug.Assert(success, "node was not added to its old executor"); + } + + foreach (IRawPublisher publisher in this.CurrentPublishers) + { + publisher.DisposeFromNode(); + } + this.CurrentPublishers.Clear(); + + foreach (IRawSubscription subscription in this.CurrentSubscriptions) + { + subscription.DisposeFromNode(); + } + this.CurrentSubscriptions.Clear(); + + foreach (IRawService service in this.CurrentServices) + { + service.DisposeFromNode(); + } + this.CurrentServices.Clear(); + + foreach (IRawClient client in this.CurrentClients) + { + client.DisposeFromNode(); + } + this.CurrentClients.Clear(); + + Utils.CheckReturnEnum(NativeRcl.rcl_node_fini(this.Handle)); + this.FreeHandles(); + } + + /// + /// Free the rcl handles and replace them with null pointers. + /// + /// + /// The handles are not finalised by this method. + /// + private void FreeHandles() { - logger.LogInfo("Removing subscription for topic " + subscription.Topic); - subscription.Dispose(); - return subscriptions.Remove(subscription); + NativeRclInterface.rclcs_free_node(this.Handle); + this.Handle = IntPtr.Zero; + NativeRclInterface.rclcs_node_dispose_options(this.Options); + this.Options = IntPtr.Zero; } - return false; - } } - } } diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index cfe9cb68..6b834c61 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,79 +18,163 @@ namespace ROS2 { - /// Publisher of a topic with a given type - /// Publishers are created through INode.CreatePublisher - public class Publisher: IPublisher where T : Message, new () - { - public string Topic { get { return topic; } } - private string topic; - - private Ros2csLogger logger = Ros2csLogger.GetInstance(); - rcl_publisher_t publisherHandle; - IntPtr publisherOptions = IntPtr.Zero; - rcl_node_t nodeHandle; - private bool disposed = false; - - public bool IsDisposed { get { return disposed; } } - - /// Internal constructor for Publsher. Use INode.CreatePublisher to construct - /// - public Publisher(string pubTopic, Node node, QualityOfServiceProfile qos = null) + /// + /// Publisher of a topic with a given type wrapping a rcl publisher. + /// + /// + /// This is the implementation produced by , + /// use this method to create new instances. + /// + /// + /// + public sealed class Publisher : IPublisher, IRawPublisher where T : Message, new() { - topic = pubTopic; - nodeHandle = node.nodeHandle; + /// + public string Topic { get; private set; } - QualityOfServiceProfile qualityOfServiceProfile = qos; - if (qualityOfServiceProfile == null) - qualityOfServiceProfile = new QualityOfServiceProfile(); + /// + public bool IsDisposed + { + get + { + bool ok = NativeRclInterface.rclcs_publisher_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } + } - publisherOptions = NativeRclInterface.rclcs_publisher_create_options(qualityOfServiceProfile.handle); + /// + /// Handle to the rcl publisher + /// + private IntPtr Handle = IntPtr.Zero; - IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); + /// + /// Handle to the rcl publisher options + /// + private IntPtr Options = IntPtr.Zero; - publisherHandle = NativeRcl.rcl_get_zero_initialized_publisher(); - Utils.CheckReturnEnum(NativeRcl.rcl_publisher_init( - ref publisherHandle, - ref nodeHandle, - typeSupportHandle, - topic, - publisherOptions)); - } + /// + /// Node associated with this instance. + /// + private readonly Node Node; - ~Publisher() - { - Dispose(); - } + /// + /// Create a new instance. + /// + /// + /// The caller is responsible for adding the instance to . + /// This action is not thread safe. + /// + /// Topic to publish to. + /// Node to associate with. + /// QOS setting for this publisher. + /// If was disposed. + internal Publisher(string topic, Node node, QualityOfServiceProfile qos = null) + { + this.Topic = topic; + this.Node = node; - public void Dispose() - { - DestroyPublisher(); - } + QualityOfServiceProfile qualityOfServiceProfile = qos ?? new QualityOfServiceProfile(); - /// "Destructor" supporting disposable model - private void DestroyPublisher() - { - if (!disposed) - { - Utils.CheckReturnEnum(NativeRcl.rcl_publisher_fini(ref publisherHandle, ref nodeHandle)); - NativeRclInterface.rclcs_publisher_dispose_options(publisherOptions); - logger.LogInfo("Publisher destroyed"); - disposed = true; - } - } + this.Options = NativeRclInterface.rclcs_publisher_create_options(qualityOfServiceProfile.handle); - /// Publish a message - /// - public void Publish(T msg) - { - if (!Ros2cs.Ok() || disposed) - { - logger.LogWarning("Cannot publish as the class is already disposed or shutdown was called"); - return; - } - MessageInternals msgInternals = msg as MessageInternals; - msgInternals.WriteNativeMessage(); - Utils.CheckReturnEnum(NativeRcl.rcl_publish(ref publisherHandle, msgInternals.Handle, IntPtr.Zero)); + IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); + + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_publisher(); + int ret = NativeRcl.rcl_publisher_init( + this.Handle, + this.Node.Handle, + typeSupportHandle, + this.Topic, + this.Options + ); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } + } + + + /// + /// Message memory is copied into native structures and + /// the message can be safely changed or disposed after this call. + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously. + /// + /// If the instance was disposed. + /// + public void Publish(T msg) + { + MessageInternals msgInternals = msg as MessageInternals; + // may not be thread safe + msgInternals.WriteNativeMessage(); + // confused by the rcl documentation, assume it is not thread safe + Utils.CheckReturnEnum(NativeRcl.rcl_publish(this.Handle, msgInternals.Handle, IntPtr.Zero)); + GC.KeepAlive(this); + } + + /// + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously or while the publisher is in use. + /// Disposal is automatically performed on finalization by the GC. + /// + /// + public void Dispose() + { + this.Dispose(true); + // finalizer not needed when we disposed successfully + GC.SuppressFinalize(this); + } + + /// Disposal logic. + /// If this method is not called in a finalizer. + private void Dispose(bool disposing) + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + // only do if Node.CurrentPublishers has not been finalized + if (disposing) + { + bool success = this.Node.RemovePublisher(this); + Debug.Assert(success, "failed to remove publisher"); + } + + (this as IRawPublisher).DisposeFromNode(); + } + + /// + void IRawPublisher.DisposeFromNode() + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + Utils.CheckReturnEnum(NativeRcl.rcl_publisher_fini(this.Handle, this.Node.Handle)); + this.FreeHandles(); + } + + /// + /// Free the rcl handles and replace them with null pointers. + /// + /// + /// The handles are not finalised by this method. + /// + private void FreeHandles() + { + NativeRclInterface.rclcs_free_publisher(this.Handle); + this.Handle = IntPtr.Zero; + NativeRclInterface.rclcs_publisher_dispose_options(this.Options); + this.Options = IntPtr.Zero; + } + + ~Publisher() + { + this.Dispose(false); + } } - } } diff --git a/src/ros2cs/ros2cs_core/Ros2cs.cs b/src/ros2cs/ros2cs_core/Ros2cs.cs deleted file mode 100644 index b7303178..00000000 --- a/src/ros2cs/ros2cs_core/Ros2cs.cs +++ /dev/null @@ -1,277 +0,0 @@ -// Copyright 2019-2021 Robotec.ai -// -// 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.Linq; -using System.Diagnostics; -using System.Collections.Generic; -using System.Threading; - -namespace ROS2 -{ - /// Primary ros2 C# static class - /// This class interfaces with rcl library to handle initalization, shutdown, - /// creation and removal of nodes as well as spinning (no executors are implemented). - /// Note that the interface is through rcl and not rclcpp, the primary reason is that marshalling - /// into generic interface api is not feasible, especially when we don't know all possible instantiations - /// (as it is the case with custom generated messages). - /// - public static class Ros2cs - { - private static readonly Destructor destructor = new Destructor(); - private static readonly object mutex = new object(); - private static bool initialized = false; // for most part equivalent to rcl::ok() - private static rcl_context_t global_context; // a simplification, we only use global default context - private static rcl_allocator_t default_allocator; - private static List nodes = new List(); // kept to shutdown everything in order - - private static WaitSet WaitSet; - - /// Globally initialize ros2 (rcl) - /// Note that only a single context is used. - /// If needed, support for multiple contexts can be added - /// in a rather straightforward way throughout api. - public static void Init() - { - lock (mutex) - { - if (initialized) - { - return; - } - - default_allocator = NativeRcl.rcutils_get_default_allocator(); - global_context = NativeRcl.rcl_get_zero_initialized_context(); - Utils.CheckReturnEnum(NativeRclInterface.rclcs_init(ref global_context, default_allocator)); - WaitSet = new WaitSet(ref global_context); - initialized = true; - } - } - - public static string GetRMWImplementation() - { - return Utils.PtrToString(NativeRmwInterface.rmw_native_interface_get_implementation_identifier()); - } - - /// Globally shutdown ros2 (rcl) - /// Can be called multiple times with no effects after the first one. - /// Shutdowns ros2 and disposes all the nodes. Ok() function will return false after Shutdown is called. - /// - public static void Shutdown() - { - lock (mutex) - { - if (!initialized) - { - return; - } - initialized = false; - - Ros2csLogger.GetInstance().LogInfo("Ros2cs shutdown"); - Utils.CheckReturnEnum(NativeRcl.rcl_shutdown(ref global_context)); - - foreach (var node in nodes) - { - node.Dispose(); - } - nodes.Clear(); - } - } - - /// Whether ros2 C# is initialized - /// - /// Only when this function returns true a node can be created and spinning works - /// - public static bool Ok() - { - return initialized && NativeRcl.rcl_context_is_valid(ref global_context); - } - - /// Helper class to handle Ros2cs finalization - /// Could be understood as Ros2cs destructor. Can be called from GC if Shutdown - /// was not called explicitly. Also, handles context finalization. - private sealed class Destructor - { - ~Destructor() - { - Ros2csLogger.GetInstance().LogInfo("Ros2cs destructor called"); - Ros2cs.Shutdown(); - NativeRcl.rcl_context_fini(ref global_context); - } - } - - /// Create a ros2 (rcl) node - /// Creates a node in the global context and adds it to an internal collection. - /// Checks for name uniqueness. Throws if name is not unique or Ok() is not true. - /// Note that node options are not exposed. Default node options are used. - /// This can be extended by exposing desired configurations and adding a library call to set - /// them in the native code. - /// A valid node name, which will be first checked for uniqueness, - /// then validated inside rcl according to naming rules (will throw exception if invalid). - /// INode interface, which can be used to create subs and pubs - public static INode CreateNode(string nodeName) - { - lock (mutex) - { - if (!Ok()) - { - Ros2csLogger.GetInstance().LogError("Ros2cs is not initialized, cannot create node"); - throw new NotInitializedException(); - } - - foreach (var node in nodes) - { - if (node.Name == nodeName) - { - throw new InvalidOperationException("Node with name " + nodeName + " already exists, cannot create"); - } - } - - var new_node = new Node(nodeName, ref global_context); - nodes.Add(new_node); - return new_node; - } - } - - /// Remove and dispose ros2 (rcl) node - /// You can call Shutdown to dispose all the nodes, this is only needed when - /// node needs to be removed while others are still meant to be running - /// a node to remove as returned by previous CreateNode call - /// Whether the node was in the internal collection, which should always be true - /// unless this is called more than once for a node (which is ok). Return value can be - /// safely ignored - public static bool RemoveNode(INode node) - { - lock (mutex) - { - if (!initialized) - { - return false; // removal is handled with shutdown already - } - node.Dispose(); - return nodes.Remove(node); - } - } - - /// Spin on a single node - /// Spin should be called in a dedicate spinning thread in your - /// application layer since it runs in a blocking infinite loop. Will return when some work is - /// executed (a callback for each subscription that received a message) or after a timeout. - /// Note that you don't need to spin if you are only publishing (like in ros2) - /// Only subscriptions are executed currently, no timers or other executables - /// A node to spin on - /// Maximum time to wait for execution item (e. g. subscription) - public static void Spin(INode node, double timeoutSec = 0.1) - { - var nodes = new List{ node }; - Spin(nodes, timeoutSec); - } - - /// Spin overload for multiple nodes - /// This overload saves on implicit List creation - /// - public static void Spin(List nodes, double timeoutSec = 0.1) - { - while (initialized) - { - if (!SpinOnce(nodes, timeoutSec)) - { - Thread.Sleep(TimeSpan.FromSeconds(timeoutSec)); - } - } - } - - /// Spin only once - /// This overload is meant for when the while loop is better to - /// handle in the application layer - /// Whether the spin was successful (wait set not empty or Ros2cs not initialized) - /// - public static bool SpinOnce(INode node, double timeoutSec = 0.1) - { - var nodes = new List{ node }; - return SpinOnce(nodes, timeoutSec); - } - - private static bool warned_once = false; - - /// SpinOnce overload for multiple nodes - /// This overload saves on implicit List creation - /// Whether the spin was successful (wait set not empty or Ros2cs not initialized) - /// - public static bool SpinOnce(List nodes, double timeoutSec = 0.1) - { - lock (mutex) - { // Figure out how to minimize this lock - if (!initialized) - { - return false; - } - - // TODO - This can be optimized so that we cache the list and invalidate only with changes - var allSubscriptions = new List(); - var allClients = new List(); - var allServices = new List(); - foreach (INode node_interface in nodes) - { - Node node = node_interface as Node; - if (node == null) - continue; //Rare situation in which we are disposing - - allSubscriptions.AddRange(node.Subscriptions.Where(s => s != null)); - allClients.AddRange(node.Clients.Where(c => c != null)); - allServices.AddRange(node.Services.Where(s => s != null)); - } - - // TODO - investigate performance impact - WaitSet.Resize( - (ulong)allSubscriptions.Count, - (ulong)allClients.Count, - (ulong)allServices.Count - ); - foreach(var subscription in allSubscriptions) - { - AddResult result = WaitSet.TryAddSubscription(subscription, out ulong _); - Debug.Assert(result != AddResult.FULL, "no space for subscription in WaitSet"); - } - foreach(var client in allClients) - { - AddResult result = WaitSet.TryAddClient(client, out ulong _); - Debug.Assert(result != AddResult.FULL, "no space for client in WaitSet"); - } - foreach(var service in allServices) - { - AddResult result = WaitSet.TryAddService(service, out ulong _); - Debug.Assert(result != AddResult.FULL, "no space for Service in WaitSet"); - } - bool success; - try - { - success = WaitSet.Wait(TimeSpan.FromSeconds(timeoutSec)); - } - catch (WaitSetEmptyException) - { - return false; - } - if (success) - { - // Sequential processing - allSubscriptions.ForEach(subscription => subscription.TakeMessage()); - allClients.ForEach(client => client.TakeMessage()); - allServices.ForEach(service => service.TakeMessage()); - } - return true; - } - } - } -} diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 73b84a6d..1783f0b6 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,150 +13,219 @@ // limitations under the License. using System; +using System.Diagnostics; using ROS2.Internal; namespace ROS2 { - /// Service with a topic and Types for Messages - /// Instances are created by - /// Message Type to be received - /// Message Type to be send - public class Service: IService - where I : Message, new () - where O : Message, new () - { - public rcl_service_t Handle { get { return serviceHandle; } } - private rcl_service_t serviceHandle; - - /// - /// Topic of this Service - /// - public string Topic { get { return topic; } } - private string topic; - - /// - public bool IsDisposed { get { return disposed; } } - private bool disposed = false; - - /// - private rcl_node_t nodeHandle; - /// - /// Callback to be called to process incoming requests + /// Service with a topic and types for Messages wrapping a rcl service. /// - private readonly Func callback; - private IntPtr serviceOptions; + /// + /// This is the implementation produced by , + /// use this method to create new instances. + /// + /// + /// + public sealed class Service : IService, IRawService + where I : Message, new() + where O : Message, new() + { + /// + public string Topic { get; private set; } - /// - public object Mutex { get { return mutex; } } - private object mutex = new object(); + /// + public bool IsDisposed + { + get + { + bool ok = NativeRclInterface.rclcs_service_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } + } - /// - /// Internal constructor for Service - /// - /// Use to construct new Instances - internal Service(string subTopic, Node node, Func cb, QualityOfServiceProfile qos = null) - { - callback = cb; - nodeHandle = node.nodeHandle; - topic = subTopic; - serviceHandle = NativeRcl.rcl_get_zero_initialized_service(); - - QualityOfServiceProfile qualityOfServiceProfile = qos; - if (qualityOfServiceProfile == null) - { - qualityOfServiceProfile = new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT); - } - - serviceOptions = NativeRclInterface.rclcs_service_create_options(qualityOfServiceProfile.handle); - - I msg = new I(); - MessageInternals msgInternals = msg as MessageInternals; - IntPtr typeSupportHandle = msgInternals.TypeSupportHandle; - msg.Dispose(); - - Utils.CheckReturnEnum(NativeRcl.rcl_service_init( - ref serviceHandle, - ref node.nodeHandle, - typeSupportHandle, - topic, - serviceOptions)); - } + /// + /// Handle to the rcl service + /// + public IntPtr Handle { get; private set; } = IntPtr.Zero; + + /// + /// Handle to the rcl service options + /// + private IntPtr Options = IntPtr.Zero; + + /// + /// Node associated with this instance. + /// + private readonly Node Node; + + /// + /// Callback to be called to process incoming requests. + /// + private readonly Func Callback; + + /// + /// Create a new instance. + /// + /// + /// The caller is responsible for adding the instance to . + /// This action is not thread safe. + /// + /// Topic to receive requests from. + /// Node to associate with. + /// Callback to be called to process incoming requests. + /// QOS setting for this subscription. + /// If was disposed. + internal Service(string topic, Node node, Func callback, QualityOfServiceProfile qos = null) + { + this.Topic = topic; + this.Node = node; + this.Callback = callback; + + QualityOfServiceProfile qualityOfServiceProfile = qos ?? new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT); + + this.Options = NativeRclInterface.rclcs_service_create_options(qualityOfServiceProfile.handle); + + IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); + + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_service(); + int ret = NativeRcl.rcl_service_init( + this.Handle, + this.Node.Handle, + typeSupportHandle, + this.Topic, + this.Options + ); + + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } + } - /// - /// Send Response Message with rcl/rmw layers - /// - /// request id received when taking the Request - /// Message to be send - private void SendResp(rcl_rmw_request_id_t header, O msg) - { - RCLReturnEnum ret; - MessageInternals msgInternals = msg as MessageInternals; - msgInternals.WriteNativeMessage(); - ret = (RCLReturnEnum)NativeRcl.rcl_send_response(ref serviceHandle, ref header, msgInternals.Handle); - } + /// + /// This method is not thread safe. + /// + /// + public bool TryProcess() + { + rcl_rmw_request_id_t header = default(rcl_rmw_request_id_t); + I message = new I(); + int ret = NativeRcl.rcl_take_request( + this.Handle, + ref header, + (message as MessageInternals).Handle + ); + GC.KeepAlive(this); + + switch ((RCLReturnEnum)ret) + { + case RCLReturnEnum.RCL_RET_SERIVCE_TAKE_FAILD: + case RCLReturnEnum.RCL_RET_SERVICE_INVALID: + return false; + default: + Utils.CheckReturnEnum(ret); + break; + } + + Utils.CheckReturnEnum(ret); + this.ProcessRequest(header, message); + return true; + } - /// - // TODO(adamdbrw) this should not be public - add an internal interface - public void TakeMessage() - { - RCLReturnEnum ret; - rcl_rmw_request_id_t header = default(rcl_rmw_request_id_t); - MessageInternals message; + /// + /// Populates managed fields with native values and calls the callback with the created message + /// + /// Sending the Response is also takes care of by this method + /// Message that will be populated and provided to the callback + /// request id received when taking the Request + private void ProcessRequest(rcl_rmw_request_id_t header, I message) + { + (message as MessageInternals).ReadNativeMessage(); + this.SendResp(header, this.Callback(message)); + } - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + /// Send Response Message with rcl/rmw layers + /// + /// request id received when taking the Request + /// Message to be send + private void SendResp(rcl_rmw_request_id_t header, O msg) { - return; + MessageInternals msgInternals = msg as MessageInternals; + msgInternals.WriteNativeMessage(); + Utils.CheckReturnEnum(NativeRcl.rcl_send_response( + this.Handle, + ref header, + msgInternals.Handle + )); + GC.KeepAlive(this); } - message = new I() as MessageInternals; - ret = (RCLReturnEnum)NativeRcl.rcl_take_request(ref serviceHandle, ref header, message.Handle); - } + /// + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously or while the service is in use. + /// Disposal is automatically performed on finalization by the GC. + /// + /// + public void Dispose() + { + this.Dispose(true); + // finalizer not needed when we disposed successfully + GC.SuppressFinalize(this); + } - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_OK) - { - ProcessRequest(header, message); - } - } + /// Disposal logic. + /// If this method is not called in a finalizer. + private void Dispose(bool disposing) + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + // only do if Node.CurrentServices has not been finalized + // save since if we are being finalized we are not in a wait set anymore + if (disposing) + { + bool success = this.Node.RemoveService(this); + Debug.Assert(success, "failed to remove service"); + } + + (this as IRawService).DisposeFromNode(); + } - /// - /// Populates managed fields with native values and calls the callback with the created message - /// - /// Sending the Response is also takes care of by this method - /// Message that will be populated and provided to the callback - /// request id received when taking the Request - private void ProcessRequest(rcl_rmw_request_id_t header, MessageInternals message) - { - message.ReadNativeMessage(); - O response = callback((I)message); - SendResp(header, response); - } + /// + void IRawService.DisposeFromNode() + { + if (this.Handle == IntPtr.Zero) + { + return; + } - ~Service() - { - DestroyService(); - } + Utils.CheckReturnEnum(NativeRcl.rcl_service_fini(this.Handle, this.Node.Handle)); + this.FreeHandles(); + } - public void Dispose() - { - DestroyService(); - } + /// + /// Free the rcl handles and replace them with null pointers. + /// + /// + /// The handles are not finalised by this method. + /// + private void FreeHandles() + { + NativeRclInterface.rclcs_free_service(this.Handle); + this.Handle = IntPtr.Zero; + NativeRclInterface.rclcs_service_dispose_options(this.Options); + this.Options = IntPtr.Zero; + } - /// "Destructor" supporting disposable model - private void DestroyService() - { - lock (mutex) - { - if (!disposed) + ~Service() { - Utils.CheckReturnEnum(NativeRcl.rcl_service_fini(ref serviceHandle, ref nodeHandle)); - NativeRclInterface.rclcs_node_dispose_options(serviceOptions); - disposed = true; - Ros2csLogger.GetInstance().LogInfo("Service destroyed"); + this.Dispose(false); } - } } - } } diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 19885f97..4132b9c1 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,122 +13,187 @@ // limitations under the License. using System; +using System.Diagnostics; using ROS2.Internal; namespace ROS2 { - /// Subscription to a topic with a given type - /// Subscriptions are created through INode interface (CreateSubscription) - public class Subscription: ISubscription where T : Message, new () - { - public rcl_subscription_t Handle { get { return subscriptionHandle; } } - private rcl_subscription_t subscriptionHandle; - - public string Topic { get { return topic; } } - private string topic; - - public bool IsDisposed { get { return disposed; } } - private bool disposed = false; - - private rcl_node_t nodeHandle; - private readonly Action callback; - private IntPtr subscriptionOptions; - - public object Mutex { get { return mutex; } } - private object mutex = new object(); - - /// Tries to get a message from rcl/rmw layers. Calls the callback if successful - // TODO(adamdbrw) this should not be public - add an internal interface - public void TakeMessage() + /// + /// Subscription of a topic with a given type wrapping a rcl subscription. + /// + /// + /// This is the implementation produced by , + /// use this method to create new instances. + /// + /// + /// + public sealed class Subscription : ISubscription, IRawSubscription where T : Message, new() { - RCLReturnEnum ret; - MessageInternals message; - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + public string Topic { get; private set; } + + /// + public bool IsDisposed { - return; + get + { + bool ok = NativeRclInterface.rclcs_subscription_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } } - message = CreateMessage(); - ret = (RCLReturnEnum)NativeRcl.rcl_take(ref subscriptionHandle, message.Handle, IntPtr.Zero, IntPtr.Zero); - } - - bool gotMessage = ret == RCLReturnEnum.RCL_RET_OK; + /// + /// Handle to the rcl subscription + /// + public IntPtr Handle { get; private set; } = IntPtr.Zero; + + /// + /// Handle to the rcl subscription options + /// + private IntPtr Options = IntPtr.Zero; + + /// + /// Node associated with this instance. + /// + private readonly Node Node; + + /// + /// Callback invoked when a message is received. + /// + private readonly Action Callback; + + /// + /// Create a new instance. + /// + /// + /// The caller is responsible for adding the instance to . + /// This action is not thread safe. + /// + /// Topic to subscribe to. + /// Node to associate with. + /// Callback invoked when a message is received. + /// QOS setting for this subscription. + /// If was disposed. + internal Subscription(string topic, Node node, Action callback, QualityOfServiceProfile qos = null) + { + this.Topic = topic; + this.Node = node; + this.Callback = callback; + + QualityOfServiceProfile qualityOfServiceProfile = qos ?? new QualityOfServiceProfile(); + + this.Options = NativeRclInterface.rclcs_subscription_create_options(qualityOfServiceProfile.handle); + + IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); + + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_subscription(); + int ret = NativeRcl.rcl_subscription_init( + this.Handle, + this.Node.Handle, + typeSupportHandle, + this.Topic, + this.Options + ); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } + } - if (gotMessage) - { - TriggerCallback(message); - } - } + /// + /// This method is not thread safe. + /// + /// + public bool TryProcess() + { + T message = new T(); + int ret = NativeRcl.rcl_take( + this.Handle, + (message as MessageInternals).Handle, + IntPtr.Zero, + IntPtr.Zero + ); + GC.KeepAlive(this); + + switch ((RCLReturnEnum)ret) + { + case RCLReturnEnum.RCL_RET_SUBSCRIPTION_TAKE_FAILED: + case RCLReturnEnum.RCL_RET_SUBSCRIPTION_INVALID: + return false; + default: + Utils.CheckReturnEnum(ret); + break; + } + + (message as MessageInternals).ReadNativeMessage(); + this.Callback(message); + return true; + } - /// Construct a message of the subscription type - private MessageInternals CreateMessage() - { - return new T() as MessageInternals; - } + /// + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously or while the subscription is in use. + /// Disposal is automatically performed on finalization by the GC. + /// + /// + public void Dispose() + { + this.Dispose(true); + // finalizer not needed when we disposed successfully + GC.SuppressFinalize(this); + } - /// Populates managed fields with native values and calls the callback with created message - /// Message that will be populated and returned through callback - private void TriggerCallback(MessageInternals message) - { - message.ReadNativeMessage(); - callback((T)message); - } + /// Disposal logic. + /// If this method is not called in a finalizer. + private void Dispose(bool disposing) + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + // only do if Node.CurrentSubscriptions has not been finalized + // save since if we are being finalized we are not in a wait set anymore + if (disposing) + { + bool success = this.Node.RemoveSubscription(this); + Debug.Assert(success, "failed to remove subscription"); + } + + (this as IRawSubscription).DisposeFromNode(); + } - /// Internal constructor for Subscription. Use INode.CreateSubscription to construct - /// - internal Subscription(string subTopic, Node node, Action cb, QualityOfServiceProfile qos = null) - { - callback = cb; - nodeHandle = node.nodeHandle; - topic = subTopic; - subscriptionHandle = NativeRcl.rcl_get_zero_initialized_subscription(); - - QualityOfServiceProfile qualityOfServiceProfile = qos; - if (qualityOfServiceProfile == null) - { - qualityOfServiceProfile = new QualityOfServiceProfile(); - } - - subscriptionOptions = NativeRclInterface.rclcs_subscription_create_options(qualityOfServiceProfile.handle); - - T msg = new T(); - MessageInternals msgInternals = msg as MessageInternals; - IntPtr typeSupportHandle = msgInternals.TypeSupportHandle; - msg.Dispose(); - - Utils.CheckReturnEnum(NativeRcl.rcl_subscription_init( - ref subscriptionHandle, - ref node.nodeHandle, - typeSupportHandle, - topic, - subscriptionOptions)); - } + /// + void IRawSubscription.DisposeFromNode() + { + if (this.Handle == IntPtr.Zero) + { + return; + } - ~Subscription() - { - DestroySubscription(); - } + Utils.CheckReturnEnum(NativeRcl.rcl_subscription_fini(this.Handle, this.Node.Handle)); + this.FreeHandles(); + } - public void Dispose() - { - DestroySubscription(); - } + /// + /// Free the rcl handles and replace them with null pointers. + /// + /// + /// The handles are not finalised by this method. + /// + private void FreeHandles() + { + NativeRclInterface.rclcs_free_subscription(this.Handle); + this.Handle = IntPtr.Zero; + NativeRclInterface.rclcs_subscription_dispose_options(this.Options); + this.Options = IntPtr.Zero; + } - /// "Destructor" supporting disposable model - private void DestroySubscription() - { - lock (mutex) - { - if (!disposed) + ~Subscription() { - Utils.CheckReturnEnum(NativeRcl.rcl_subscription_fini(ref subscriptionHandle, ref nodeHandle)); - NativeRclInterface.rclcs_node_dispose_options(subscriptionOptions); - disposed = true; - Ros2csLogger.GetInstance().LogInfo("Subscription destroyed"); + this.Dispose(false); } - } } - } } diff --git a/src/ros2cs/ros2cs_core/WaitSet.cs b/src/ros2cs/ros2cs_core/WaitSet.cs index 18d21935..3bfa9c44 100644 --- a/src/ros2cs/ros2cs_core/WaitSet.cs +++ b/src/ros2cs/ros2cs_core/WaitSet.cs @@ -1,4 +1,5 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// Copyright 2019-2022 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,181 +14,491 @@ // limitations under the License. using System; +using System.Collections; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; namespace ROS2 { - internal enum AddResult - { - SUCCESS, - FULL, - DISPOSED - } + /// + /// Collection used for waiting on resources to become ready. + /// All methods and properties are NOT thread safe. + /// + internal sealed class WaitSet : IReadOnlyCollection, IExtendedDisposable + { + /// + /// The instances currently in the wait set. + /// + public ICollection Subscriptions { get { return this.CurrentSubscriptions; } } - internal class WaitSet - { - internal ulong SubscriptionCount {get { return Handle.size_of_subscriptions.ToUInt64(); }} + /// + /// The instances currently in the wait set. + /// + public ICollection Clients { get { return this.CurrentClients; } } - internal ulong ClientCount {get { return Handle.size_of_clients.ToUInt64(); }} + /// + /// The instances currently in the wait set. + /// + public ICollection Services { get { return this.CurrentServices; } } - internal ulong ServiceCount {get { return Handle.size_of_services.ToUInt64(); }} + /// + /// The instances currently in the wait set. + /// + public ICollection GuardConditions { get { return this.CurrentGuardConditions; } } - private rcl_wait_set_t Handle; + /// + /// Context associated with this wait set. + /// + public IContext Context + { + get => this.ROSContext; + } - internal WaitSet(ref rcl_context_t context) - { - Handle = NativeRcl.rcl_get_zero_initialized_wait_set(); - Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_init( - ref Handle, - (UIntPtr)0, - (UIntPtr)0, - (UIntPtr)0, - (UIntPtr)0, - (UIntPtr)0, - (UIntPtr)0, - ref context, - NativeRcl.rcutils_get_default_allocator())); - } + private Context ROSContext; - ~WaitSet() - { - Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_fini(ref Handle)); - } + /// + public bool IsDisposed + { + get + { + bool ok = NativeRclInterface.rclcs_wait_set_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } + } - internal void Clear() - { - Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_clear(ref Handle)); - } + /// + public int Count + { + get + { + return this.Subscriptions.Count + + this.Clients.Count + + this.Services.Count + + this.GuardConditions.Count; + } + } - internal void Resize(ulong subscriptionCount, ulong clientCount, ulong serviceCount) - { - Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_resize( - ref Handle, - (UIntPtr)subscriptionCount, - (UIntPtr)0, - (UIntPtr)0, - (UIntPtr)clientCount, - (UIntPtr)serviceCount, - (UIntPtr)0)); - } + /// + /// Handle to the rcl wait set. + /// + private IntPtr Handle = IntPtr.Zero; - internal AddResult TryAddSubscription(ISubscriptionBase subscription, out ulong index) - { - UIntPtr native_index = default(UIntPtr); - int ret; - lock (subscription.Mutex) - { - if (subscription.IsDisposed) - { - index = default(ulong); - return AddResult.DISPOSED; - } - - rcl_subscription_t subscription_handle = subscription.Handle; - ret = NativeRcl.rcl_wait_set_add_subscription( - ref Handle, - ref subscription_handle, - ref native_index - ); - } - - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_WAIT_SET_FULL) - { - index = default(ulong); - return AddResult.FULL; - } - else - { - Utils.CheckReturnEnum(ret); - index = native_index.ToUInt64(); - return AddResult.SUCCESS; - } - } + /// + /// Modification version used to detect if the wait set was modified. + /// + private uint Version = 0; - internal AddResult TryAddClient(IClientBase client, out ulong index) - { - UIntPtr native_index = default(UIntPtr); - int ret; - lock (client.Mutex) - { - if (client.IsDisposed) - { - index = default(ulong); - return AddResult.DISPOSED; - } - - rcl_client_t client_handle = client.Handle; - ret = NativeRcl.rcl_wait_set_add_client( - ref Handle, - ref client_handle, - ref native_index - ); - } - - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_WAIT_SET_FULL) - { - index = default(ulong); - return AddResult.FULL; - } - else - { - Utils.CheckReturnEnum(ret); - index = native_index.ToUInt64(); - return AddResult.SUCCESS; - } - } + // are exposed as collections to prevent users from depending on the changing indexes + private readonly List CurrentSubscriptions = new List(); - internal AddResult TryAddService(IServiceBase service, out ulong index) - { - UIntPtr native_index = default(UIntPtr); - int ret; - - lock (service.Mutex) - { - if (service.IsDisposed) - { - index = default(ulong); - return AddResult.DISPOSED; - } - - rcl_service_t service_handle = service.Handle; - ret = NativeRcl.rcl_wait_set_add_service( - ref Handle, - ref service_handle, - ref native_index - ); - } - - - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_WAIT_SET_FULL) - { - index = default(ulong); - return AddResult.FULL; - } - else - { - Utils.CheckReturnEnum(ret); - index = native_index.ToUInt64(); - return AddResult.SUCCESS; - } - } + private readonly List CurrentClients = new List(); - internal bool Wait() - { - return Wait(TimeSpan.FromTicks(-1)); - } + private readonly List CurrentServices = new List(); - internal bool Wait(TimeSpan timeout) - { - int ret = NativeRcl.rcl_wait(ref Handle, timeout.Ticks * 100); - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_TIMEOUT) - { - return false; - } - else - { - Utils.CheckReturnEnum(ret); - return true; - } + private readonly List CurrentGuardConditions = new List(); + + /// + /// Construct a new instance. + /// + /// Associated context + internal WaitSet(Context context) + { + this.ROSContext = context; + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_wait_set(); + int ret = NativeRcl.rcl_wait_set_init( + this.Handle, + new UIntPtr(Convert.ToUInt32(this.CurrentSubscriptions.Capacity)), + new UIntPtr(Convert.ToUInt32(this.CurrentGuardConditions.Capacity)), + UIntPtr.Zero, + new UIntPtr(Convert.ToUInt32(this.CurrentClients.Capacity)), + new UIntPtr(Convert.ToUInt32(this.CurrentServices.Capacity)), + UIntPtr.Zero, + context.Handle, + NativeRcl.rcutils_get_default_allocator() + ); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } + } + + /// + public IEnumerator GetEnumerator() + { + return this.Subscriptions + .Concat(this.Clients) + .Concat(this.Services) + .Concat(this.GuardConditions) + .GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + + /// + /// Wait for something to become ready. + /// + /// + /// This will invalidate previous wait results. + /// + /// The resources that became ready + /// + /// The wait set can only be waited on if it contains something + /// + public WaitResult Wait() + { + if (this.TryWait(TimeSpan.FromTicks(-1), out WaitResult ready)) + { + return ready; + } + // should never happen + throw new TimeoutException("infinite wait timed out"); + } + + /// + /// Resize the wait set to have the same size as the collections holding the resources and clear it. + /// + /// + /// No allocation will be done if the new size of the wait set matches the current size. + /// + private void PrepareWaitSet() + { + int ret = NativeRcl.rcl_wait_set_resize( + this.Handle, + new UIntPtr(Convert.ToUInt32(this.CurrentSubscriptions.Count)), + new UIntPtr(Convert.ToUInt32(this.CurrentGuardConditions.Count)), + UIntPtr.Zero, + new UIntPtr(Convert.ToUInt32(this.CurrentClients.Count)), + new UIntPtr(Convert.ToUInt32(this.CurrentServices.Count)), + UIntPtr.Zero + ); + if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_INVALID_ARGUMENT) + { + throw new ObjectDisposedException("RCL wait set"); + } + Utils.CheckReturnEnum(ret); + } + + /// + /// Check if the wait set contains something at an index. + /// + /// Delegate used for accessing the array of that resource + /// Index to check + /// Whether the wait set already contains a resource + /// The wait set does not contain the index + private bool IsAdded(NativeRclInterface.WaitSetGetType getter, int index) + { + if (getter(this.Handle, new UIntPtr(Convert.ToUInt32(index)), out IntPtr ptr)) + { + return ptr != IntPtr.Zero; + } + throw new IndexOutOfRangeException($"wait set has no index {index}"); + } + + /// + /// Fill the wait set of a resource. + /// + /// + /// The wrapper will be updated if the wait set adds resources at different indexes. + /// + /// Type of the resource + /// Delegate used for adding to the wait set + /// Delegate used for accessing the wait set + /// Resources to add + private void FillWaitSet(NativeRcl.WaitSetAddType adder, NativeRclInterface.WaitSetGetType getter, IList wrappers) + where T : IWaitable + { + if (wrappers.Count == 0) + { + return; + } + int filled = 0; + int index = 0; + // add index to wait set until it is filled + while (true) + { + Utils.CheckReturnEnum(adder(this.Handle, wrappers[index].Handle, out UIntPtr destination)); + filled += 1; + int newIndex = Convert.ToInt32(destination.ToUInt32()); + if (newIndex != index) + { + // different wait set index, update wrappers and repeat with not added resource + (wrappers[index], wrappers[newIndex]) = (wrappers[newIndex], wrappers[index]); + continue; + } + if (filled >= wrappers.Count) + { + // all wrappers filled, skip searching for next index to prevent triggering IndexOutOfRangeException + break; + } + // some wrappers are not added yet, advance to next index not already in wait set + // IndexOutOfRangeException indicates that not all wrappers could be added and + // should not be ignored since it hints at a bug or threading issue + do + { + index += 1; + } + while (this.IsAdded(getter, index)); + } + } + + /// + /// Fill the wait set of all resources. + /// + /// + /// This will clear and resize the wait set first. + /// + private void FillWaitSet() + { + this.PrepareWaitSet(); + this.FillWaitSet( + NativeRcl.rcl_wait_set_add_subscription, + NativeRclInterface.rclcs_wait_set_get_subscription, + this.CurrentSubscriptions + ); + this.FillWaitSet( + NativeRcl.rcl_wait_set_add_client, + NativeRclInterface.rclcs_wait_set_get_client, + this.CurrentClients + ); + this.FillWaitSet( + NativeRcl.rcl_wait_set_add_service, + NativeRclInterface.rclcs_wait_set_get_service, + this.CurrentServices + ); + this.FillWaitSet( + NativeRcl.rcl_wait_set_add_guard_condition, + NativeRclInterface.rclcs_wait_set_get_guard_condition, + this.CurrentGuardConditions + ); + } + + /// Timeout for waiting, infinite if negative + /// The resources that became ready + /// Whether the wait did not timed out + /// + public bool TryWait(TimeSpan timeout, out WaitResult result) + { + // invalidate last wait result + this.Version += 1; + + this.FillWaitSet(); + + long nanoSeconds = timeout.Ticks * (1_000_000_000 / TimeSpan.TicksPerSecond); + int ret = NativeRcl.rcl_wait(this.Handle, nanoSeconds); + if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_TIMEOUT) + { + result = default(WaitResult); + return false; + } + Utils.CheckReturnEnum(ret); + + result = new WaitResult(this); + return true; + } + + /// + public void Dispose() + { + this.Dispose(true); + GC.SuppressFinalize(this); + } + + /// Disposal logic. + /// If this method is not called in a finalizer + private void Dispose(bool disposing) + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + if (disposing) + { + bool success = this.ROSContext.RemoveWaitSet(this); + Debug.Assert(success, "failed to remove wait set"); + this.ClearCollections(); + } + + Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_fini(this.Handle)); + this.FreeHandles(); + } + + /// Dispose without modifying the context. + internal void DisposeFromContext() + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + this.ClearCollections(); + + Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_fini(this.Handle)); + this.FreeHandles(); + } + + private void ClearCollections() + { + this.Subscriptions.Clear(); + this.Clients.Clear(); + this.Services.Clear(); + this.GuardConditions.Clear(); + } + + private void FreeHandles() + { + NativeRclInterface.rclcs_free_wait_set(this.Handle); + this.Handle = IntPtr.Zero; + } + + ~WaitSet() + { + this.Dispose(false); + } + + /// + /// Result of waiting on a wait set. + /// + /// + /// The enumerables are invalidated when waiting on the wait set again, + /// which is only done for debugging purposes and not done when the + /// collections containing the primitives change. + /// + public struct WaitResult : IEnumerable + { + /// + /// Subscriptions which are ready. + /// + public IEnumerable ReadySubscriptions + { + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_subscription, + this.WaitSet.CurrentSubscriptions + ); + } + + /// + /// Clients which are ready. + /// + public IEnumerable ReadyClients + { + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_client, + this.WaitSet.CurrentClients + ); + } + + /// + /// Services which are ready. + /// + public IEnumerable ReadyServices + { + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_service, + this.WaitSet.CurrentServices + ); + } + + /// + /// Guard conditions which are ready. + /// + public IEnumerable ReadyGuardConditions + { + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_guard_condition, + this.WaitSet.CurrentGuardConditions + ); + } + + /// + /// Wait set associated with this result. + /// + private readonly WaitSet WaitSet; + + /// + /// Version when this result was created. + /// + private readonly uint CreatedVersion; + + internal WaitResult(WaitSet waitSet) + { + this.WaitSet = waitSet; + this.CreatedVersion = waitSet.Version; + } + + /// + /// Assert that the wait set is valid and has not been waited on. + /// + /// If the wait set was disposed. + private void AssertOk() + { + if (this.WaitSet.Version != this.CreatedVersion || this.WaitSet.IsDisposed) + { + throw new ObjectDisposedException("rcl wait set"); + } + } + + /// + /// Primitives currently in the wait set. + /// + /// + /// After waiting the only primitives left in + /// the wait set are ready. + /// + /// Primitive type + /// Function to access the wait set array. + /// List holding the primitives. + /// Enumerable of the primitives. + private IEnumerable CurrentPrimitives(NativeRclInterface.WaitSetGetType getter, IList primitives) where T : IWaitable + { + this.AssertOk(); + for (UIntPtr index = UIntPtr.Zero; getter(this.WaitSet.Handle, index, out IntPtr ptr); index += 1) + { + if (ptr != IntPtr.Zero) + { + yield return primitives[Convert.ToInt32(index.ToUInt32())]; + this.AssertOk(); + } + } + } + + /// + public IEnumerator GetEnumerator() + { + return this.ReadySubscriptions + .Concat(this.ReadyClients) + .Concat(this.ReadyServices) + .Concat(this.ReadyGuardConditions) + .GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + + /// + /// Deconstruct the result into the resources which are ready. + /// + public void Deconstruct( + out IEnumerable subscriptions, + out IEnumerable clients, + out IEnumerable services, + out IEnumerable guard_conditions) + { + subscriptions = this.ReadySubscriptions; + clients = this.ReadyClients; + services = this.ReadyServices; + guard_conditions = this.ReadyGuardConditions; + } + } } - } } diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs new file mode 100644 index 00000000..3889f75b --- /dev/null +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -0,0 +1,583 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; + +namespace ROS2.Executors +{ + /// + /// Executor which has to be spun manually. + /// + /// + /// Spinning is impossible if a rescan is scheduled + /// to allow waiting to stop when the executor is not spinning. + /// + public sealed class ManualExecutor : IExecutor + { + /// + /// Context associated with this executor. + /// + public IContext Context + { + get { return this.WaitSet.Context; } + } + + /// + /// Whether the executor is currently spinning. + /// + public bool IsSpinning + { + get => this._IsSpinning; + private set => this._IsSpinning = value; + } + + private volatile bool _IsSpinning = false; + + /// + /// Whether a rescan is scheduled. + /// + public bool RescanScheduled + { + get => this._RescanScheduled; + private set => this._RescanScheduled = value; + } + + // volatile since it may be changed by multiple threads + private volatile bool _RescanScheduled = false; + + /// + /// To prevent from being starved by multiple spins. + /// + private long SpinId = 0; + + /// + public bool IsDisposed + { + get { return this.WaitSet.IsDisposed || this.InterruptCondition.IsDisposed; } + } + + /// + /// This property is thread safe. + /// + /// + public int Count + { + get + { + lock (this.Nodes) + { + return this.Nodes.Count; + } + } + } + + /// + /// This property is thread safe. + /// + /// + public bool IsReadOnly + { + get { return false; } + } + + /// + /// Wait set used while spinning. + /// + /// + /// Is also used to notify when the executor finished spinning. + /// + private readonly WaitSet WaitSet; + + /// + /// Guard condition used for interrupting waits. + /// + private readonly GuardCondition InterruptCondition; + + /// + /// Nodes in the executor. + /// + private readonly HashSet Nodes = new HashSet(); + + /// + /// Create a new instance. + /// + /// Context to associate with. + /// If is disposed. + public ManualExecutor(Context context) : this( + context.CreateWaitSet(), + context.CreateGuardCondition(() => { }) + ) + { } + + /// + internal ManualExecutor(WaitSet waitSet, GuardCondition interruptCondition) + { + this.WaitSet = waitSet; + this.InterruptCondition = interruptCondition; + this.WaitSet.GuardConditions.Add(this.InterruptCondition); + } + + /// + /// This method is thread safe when setting + /// is thread safe + /// and not changed concurrently. + /// + /// If the node already has an executor. + /// + public void Add(INode node) + { + if (!(node.Executor is null)) + { + throw new InvalidOperationException("node already has an executor"); + } + // make sure the node knows its + // new executor before it can be added to the wait set + node.Executor = this; + lock (this.Nodes) + { + this.Nodes.Add(node); + } + this.ScheduleRescan(); + } + + /// + /// This method is thread safe when setting + /// is thread safe. + /// + /// If the executor was disposed. + /// + public bool Remove(INode node) + { + bool removed; + lock (this.Nodes) + { + removed = this.Nodes.Remove(node); + } + if (removed) + { + try + { + Debug.Assert( + Object.ReferenceEquals(node.Executor, this), + "node has different executor" + ); + this.ScheduleRescan(); + this.Wait(); + } + finally + { + // clear executor after it + // is safe to do so + node.Executor = null; + } + } + return removed; + } + + /// + /// This method is thread safe when setting + /// is thread safe. + /// + /// If the executor was disposed. + /// + public void Clear() + { + // use thread safe enumerator + foreach (INode node in this) + { + this.Remove(node); + } + } + + /// + /// This method is thread safe. + /// + /// + public bool Contains(INode node) + { + lock (this.Nodes) + { + return this.Nodes.Contains(node); + } + } + + /// + /// This method is thread safe. + /// + /// + public void CopyTo(INode[] array, int index) + { + if (array is null) + { + throw new ArgumentException("array is null"); + } + if (index < 0) + { + throw new ArgumentOutOfRangeException("index is less than 0"); + } + lock (this.Nodes) + { + foreach (INode item in this.Nodes) + { + try + { + array[index] = item; + } + catch (IndexOutOfRangeException e) + { + throw new ArgumentException("array is too small", e); + } + index += 1; + } + } + } + + /// + /// The enumerator is thread safe. + /// + /// + public IEnumerator GetEnumerator() + { + lock (this.Nodes) + { + return this.Nodes.ToArray().AsEnumerable().GetEnumerator(); + } + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + + /// + /// This method is thread safe. + /// + /// + public void ScheduleRescan() + { + this.RescanScheduled = true; + } + + /// + /// This method is an alias for . + /// + /// + public bool TryScheduleRescan(INode node) + { + this.ScheduleRescan(); + return true; + } + + /// + /// This method is thread safe and uses . + /// + /// + public void Wait() + { + if (this.RescanScheduled) + { + lock (this.WaitSet) + { + this.WaitUntilDone(this.SpinId); + } + } + } + + /// + /// This method is thread safe. + /// + /// If the executor was disposed. + /// If the timeout is negative or too big. + /// + public bool TryWait(TimeSpan timeout) + { + if (timeout.Ticks < 0) + { + throw new ArgumentOutOfRangeException("timeout is negative"); + } + if (this.RescanScheduled) + { + lock (this.WaitSet) + { + // read id inside the lock to prevent an outdated id from being copied + return this.WaitUntilDone(this.SpinId, timeout); + } + } + return true; + } + + /// + /// Utility method to wait until the current spin has finished. + /// + /// + /// This replaces a which did starve waiters + /// when spinning multiple times. + /// + /// Current spin id. + private void WaitUntilDone(long spinId) + { + // the condition is checked with the lock held to prevent + // a the spin from pulsing before the wait can be started + while (this.IsSpinning && this.SpinId == spinId) + { + try + { + // stop a possible current spin + this.Interrupt(); + } + catch (ObjectDisposedException) + { + // if the context is shut down then the + // guard condition might be disposed but + // nodes still have to be removed + } + Monitor.Wait(this.WaitSet); + } + } + + /// + /// Utility method to wait until the current spin has finished. + /// + /// Current spin id. + /// Timeout when waiting + /// Whether the wait did not time out. + /// Timeout is too big. + private bool WaitUntilDone(long spinId, TimeSpan timeout) + { + int milliSeconds; + try + { + milliSeconds = Convert.ToInt32(timeout.TotalMilliseconds); + } + catch (OverflowException e) + { + throw new ArgumentOutOfRangeException("timeout too big", e); + } + int remainingTimeout = milliSeconds; + uint startTime = (uint)Environment.TickCount; + while (this.IsSpinning && this.SpinId == spinId) + { + try + { + // stop a possible current spin + this.Interrupt(); + } + catch (ObjectDisposedException) + { + // if the context is shut down then the + // guard condition might be disposed but + // nodes still have to be removed + } + if (!Monitor.Wait(this.WaitSet, remainingTimeout)) + { + // if the wait timed out return immediately + return false; + } + // update the timeout for the next wait + uint elapsed = (uint)Environment.TickCount - startTime; + if (elapsed > int.MaxValue) + { + return false; + } + remainingTimeout = milliSeconds - (int)elapsed; + if (remainingTimeout <= 0) + { + return false; + } + } + return true; + } + + /// + /// Interrupt the next or current . + /// + /// + /// This method only causes the wait to be skipped, work which is ready will be executed. + /// This method is thread safe. + /// + /// If the executor or context was disposed. + public void Interrupt() + { + this.InterruptCondition.Trigger(); + } + + /// + /// Try to process work if no rescan is scheduled. + /// + /// + /// This method is thread safe if it itself or is not executed concurrently. + /// + /// Maximum time to wait for work to become available. + /// If the executor or context was disposed. + /// Whether work could be processed since no rescan was scheduled. + public bool TrySpin(TimeSpan timeout) + { + this.IsSpinning = true; + try + { + // check after setting IsSpinning to + // prevent race condition + if (this.RescanScheduled) + { + return false; + } + if (this.WaitSet.TryWait(timeout, out var result)) + { + foreach (IWaitable waitable in result) + { + waitable.TryProcess(); + } + } + } + finally + { + // update flag before waking threads + this.IsSpinning = false; + lock (this.WaitSet) + { + // prevent other threads from reading stale result + // overflow is acceptable + unchecked { this.SpinId++; } + // notify other threads that we finished spinning + Monitor.PulseAll(this.WaitSet); + } + } + return true; + } + + /// + /// Rescan the nodes of this executor for + /// new objects to wait for and clear scheduled rescans. + /// + /// + /// This method is thread safe if it itself or is not executed concurrently + /// and enumerating the primitives of the nodes is thread safe. + /// + public void Rescan() + { + // clear the wait set first to + // assert that the removed objects + // can be disposed even on error + this.WaitSet.Subscriptions.Clear(); + this.WaitSet.Services.Clear(); + this.WaitSet.Clients.Clear(); + // clear the flag to prevent clearing rescans + // scheduled just after we finished rescaning + this.RescanScheduled = false; + try + { + // use the thread safe GetEnumerator wrapper + foreach (INode node in this) + { + foreach (ISubscriptionBase subscription in node.Subscriptions) + { + this.WaitSet.Subscriptions.Add(subscription); + } + foreach (IServiceBase service in node.Services) + { + this.WaitSet.Services.Add(service); + } + foreach (IClientBase client in node.Clients) + { + this.WaitSet.Clients.Add(client); + } + } + } + catch (Exception) + { + // schedule rescan since the wait set may not be filled completely + this.ScheduleRescan(); + throw; + } + } + + /// + /// Utility which spins while a condition is true + /// and handles automatic rescanning. + /// + /// + /// The condition check is performed before each spin. + /// + /// Condition which has to be true to continue spinning. + public void SpinWhile(Func condition) + { + this.SpinWhile(condition, TimeSpan.FromSeconds(0.1)); + } + + /// + /// Maximum time to wait for work to become available. + public void SpinWhile(Func condition, TimeSpan timeout) + { + while (condition()) + { + if (!this.TrySpin(timeout)) + { + this.Rescan(); + } + } + } + + /// + /// Create a task which calls when started. + /// + /// + /// The resulting task prevents and from being called + /// and this instance as well as its context from being disposed safely while it is running. + /// + /// Maximum time to wait for work to become available. + /// Token to cancel the task. + /// Task representing the spin operation. + public Task CreateSpinTask(TimeSpan timeout, CancellationToken cancellationToken) + { + return new Task(() => { + using (cancellationToken.Register(this.Interrupt)) + { + this.SpinWhile(() => !cancellationToken.IsCancellationRequested, timeout); + } + cancellationToken.ThrowIfCancellationRequested(); + }, cancellationToken, TaskCreationOptions.LongRunning); + } + + /// + /// This method is not thread safe and may not be called from + /// multiple threads simultaneously or while the executor is in use. + /// Furthermore, it does not dispose the nodes of this executor. + /// Remember that is called when disposing + /// with nodes in the executor. + /// + /// + public void Dispose() + { + // remove nodes one by one to + // prevent node.Executor from being out + // of sync if an exception occurs + foreach (INode node in this.Nodes.ToArray()) + { + this.Nodes.Remove(node); + // waiting not required since the executor + // should not be running + node.Executor = null; + } + this.WaitSet.Dispose(); + this.InterruptCondition.Dispose(); + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_core/executors/TaskExecutor.cs b/src/ros2cs/ros2cs_core/executors/TaskExecutor.cs new file mode 100644 index 00000000..042fb51e --- /dev/null +++ b/src/ros2cs/ros2cs_core/executors/TaskExecutor.cs @@ -0,0 +1,191 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Threading; +using System.Threading.Tasks; + +namespace ROS2.Executors +{ + /// + /// Executor which wraps a and automatically + /// executes the task created by . + /// + /// + /// The spin task is automatically stopped when + /// is called or the context is shut down. + /// + public sealed class TaskExecutor : IExecutor + { + /// + /// Task managed by this executor. + /// + public Task Task { get; private set; } + + private readonly CancellationTokenSource CancellationSource = new CancellationTokenSource(); + + private readonly ManualExecutor Executor; + + private readonly Context Context; + + /// Context associated with this executor. + /// Maximum time to wait for work to become available. + public TaskExecutor(Context context, TimeSpan timeout) + { + this.Context = context; + this.Executor = new ManualExecutor(context); + this.Task = this.Executor.CreateSpinTask(timeout, this.CancellationSource.Token); + try + { + context.OnShutdown += this.StopSpinTask; + this.Task.Start(); + } + catch (SystemException) + { + try + { + context.OnShutdown -= this.StopSpinTask; + } + finally + { + this.Executor.Dispose(); + } + throw; + } + } + + /// + public bool IsDisposed + { + get => this.Executor.IsDisposed; + } + + /// + public int Count + { + get => this.Executor.Count; + } + + /// + public bool IsReadOnly + { + get => this.Executor.IsReadOnly; + } + + /// + public void Add(INode node) + { + this.Executor.Add(node); + } + + /// + public void Clear() + { + this.Executor.Clear(); + } + + /// + public bool Contains(INode node) + { + return this.Executor.Contains(node); + } + + /// + public void CopyTo(INode[] array, int arrayIndex) + { + this.Executor.CopyTo(array, arrayIndex); + } + + /// + public bool Remove(INode node) + { + return this.Executor.Remove(node); + } + + /// + public IEnumerator GetEnumerator() + { + return this.Executor.GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + + /// + public void ScheduleRescan() + { + this.Executor.ScheduleRescan(); + } + + /// + public bool TryScheduleRescan(INode node) + { + return this.Executor.TryScheduleRescan(node); + } + + /// + public void Wait() + { + this.Executor.Wait(); + } + + /// + public bool TryWait(TimeSpan timeout) + { + return this.Executor.TryWait(timeout); + } + + /// + /// Stop the spin task and return after it has stopped. + /// + /// + /// This function returns immediately if the spin task + /// has already been stopped. + /// + private void StopSpinTask() + { + try + { + this.CancellationSource.Cancel(); + } + catch (ObjectDisposedException) + { + // task has been canceled before + } + try + { + this.Task.Wait(); + } + catch (AggregateException e) + { + e.Handle(inner => inner is TaskCanceledException); + } + catch (ObjectDisposedException) + { + // task has already stopped + } + } + + /// + /// + /// The wrapper handles stopping the spin task. + /// + public void Dispose() + { + try + { + this.StopSpinTask(); + } + catch (AggregateException) + { + // prevent faulted task from preventing disposal + } + this.Context.OnShutdown -= this.StopSpinTask; + this.Task.Dispose(); + this.Executor.Dispose(); + this.CancellationSource.Dispose(); + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_core/interfaces/IClient.cs b/src/ros2cs/ros2cs_core/interfaces/IClient.cs index 96a4e1a2..6ae8d5b6 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IClient.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IClient.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,81 +17,74 @@ namespace ROS2 { - /// Non-generic base interface for all subscriptions - /// - public interface IClientBase: IExtendedDisposable - { - /// - /// Tries to get a Response message from rcl/rmw layers - /// + /// Non-generic base interface for all clients. /// - /// Marks the corresponding as finished if successful + /// This interface is useful for managing client collections and disposal. + /// Create instances with . /// - // TODO this should not be public - add an internal interface - void TakeMessage(); + public interface IClientBase : IExtendedDisposable, IWaitable + { + /// Topic of this client. + string Topic { get; } - /// - /// topic name which was used when calling - /// - string Topic {get;} + /// + /// Requests which are pending for this client. + /// + IReadOnlyDictionary PendingRequests { get; } - /// - /// Requests which are pending for this client. - /// - IReadOnlyDictionary PendingRequests {get;} + /// + /// Check if the service to be called is available + /// + /// if the service is avilable + bool IsServiceAvailable(); - /// - /// Remove a pending and cancel it. - /// - /// - /// Tasks are automatically removed on completion and have to be removed only when canceled. - /// - /// Task to be removed. - /// Whether the Task was removed successfully. - bool Cancel(Task task); - - rcl_client_t Handle {get;} - - /// service mutex for internal use - object Mutex { get; } - } + /// + /// Remove a pending and cancel it. + /// + /// + /// Tasks are automatically removed on completion and have to be removed only when canceled. + /// + /// Task to be removed. + /// Whether the Task was removed successfully. + bool Cancel(Task task); + } - /// Generic base interface for all subscriptions - /// Message Type to be send - /// Message Type to be received - /// - public interface IClient: IClientBase - where I: Message - where O: Message - { - /// - /// Requests which are pending for this client. - /// - new IReadOnlyDictionary> PendingRequests {get;} + /// Internal client extensions. + internal interface IRawClient : IClientBase + { + /// Dispose without modifying the node. + void DisposeFromNode(); + } - /// - /// Check if the service to be called is available - /// - /// if the service is avilable - bool IsServiceAvailable(); + /// Generic base interface for all clients. + /// Message Type to be send. + /// Message Type to be received. + public interface IClient : IClientBase + where I : Message + where O : Message + { + /// + /// Requests which are pending for this client. + /// + new IReadOnlyDictionary> PendingRequests { get; } - /// - /// Send a Request to a Service and wait for a Response - /// - /// The provided message can be modified or disposed after this call - /// Message to be send - /// Response of the Service - O Call(I msg); + /// + /// Send a Request to a Service and wait for a Response + /// + /// The provided message can be modified or disposed after this call + /// Message to be send + /// Response of the Service + O Call(I msg); - /// - /// Send a Request to a Service and wait for a Response asynchronously - /// - /// Message to be send - /// representing the Response of the Service - Task CallAsync(I msg); + /// + /// Send a Request to a Service and wait for a Response asynchronously + /// + /// Message to be send + /// representing the Response of the Service + Task CallAsync(I msg); - /// - /// Options used when creating the - Task CallAsync(I msg, TaskCreationOptions options); - } + /// + /// Options used when creating the + Task CallAsync(I msg, TaskCreationOptions options); + } } diff --git a/src/ros2cs/ros2cs_core/interfaces/IContext.cs b/src/ros2cs/ros2cs_core/interfaces/IContext.cs new file mode 100644 index 00000000..44fba76a --- /dev/null +++ b/src/ros2cs/ros2cs_core/interfaces/IContext.cs @@ -0,0 +1,51 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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; + +namespace ROS2 +{ + /// + /// ROS Context encapsulating the non-global state of an init/shutdown cycle. + /// + /// + /// Instances should be disposed with which may NOT automatically performed completely on garbage collection. + /// + public interface IContext : IExtendedDisposable + { + /// + /// Nodes associated with this instance + /// + /// Will be disposed on disposal of this instance. + IReadOnlyDictionary Nodes { get; } + + /// + /// Event triggered after context shutdown before disposing nodes and finalization. + /// + event Action OnShutdown; + + /// + /// Check if the instance is valid (has not been disposed). + /// + bool Ok(); + + /// + /// Try to create a . + /// + /// Name of the node, has to be unqiue + /// If the instance could be created. + bool TryCreateNode(string name, out INode node); + } +} diff --git a/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs new file mode 100644 index 00000000..5c658a36 --- /dev/null +++ b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs @@ -0,0 +1,60 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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; + +namespace ROS2 +{ + /// + /// Executor controlling the processing of callbacks of a set of nodes. + /// + /// + /// Adding and removing Nodes has to update . + /// Furthermore, removing Nodes has to guarantee that the Node is ready to be disposed. + /// + public interface IExecutor: IExtendedDisposable, ICollection + { + /// + /// Notify the instance that some nodes changed. + /// + /// + /// This is used to tell the executor when entities are created or destroyed. + /// + void ScheduleRescan(); + + /// + /// Notify the instance that a node changed. + /// + /// Node which changed. + /// If a rescan was scheduled. + /// + bool TryScheduleRescan(INode node); + + /// + /// Wait for scheduled rescans to complete. + /// + /// + /// This is used for example to ensure that removed objects + /// are removed from the executor before they are disposed. + /// Return immediately if no rescans are scheduled. + /// + void Wait(); + + /// positive Amount of time to wait. + /// Wether no timeout occurred. + /// + bool TryWait(TimeSpan timeout); + } +} diff --git a/src/ros2cs/ros2cs_core/interfaces/INode.cs b/src/ros2cs/ros2cs_core/interfaces/INode.cs index 90b59d4c..f314af70 100644 --- a/src/ros2cs/ros2cs_core/interfaces/INode.cs +++ b/src/ros2cs/ros2cs_core/interfaces/INode.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,68 +17,79 @@ namespace ROS2 { - /// Ros2cs node, created with Ros2cs.CreateNode and supporting use of publishers and subscribers - /// Is automatically disposed when Ros2cs.Shutdown is called. - /// Can also be disposed through IDisposable interface. Ros2cs.RemoveNode should be called in such case - // TODO(adamdbrw) wrap disposing so that user does not need to handle anything - public interface INode: IExtendedDisposable - { - /// Node name as given in Ros2cs.CreateNode - string Name {get;} + /// + /// Ros2cs node, created with . + /// + /// + /// Instances should be disposed with which is NOT automatically performed on shutdown. + /// + public interface INode : IExtendedDisposable + { + /// + /// Node name as given in . + /// + /// Is unique per context while node is not disposed. + string Name { get; } - /// Create a client for this node for a given topic, qos and message type - /// Can only be called in an initialized Ros2cs state. - /// Topic for the client. Naming restrictions of ros2 apply and violation results in an exception - /// Quality of Client settings. Not passing this parameter will result in default settings - /// Client for the topic, which can be used to client messages - Client CreateClient(string topic, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new(); + /// + /// Context containing this node. + /// + IContext Context { get; } - /// Remove a client - /// Note that this does not call Dispose on Client - /// Client created with earlier CreateClient call - /// Whether removal actually took place. Safe to ignore - bool RemoveClient(IClientBase client); + /// + /// Executor handling callbacks of this node, may be null. + /// + /// + /// Users have to guarantee that a node is associated with at most one executor at any given time + /// to prevent undefined behaviour when multithreading is used. + /// It is recommended to not set this property directly and leave this task to the executor. + /// + IExecutor Executor { get; set; } - /// Create a service for this node for a given topic, callback, qos and message type - /// Can only be called in an initialized Ros2cs state. - /// Topic to service to. Naming restrictions of ros2 apply and violation results in an exception - /// Action to be called when message is received (through Spin or SpinOnce). Provide a lambda or a method - /// Quality of Service settings. Not passing this parameter will result in default settings - /// Service for the topic - Service CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new(); + /// Create a publisher for this node for a given topic, qos and message type + /// Topic for the publisher. Naming restrictions of ros2 apply and violation results in an exception + /// Quality of Service settings. Not passing this parameter will result in default settings + /// Publisher for the topic, which can be used to publish messages + IPublisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new(); - /// Remove a service - /// Note that this does not call Dispose on Service - /// Service created with earlier CreateService call - /// Whether removal actually took place. Safe to ignore - bool RemoveService(IServiceBase service); + /// + /// Publishers created on this node. + /// + IReadOnlyCollection Publishers { get; } - /// Create a publisher for this node for a given topic, qos and message type - /// Can only be called in an initialized Ros2cs state. - /// Topic for the publisher. Naming restrictions of ros2 apply and violation results in an exception - /// Quality of Service settings. Not passing this parameter will result in default settings - /// Publisher for the topic, which can be used to publish messages - Publisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new(); + /// Create a subscription for this node for a given topic, callback, qos and message type + /// Topic to subscribe to. Naming restrictions of ros2 apply and violation results in an exception + /// Action to be called when message is received (through Spin or SpinOnce). Provide a lambda or a method + /// Quality of Service settings. Not passing this parameter will result in default settings + /// Subscription for the topic + ISubscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new(); - /// Create a subscription for this node for a given topic, callback, qos and message type - /// Can only be called in an initialized Ros2cs state. - /// Topic to subscribe to. Naming restrictions of ros2 apply and violation results in an exception - /// Action to be called when message is received (through Spin or SpinOnce). Provide a lambda or a method - /// Quality of Service settings. Not passing this parameter will result in default settings - /// Subscription for the topic - Subscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new(); + /// + /// Subscriptions created on this node. + /// + IReadOnlyCollection Subscriptions { get; } - /// Remove a publisher - /// Note that this does not call Dispose on Publisher - /// Publisher created with earlier CreatePublisher call - /// Whether removal actually took place. Safe to ignore - bool RemovePublisher(IPublisherBase publisher); + /// Create a client for this node for a given topic, qos and message type + /// Topic for the client. Naming restrictions of ros2 apply and violation results in an exception + /// Quality of Client settings. Not passing this parameter will result in default settings + /// Client for the topic, which can be used to client messages + IClient CreateClient(string topic, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new(); - /// Remove a subscription - /// Note that this does not call Dispose on Subscription. If the caller also does not own - /// the subscription, it can be garbage collected. You can also call Dispose after calling this - /// Subscription created with earlier CreateSubscription call - /// Whether removal actually took place. Safe to ignore - bool RemoveSubscription(ISubscriptionBase subscription); - } + /// + /// Clients created on this node. + /// + IReadOnlyCollection Clients { get; } + + /// Create a service for this node for a given topic, callback, qos and message type + /// Topic to service to. Naming restrictions of ros2 apply and violation results in an exception + /// Action to be called when message is received. Provide a lambda or a method + /// Quality of Service settings. Not passing this parameter will result in default settings + /// Service for the topic + IService CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new(); + + /// + /// Services created on this node. + /// + IReadOnlyCollection Services { get; } + } } diff --git a/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs b/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs index f58e526d..64144d4a 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,25 +12,37 @@ // See the License for the specific language governing permissions and // limitations under the License. -using System; - namespace ROS2 { - /// Non-generic base interface for all subscriptions - /// Use Ros2cs.CreatePublisher to construct. - /// This interface is useful for managing publisher collections and disposal - public interface IPublisherBase: IExtendedDisposable - { - string Topic {get;} - } + /// Non-generic base interface for all publishers. + /// + /// This interface is useful for managing publisher collections and disposal. + /// Create instances with . + /// + public interface IPublisherBase : IExtendedDisposable + { + /// Topic of this publisher. + string Topic { get; } + } + + /// Internal publisher extensions. + internal interface IRawPublisher : IPublisherBase + { + /// Dispose without modifying the node. + void DisposeFromNode(); + } - /// Generic base interface for all subscriptions - public interface IPublisher: IPublisherBase - where T: Message - { - /// Publish a message - /// Message memory is copied into native structures and the message - /// can be safely changed or disposed after this call - void Publish(T msg); - } + /// Generic base interface for all publishers. + /// Message Type to be published. + public interface IPublisher : IPublisherBase + where T : Message + { + /// Publish a message + /// + /// Message memory is copied into native structures and the message + /// can be safely changed or disposed after this call. + /// + /// Message to be published. + void Publish(T msg); + } } diff --git a/src/ros2cs/ros2cs_core/interfaces/IService.cs b/src/ros2cs/ros2cs_core/interfaces/IService.cs index 51275ef4..b146ad8d 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IService.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IService.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,40 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -using System; - namespace ROS2 { - /// Non-generic base interface for all subscriptions - /// - public interface IServiceBase : IExtendedDisposable - { - /// - /// Tries to get a request message from rcl/rmw layers - /// - /// Invokes the callback if successful - // TODO(adamdbrw) this should not be public - add an internal interface - void TakeMessage(); - - /// - /// topic name which was used when calling - /// - string Topic {get;} - - // TODO(adamdbrw) this should not be public - add an internal interface - rcl_service_t Handle {get;} + /// Non-generic base interface for all services. + /// + /// This interface is useful for managing service collections and disposal. + /// Create instances with . + /// + public interface IServiceBase : IExtendedDisposable, IWaitable + { + /// Topic of this service. + string Topic { get; } + } - /// service mutex for internal use - object Mutex { get; } - } + /// Internal service extensions. + internal interface IRawService : IServiceBase + { + /// Dispose without modifying the node. + void DisposeFromNode(); + } - /// Generic base interface for all services - /// Message Type to be received - /// Message Type to be send - /// - public interface IService: IServiceBase - where I: Message - where O: Message - { - } + /// Generic base interface for all services + /// Message Type to be received. + /// Message Type to be send. + public interface IService : IServiceBase + where I : Message + where O : Message + { + } } diff --git a/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs b/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs index f18a732c..06d95363 100644 --- a/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs +++ b/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,27 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -using System; - namespace ROS2 { - /// Non-generic base interface for all subscriptions - /// Use Ros2cs.CreateSubscription to construct - public interface ISubscriptionBase : IExtendedDisposable - { - // TODO(adamdbrw) this should not be public - add an internal interface - void TakeMessage(); - - /// topic name which was used when calling Ros2cs.CreateSubscription - string Topic {get;} - - // TODO(adamdbrw) this should not be public - add an internal interface - rcl_subscription_t Handle {get;} + /// Non-generic base interface for all subscriptions. + /// + /// This interface is useful for managing subscription collections and disposal. + /// Create instances with . + /// + public interface ISubscriptionBase : IExtendedDisposable, IWaitable + { + /// Topic of this subscription. + string Topic { get; } + } - /// subscription mutex for internal use - object Mutex { get; } - } + /// Internal subscription extensions. + internal interface IRawSubscription : ISubscriptionBase + { + /// Dispose without modifying the node. + void DisposeFromNode(); + } - /// Generic base interface for all subscriptions - public interface ISubscription: ISubscriptionBase where T: Message {} + /// Generic base interface for all subscriptions. + /// Message Type to be received. + public interface ISubscription : ISubscriptionBase where T : Message { } } diff --git a/src/ros2cs/ros2cs_tests/src/CreateNodeTest.cs b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs similarity index 50% rename from src/ros2cs/ros2cs_tests/src/CreateNodeTest.cs rename to src/ros2cs/ros2cs_core/interfaces/IWaitable.cs index b112dc87..898feb86 100644 --- a/src/ros2cs/ros2cs_tests/src/CreateNodeTest.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs @@ -1,5 +1,4 @@ -// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) -// Copyright 2019-2021 Robotec.ai +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,30 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -using NUnit.Framework; +using System; -namespace ROS2.Test +namespace ROS2 { - [TestFixture] - public class CreateNodeTest + /// + /// Object which can process becoming ready. + /// + public interface IWaitable { - [SetUp] - public void SetUp() - { - Ros2cs.Init(); - } + /// + /// The handle used for adding to a wait set. + /// + IntPtr Handle { get; } - [TearDown] - public void TearDown() - { - Ros2cs.Shutdown(); - } - - [Test] - public void CreateNode() - { - string nodeName = "create_node_test"; - Ros2cs.CreateNode(nodeName).Dispose(); - } + /// + /// Try to process if this instance is ready. + /// + /// If the instance was ready. + bool TryProcess(); } } diff --git a/src/ros2cs/ros2cs_core/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index 4fbea984..e98aa28e 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -26,15 +26,6 @@ internal static class NativeRcl private static readonly IntPtr nativeRCL = dllLoadUtils.LoadLibraryNoSuffix("rcl"); private static readonly IntPtr nativeRCUtils = dllLoadUtils.LoadLibraryNoSuffix("rcutils"); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate rcl_context_t GetZeroInitializedContextType(); - internal static GetZeroInitializedContextType - rcl_get_zero_initialized_context = - (GetZeroInitializedContextType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_get_zero_initialized_context"), - typeof(GetZeroInitializedContextType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate rcl_init_options_t GetZeroInitializedInitOptionsType(); internal static GetZeroInitializedInitOptionsType @@ -45,7 +36,7 @@ internal static GetZeroInitializedInitOptionsType typeof(GetZeroInitializedInitOptionsType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int InitOptionsInitType(ref rcl_init_options_t init_options, rcl_allocator_t allocator); + internal delegate int InitOptionsInitType([In, Out] ref rcl_init_options_t init_options, rcl_allocator_t allocator); internal static InitOptionsInitType rcl_init_options_init = (InitOptionsInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -54,7 +45,7 @@ internal static InitOptionsInitType typeof(InitOptionsInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ShutdownType(ref rcl_context_t context); + internal delegate int ShutdownType(IntPtr context); internal static ShutdownType rcl_shutdown = (ShutdownType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -63,16 +54,7 @@ internal static ShutdownType typeof(ShutdownType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool ContextIsValidType(ref rcl_context_t context); - internal static ContextIsValidType - rcl_context_is_valid = - (ContextIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_context_is_valid"), - typeof(ContextIsValidType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int InitType(int argc, [In, Out] string[] argv, ref rcl_init_options_t option, ref rcl_context_t context); + internal delegate int InitType(int argc, [In, Out] string[] argv, [In] ref rcl_init_options_t option, IntPtr context); internal static InitType rcl_init = (InitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -81,7 +63,7 @@ internal static InitType typeof(InitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ContextFiniType(ref rcl_context_t context); + internal delegate int ContextFiniType(IntPtr context); internal static ContextFiniType rcl_context_fini = (ContextFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -90,16 +72,7 @@ internal static ContextFiniType typeof(ContextFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate rcl_node_t GetZeroInitializedNodeType(); - internal static GetZeroInitializedNodeType - rcl_get_zero_initialized_node = - (GetZeroInitializedNodeType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_get_zero_initialized_node"), - typeof(GetZeroInitializedNodeType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int NodeInitType(ref rcl_node_t node, string name, string node_namespace, ref rcl_context_t context, IntPtr default_options); + internal delegate int NodeInitType(IntPtr node, string name, string node_namespace, IntPtr context, IntPtr default_options); internal static NodeInitType rcl_node_init = (NodeInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -108,7 +81,7 @@ internal static NodeInitType typeof(NodeInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int NodeFiniType(ref rcl_node_t node); + internal delegate int NodeFiniType(IntPtr node); internal static NodeFiniType rcl_node_fini = (NodeFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -117,7 +90,7 @@ internal static NodeFiniType typeof(NodeFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate IntPtr NodeGetNameType(ref rcl_node_t node); + internal delegate IntPtr NodeGetNameType(IntPtr node); internal static NodeGetNameType rcl_node_get_name = (NodeGetNameType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -126,7 +99,7 @@ internal static NodeGetNameType typeof(NodeGetNameType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate IntPtr NodeGetNamespaceType(ref rcl_node_t node); + internal delegate IntPtr NodeGetNamespaceType(IntPtr node); internal static NodeGetNamespaceType rcl_node_get_namespace = (NodeGetNamespaceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -144,16 +117,7 @@ internal static ClientGetDefaultOptionsType typeof(ClientGetDefaultOptionsType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate rcl_client_t GetZeroInitiazizedClientType(); - internal static GetZeroInitiazizedClientType - rcl_get_zero_initialized_client = - (GetZeroInitiazizedClientType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_get_zero_initialized_client"), - typeof(GetZeroInitiazizedClientType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ClientInitType(ref rcl_client_t client, ref rcl_node_t node, IntPtr type_support_ptr, string topic_name, IntPtr client_options); + internal delegate int ClientInitType(IntPtr client, IntPtr node, IntPtr type_support_ptr, string topic_name, IntPtr client_options); internal static ClientInitType rcl_client_init = (ClientInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -162,7 +126,7 @@ internal static ClientInitType typeof(ClientInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ClientFiniType(ref rcl_client_t client, ref rcl_node_t node); + internal delegate int ClientFiniType(IntPtr client, IntPtr node); internal static ClientFiniType rcl_client_fini = (ClientFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -171,7 +135,7 @@ internal static ClientFiniType typeof(ClientFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int SendRequestType(ref rcl_client_t client, IntPtr message, ref long sequence_number); + internal delegate int SendRequestType(IntPtr client, IntPtr message, out long sequence_number); internal static SendRequestType rcl_send_request = (SendRequestType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -180,7 +144,7 @@ internal static SendRequestType typeof(SendRequestType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int TakeResponceType(ref rcl_client_t client, ref rcl_rmw_request_id_t request_header, IntPtr ros_response); + internal delegate int TakeResponceType(IntPtr client, [In, Out] ref rcl_rmw_request_id_t request_header, IntPtr ros_response); internal static TakeResponceType rcl_take_response = (TakeResponceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -189,7 +153,7 @@ internal static TakeResponceType typeof(TakeResponceType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ServiceIsAvailableType(ref rcl_node_t node, ref rcl_client_t client, ref bool is_available); + internal delegate int ServiceIsAvailableType(IntPtr node, IntPtr client, out bool is_available); internal static ServiceIsAvailableType rcl_service_server_is_available = (ServiceIsAvailableType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -207,16 +171,7 @@ internal static ServiceGetDefaultOptionsType typeof(ServiceGetDefaultOptionsType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate rcl_service_t GetZeroInitiazizedServiceType(); - internal static GetZeroInitiazizedServiceType - rcl_get_zero_initialized_service = - (GetZeroInitiazizedServiceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_get_zero_initialized_service"), - typeof(GetZeroInitiazizedServiceType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ServiceInitType(ref rcl_service_t service, ref rcl_node_t node, IntPtr type_support_ptr, string topic_name, IntPtr service_options); + internal delegate int ServiceInitType(IntPtr service, IntPtr node, IntPtr type_support_ptr, string topic_name, IntPtr service_options); internal static ServiceInitType rcl_service_init = (ServiceInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -225,7 +180,7 @@ internal static ServiceInitType typeof(ServiceInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ServiceFiniType(ref rcl_service_t client, ref rcl_node_t node); + internal delegate int ServiceFiniType(IntPtr service, IntPtr node); internal static ServiceFiniType rcl_service_fini = (ServiceFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -234,7 +189,7 @@ internal static ServiceFiniType typeof(ServiceFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int TakeRequestType(ref rcl_service_t service, ref rcl_rmw_request_id_t request_header, IntPtr message_handle); + internal delegate int TakeRequestType(IntPtr service, [In, Out] ref rcl_rmw_request_id_t request_header, IntPtr message_handle); internal static TakeRequestType rcl_take_request = (TakeRequestType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -243,8 +198,7 @@ internal static TakeRequestType typeof(TakeRequestType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int SendResponceType( ref rcl_service_t service, ref rcl_rmw_request_id_t request_header, IntPtr responce_info); - ///internal delegate int SendResponceType( ref rcl_service_t service, ref rcl_rmw_request_id_t request_header, ref IntPtr responce_info); + internal delegate int SendResponceType(IntPtr service, [In, Out] ref rcl_rmw_request_id_t request_header, IntPtr responce_info); internal static SendResponceType rcl_send_response = (SendResponceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -262,16 +216,7 @@ internal static PublisherGetDefaultOptionsType typeof(PublisherGetDefaultOptionsType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate rcl_publisher_t GetZeroInitiazizedPublisherType(); - internal static GetZeroInitiazizedPublisherType - rcl_get_zero_initialized_publisher = - (GetZeroInitiazizedPublisherType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_get_zero_initialized_publisher"), - typeof(GetZeroInitiazizedPublisherType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int PublisherInitType(ref rcl_publisher_t publisher, ref rcl_node_t node, IntPtr type_support_ptr, string topic_name, IntPtr publisher_options); + internal delegate int PublisherInitType(IntPtr publisher, IntPtr node, IntPtr type_support_ptr, string topic_name, IntPtr publisher_options); internal static PublisherInitType rcl_publisher_init = (PublisherInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -280,7 +225,7 @@ internal static PublisherInitType typeof(PublisherInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int PublisherFiniType(ref rcl_publisher_t publisher, ref rcl_node_t node); + internal delegate int PublisherFiniType(IntPtr publisher, IntPtr node); internal static PublisherFiniType rcl_publisher_fini = (PublisherFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -289,7 +234,7 @@ internal static PublisherFiniType typeof(PublisherFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int PublishType(ref rcl_publisher_t publisher, IntPtr message, IntPtr allocator); + internal delegate int PublishType(IntPtr publisher, IntPtr message, IntPtr allocator); internal static PublishType rcl_publish = (PublishType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -298,16 +243,7 @@ internal static PublishType typeof(PublishType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate rcl_subscription_t GetZeroInitializedSubcriptionType(); - internal static GetZeroInitializedSubcriptionType - rcl_get_zero_initialized_subscription = - (GetZeroInitializedSubcriptionType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_get_zero_initialized_subscription"), - typeof(GetZeroInitializedSubcriptionType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int SubscriptionInitType(ref rcl_subscription_t subscription, ref rcl_node_t node, IntPtr type_support_ptr, string topic_name, IntPtr subscription_options); + internal delegate int SubscriptionInitType(IntPtr subscription, IntPtr node, IntPtr type_support_ptr, string topic_name, IntPtr subscription_options); internal static SubscriptionInitType rcl_subscription_init = (SubscriptionInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -316,7 +252,7 @@ internal static SubscriptionInitType typeof(SubscriptionInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int SubscriptionFiniType(ref rcl_subscription_t subscription, ref rcl_node_t node); + internal delegate int SubscriptionFiniType(IntPtr subscription, IntPtr node); internal static SubscriptionFiniType rcl_subscription_fini = (SubscriptionFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -325,16 +261,7 @@ internal static SubscriptionFiniType typeof(SubscriptionFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool SubscriptionIsValidType(ref rcl_subscription_t subscription); - internal static SubscriptionIsValidType - rcl_subscription_is_valid = - (SubscriptionIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_subscription_is_valid"), - typeof(SubscriptionIsValidType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int TakeType(ref rcl_subscription_t subscription, IntPtr message_handle, IntPtr message_info, IntPtr allocation); + internal delegate int TakeType(IntPtr subscription, IntPtr message_handle, IntPtr message_info, IntPtr allocation); internal static TakeType rcl_take = (TakeType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -343,16 +270,25 @@ internal static TakeType typeof(TakeType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate rcl_wait_set_t GetZeroInitializedWaitSetType(); - internal static GetZeroInitializedWaitSetType - rcl_get_zero_initialized_wait_set = - (GetZeroInitializedWaitSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + internal delegate int GuardConditionTriggerType(IntPtr guard_condition); + internal static GuardConditionTriggerType + rcl_trigger_guard_condition = + (GuardConditionTriggerType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( nativeRCL, - "rcl_get_zero_initialized_wait_set"), - typeof(GetZeroInitializedWaitSetType)); + "rcl_trigger_guard_condition"), + typeof(GuardConditionTriggerType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetResizeType(ref rcl_wait_set_t wait_set, + internal delegate int GuardConditionFiniType(IntPtr guard_condition); + internal static GuardConditionFiniType + rcl_guard_condition_fini = + (GuardConditionFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeRCL, + "rcl_guard_condition_fini"), + typeof(GuardConditionFiniType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int WaitSetResizeType(IntPtr wait_set, UIntPtr number_of_subscriptions, UIntPtr number_of_guard_conditions, UIntPtr number_of_timers, @@ -366,14 +302,14 @@ internal delegate int WaitSetResizeType(ref rcl_wait_set_t wait_set, typeof(WaitSetResizeType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetInitType(ref rcl_wait_set_t wait_set, + internal delegate int WaitSetInitType(IntPtr wait_set, UIntPtr number_of_subscriptions, UIntPtr number_of_guard_conditions, UIntPtr number_of_timers, UIntPtr number_of_clients, UIntPtr number_of_services, UIntPtr number_of_events, - ref rcl_context_t context, + IntPtr context, rcl_allocator_t allocator); internal static WaitSetInitType rcl_wait_set_init = @@ -383,7 +319,7 @@ internal static WaitSetInitType typeof(WaitSetInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WatiSetFiniType(ref rcl_wait_set_t wait_set); + internal delegate int WatiSetFiniType(IntPtr wait_set); internal static WatiSetFiniType rcl_wait_set_fini = (WatiSetFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -392,7 +328,7 @@ internal static WatiSetFiniType typeof(WatiSetFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetClearType(ref rcl_wait_set_t wait_set); + internal delegate int WaitSetClearType(IntPtr wait_set); internal static WaitSetClearType rcl_wait_set_clear = (WaitSetClearType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -401,34 +337,37 @@ internal static WaitSetClearType typeof(WaitSetClearType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetAddSubscriptionType(ref rcl_wait_set_t wait_set, ref rcl_subscription_t subscription, ref UIntPtr index); - internal static WaitSetAddSubscriptionType + internal delegate int WaitSetAddType(IntPtr wait_set, IntPtr value, out UIntPtr index); + internal static WaitSetAddType rcl_wait_set_add_subscription = - (WaitSetAddSubscriptionType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + (WaitSetAddType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( nativeRCL, "rcl_wait_set_add_subscription"), - typeof(WaitSetAddSubscriptionType)); + typeof(WaitSetAddType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetAddClientType(ref rcl_wait_set_t wait_set, ref rcl_client_t client, ref UIntPtr index); - internal static WaitSetAddClientType + internal static WaitSetAddType rcl_wait_set_add_client = - (WaitSetAddClientType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + (WaitSetAddType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( nativeRCL, "rcl_wait_set_add_client"), - typeof(WaitSetAddClientType)); + typeof(WaitSetAddType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetAddServiceType(ref rcl_wait_set_t wait_set, ref rcl_service_t service, ref UIntPtr index); - internal static WaitSetAddServiceType + internal static WaitSetAddType rcl_wait_set_add_service = - (WaitSetAddServiceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + (WaitSetAddType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( nativeRCL, "rcl_wait_set_add_service"), - typeof(WaitSetAddServiceType)); + typeof(WaitSetAddType)); + + internal static WaitSetAddType + rcl_wait_set_add_guard_condition = + (WaitSetAddType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeRCL, + "rcl_wait_set_add_guard_condition"), + typeof(WaitSetAddType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitType(ref rcl_wait_set_t wait_set, long timeout); + internal delegate int WaitType(IntPtr wait_set, long timeout); internal static WaitType rcl_wait = (WaitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -437,7 +376,7 @@ internal static WaitType typeof(WaitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int RclClockGetNow(IntPtr ros_clock, ref long query_now); + internal delegate int RclClockGetNow(IntPtr ros_clock, out long query_now); internal static RclClockGetNow rcl_clock_get_now = (RclClockGetNow)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index 5d97aa31..3524de24 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -28,7 +28,36 @@ internal static class NativeRclInterface private static readonly DllLoadUtils dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); private static readonly IntPtr nativeROS2CS = dllLoadUtils.LoadLibrary("ros2cs"); - internal delegate int RCLCSInitType(ref rcl_context_t context, rcl_allocator_t allocator); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedContextType(); + internal static GetZeroInitializedContextType + rclcs_get_zero_initialized_context = + (GetZeroInitializedContextType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_zero_initialized_context"), + typeof(GetZeroInitializedContextType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreeContextType(IntPtr context); + internal static FreeContextType + rclcs_free_context = + (FreeContextType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_context"), + typeof(FreeContextType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool ContextIsValidType(IntPtr context); + internal static ContextIsValidType + rclcs_context_is_valid = + (ContextIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_context_is_valid"), + typeof(ContextIsValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int RCLCSInitType(IntPtr context, rcl_allocator_t allocator); internal static RCLCSInitType rclcs_init = (RCLCSInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -36,6 +65,34 @@ internal static RCLCSInitType "rclcs_init"), typeof(RCLCSInitType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedNodeType(); + internal static GetZeroInitializedNodeType + rclcs_get_zero_initialized_node = + (GetZeroInitializedNodeType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_zero_initialized_node"), + typeof(GetZeroInitializedNodeType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreeNodeType(IntPtr node); + internal static FreeNodeType + rclcs_free_node = + (FreeNodeType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_node"), + typeof(FreeNodeType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool NodeIsValidType(IntPtr node); + internal static NodeIsValidType + rclcs_node_is_valid = + (NodeIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_node_is_valid"), + typeof(NodeIsValidType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr GetErrorStringType(); internal static GetErrorStringType @@ -72,6 +129,34 @@ internal static NodeDisposeOptionsType "rclcs_node_dispose_options"), typeof(NodeDisposeOptionsType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedSubscriptionType(); + internal static GetZeroInitializedSubscriptionType + rclcs_get_zero_initialized_subscription = + (GetZeroInitializedSubscriptionType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_zero_initialized_subscription"), + typeof(GetZeroInitializedSubscriptionType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreeSubscriptionType(IntPtr subscription); + internal static FreeSubscriptionType + rclcs_free_subscription = + (FreeSubscriptionType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_subscription"), + typeof(FreeSubscriptionType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool SubscriptionIsValidType(IntPtr subscription); + internal static SubscriptionIsValidType + rclcs_subscription_is_valid = + (SubscriptionIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_subscription_is_valid"), + typeof(SubscriptionIsValidType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr SubscriptionCreateOptionsType(IntPtr qos); internal static SubscriptionCreateOptionsType @@ -90,6 +175,34 @@ internal static SubscriptionDisposeOptionsType "rclcs_subscription_dispose_options"), typeof(SubscriptionDisposeOptionsType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedPublisherType(); + internal static GetZeroInitializedPublisherType + rclcs_get_zero_initialized_publisher = + (GetZeroInitializedPublisherType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_zero_initialized_publisher"), + typeof(GetZeroInitializedPublisherType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreePublisherType(IntPtr publisher); + internal static FreePublisherType + rclcs_free_publisher = + (FreePublisherType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_publisher"), + typeof(FreePublisherType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool PublisherIsValidType(IntPtr publisher); + internal static PublisherIsValidType + rclcs_publisher_is_valid = + (PublisherIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_publisher_is_valid"), + typeof(PublisherIsValidType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr PublisherCreateOptionsType(IntPtr qos); internal static PublisherCreateOptionsType @@ -108,6 +221,34 @@ internal static PublisherDisposeOptionsType "rclcs_publisher_dispose_options"), typeof(PublisherDisposeOptionsType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedClientType(); + internal static GetZeroInitializedClientType + rclcs_get_zero_initialized_client = + (GetZeroInitializedClientType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_zero_initialized_client"), + typeof(GetZeroInitializedClientType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreeClientType(IntPtr client); + internal static FreeClientType + rclcs_free_client = + (FreeClientType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_client"), + typeof(FreeClientType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool ClientIsValidType(IntPtr client); + internal static ClientIsValidType + rclcs_client_is_valid = + (ClientIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_client_is_valid"), + typeof(ClientIsValidType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr ClientCreateOptionsType(IntPtr qos); internal static ClientCreateOptionsType @@ -126,6 +267,122 @@ internal static ClientDisposeOptionsType "rclcs_client_dispose_options"), typeof(ClientDisposeOptionsType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedServiceType(); + internal static GetZeroInitializedServiceType + rclcs_get_zero_initialized_service = + (GetZeroInitializedServiceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_zero_initialized_service"), + typeof(GetZeroInitializedServiceType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreeServiceType(IntPtr node); + internal static FreeServiceType + rclcs_free_service = + (FreeServiceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_service"), + typeof(FreeServiceType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool ServiceIsValidType(IntPtr service); + internal static ServiceIsValidType + rclcs_service_is_valid = + (ServiceIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_service_is_valid"), + typeof(ServiceIsValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int GuardConditionInitType(IntPtr context, out IntPtr guard_condition); + internal static GuardConditionInitType + rclcs_get_guard_condition = + (GuardConditionInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_guard_condition"), + typeof(GuardConditionInitType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreeGuardConditionType(IntPtr guard_condition); + internal static FreeGuardConditionType + rclcs_free_guard_condition = + (FreeGuardConditionType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_guard_condition"), + typeof(FreeGuardConditionType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool GuardConditionIsValidType(IntPtr guard_condition); + internal static GuardConditionIsValidType + rclcs_guard_condition_is_valid = + (GuardConditionIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_guard_condition_is_valid"), + typeof(GuardConditionIsValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedWaitSetType(); + internal static GetZeroInitializedWaitSetType + rclcs_get_zero_initialized_wait_set = + (GetZeroInitializedWaitSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_get_zero_initialized_wait_set"), + typeof(GetZeroInitializedWaitSetType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void FreeWaitSetType(IntPtr waitSet); + internal static FreeWaitSetType + rclcs_free_wait_set = + (FreeWaitSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_wait_set"), + typeof(FreeWaitSetType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool WaitSetIsValidType(IntPtr waitSet); + internal static WaitSetIsValidType + rclcs_wait_set_is_valid = + (WaitSetIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_is_valid"), + typeof(WaitSetIsValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool WaitSetGetType(IntPtr waitSet, UIntPtr index, out IntPtr value); + + internal static WaitSetGetType + rclcs_wait_set_get_subscription = + (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_get_subscription"), + typeof(WaitSetGetType)); + + internal static WaitSetGetType + rclcs_wait_set_get_client = + (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_get_client"), + typeof(WaitSetGetType)); + + internal static WaitSetGetType + rclcs_wait_set_get_service = + (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_get_service"), + typeof(WaitSetGetType)); + + internal static WaitSetGetType + rclcs_wait_set_get_guard_condition = + (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_get_guard_condition"), + typeof(WaitSetGetType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr ServiceCreateOptionsType(IntPtr qos); internal static ServiceCreateOptionsType diff --git a/src/ros2cs/ros2cs_core/native/NativeTypes.cs b/src/ros2cs/ros2cs_core/native/NativeTypes.cs index 3a06787f..7d45e6fa 100644 --- a/src/ros2cs/ros2cs_core/native/NativeTypes.cs +++ b/src/ros2cs/ros2cs_core/native/NativeTypes.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -42,13 +42,6 @@ public struct rcl_arguments_t private IntPtr impl; } - public struct rcl_context_t -{ - private IntPtr global_arguments; - private IntPtr impl; - private IntPtr instance_id_storage; - } - public struct rcl_error_string_t { internal IntPtr str; @@ -59,32 +52,6 @@ public struct rcl_init_options_t private IntPtr impl; } - public struct rcl_node_t - { - private IntPtr context; - private IntPtr rcl_node_impl_t; - } - - public struct rcl_publisher_t - { - private IntPtr impl; - } - - public struct rcl_subscription_t - { - private IntPtr impl; - } - - public struct rcl_client_t - { - private IntPtr impl; - } - - public struct rcl_service_t - { - private IntPtr impl; - } - [StructLayout(LayoutKind.Sequential)] public struct rcl_rmw_request_id_t { @@ -96,23 +63,6 @@ public struct rcl_rmw_request_id_t public long sequence_number; }; - public struct rcl_wait_set_t - { - private IntPtr subscriptions; - internal UIntPtr size_of_subscriptions; - private IntPtr guard_conditions; - internal UIntPtr size_of_guard_conditions; - private IntPtr timers; - internal UIntPtr size_of_timers; - private IntPtr clients; - internal UIntPtr size_of_clients; - private IntPtr services; - internal UIntPtr size_of_services; - private IntPtr events; - internal UIntPtr size_of_events; - private IntPtr impl; - } - public struct rcl_clock_t { private int type; diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index 7a774642..3bd2853b 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,17 @@ // 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 #include #include #include @@ -21,6 +30,31 @@ #include #include +ROSIDL_GENERATOR_C_EXPORT +rcl_context_t * rclcs_get_zero_initialized_context() +{ + rcl_context_t * context = malloc(sizeof(rcl_context_t)); + *context = rcl_get_zero_initialized_context(); + return context; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_context(rcl_context_t * context) +{ + free(context); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_context_is_valid(rcl_context_t * context) +{ + // since bool has different sizes in C and C++ + if (rcl_context_is_valid(context)) + { + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT int rclcs_init(rcl_context_t *context, rcl_allocator_t allocator) { @@ -41,6 +75,31 @@ int rclcs_init(rcl_context_t *context, rcl_allocator_t allocator) return ret; } +ROSIDL_GENERATOR_C_EXPORT +rcl_node_t * rclcs_get_zero_initialized_node() +{ + rcl_node_t * node = malloc(sizeof(rcl_node_t)); + *node = rcl_get_zero_initialized_node(); + return node; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_node(rcl_node_t * node) +{ + free(node); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_node_is_valid(rcl_node_t * node) +{ + // since bool has different sizes in C and C++ + if (rcl_node_is_valid(node)) + { + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT rcl_node_options_t * rclcs_node_create_default_options() { @@ -55,6 +114,31 @@ void rclcs_node_dispose_options(rcl_node_options_t * node_options_handle) free(node_options_handle); } +ROSIDL_GENERATOR_C_EXPORT +rcl_subscription_t * rclcs_get_zero_initialized_subscription() +{ + rcl_subscription_t * subscription = malloc(sizeof(rcl_subscription_t)); + *subscription = rcl_get_zero_initialized_subscription(); + return subscription; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_subscription(rcl_subscription_t * subscription) +{ + free(subscription); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_subscription_is_valid(rcl_subscription_t * subscription) +{ + // since bool has different sizes in C and C++ + if (rcl_subscription_is_valid(subscription)) + { + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT rcl_subscription_options_t *rclcs_subscription_create_options(rmw_qos_profile_t * qos) { @@ -70,6 +154,31 @@ void rclcs_subscription_dispose_options(rcl_subscription_options_t *subscription free(subscription_options_handle); } +ROSIDL_GENERATOR_C_EXPORT +rcl_publisher_t * rclcs_get_zero_initialized_publisher() +{ + rcl_publisher_t * publisher = malloc(sizeof(rcl_publisher_t)); + *publisher = rcl_get_zero_initialized_publisher(); + return publisher; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_publisher(rcl_publisher_t * publisher) +{ + free(publisher); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_publisher_is_valid(rcl_publisher_t * publisher) +{ + // since bool has different sizes in C and C++ + if (rcl_publisher_is_valid(publisher)) + { + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT rcl_publisher_options_t *rclcs_publisher_create_options(rmw_qos_profile_t * qos) { @@ -85,6 +194,31 @@ void rclcs_publisher_dispose_options(rcl_publisher_options_t * publisher_options free(publisher_options_handle); } +ROSIDL_GENERATOR_C_EXPORT +rcl_client_t * rclcs_get_zero_initialized_client() +{ + rcl_client_t * client = malloc(sizeof(rcl_client_t)); + *client = rcl_get_zero_initialized_client(); + return client; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_client(rcl_client_t * client) +{ + free(client); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_client_is_valid(rcl_client_t * client) +{ + // since bool has different sizes in C and C++ + if (rcl_client_is_valid(client)) + { + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT rcl_client_options_t *rclcs_client_create_options(rmw_qos_profile_t * qos) { @@ -100,6 +234,31 @@ void rclcs_client_dispose_options(rcl_client_options_t * client_options_handle) free(client_options_handle); } +ROSIDL_GENERATOR_C_EXPORT +rcl_service_t * rclcs_get_zero_initialized_service() +{ + rcl_service_t * service = malloc(sizeof(rcl_service_t)); + *service = rcl_get_zero_initialized_service(); + return service; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_service(rcl_service_t * service) +{ + free(service); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_service_is_valid(rcl_service_t * service) +{ + // since bool has different sizes in C and C++ + if (rcl_service_is_valid(service)) + { + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT rcl_service_options_t *rclcs_service_create_options(rmw_qos_profile_t * qos) { @@ -115,6 +274,105 @@ void rclcs_service_dispose_options(rcl_service_options_t * service_options_handl free(service_options_handle); } +ROSIDL_GENERATOR_C_EXPORT +rcl_ret_t rclcs_get_guard_condition(rcl_context_t * context, rcl_guard_condition_t ** guard_condition) +{ + *guard_condition = malloc(sizeof(rcl_guard_condition_t)); + **guard_condition = rcl_get_zero_initialized_guard_condition(); + rcl_ret_t ret = rcl_guard_condition_init(*guard_condition, context, rcl_guard_condition_get_default_options()); + if (ret != RCL_RET_OK) + { + free(*guard_condition); + } + return ret; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_guard_condition(rcl_guard_condition_t * guard_condition) +{ + free(guard_condition); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_guard_condition_is_valid(rcl_guard_condition_t * guard_condition) +{ + // since there is no rcl_guard_condition_is_valid + if (rcl_guard_condition_get_options(guard_condition) != NULL) + { + return 1; + } + return 0; +} + +ROSIDL_GENERATOR_C_EXPORT +rcl_wait_set_t * rclcs_get_zero_initialized_wait_set() +{ + rcl_wait_set_t * wait_set = malloc(sizeof(rcl_wait_set_t)); + *wait_set = rcl_get_zero_initialized_wait_set(); + return wait_set; +} + +ROSIDL_GENERATOR_C_EXPORT +void rclcs_free_wait_set(rcl_wait_set_t * wait_set) +{ + free(wait_set); +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_wait_set_is_valid(rcl_wait_set_t * wait_set) +{ + // since bool has different sizes in C and C++ + if (rcl_wait_set_is_valid(wait_set)) + { + return 1; + } + return 0; +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_wait_set_get_subscription(rcl_wait_set_t * wait_set, size_t index, const rcl_subscription_t ** subscription) +{ + if (index < wait_set->size_of_subscriptions) + { + *subscription = wait_set->subscriptions[index]; + return 1; + } + return 0; +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_wait_set_get_client(rcl_wait_set_t * wait_set, size_t index, const rcl_client_t ** client) +{ + if (index < wait_set->size_of_clients) + { + *client = wait_set->clients[index]; + return 1; + } + return 0; +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_wait_set_get_service(rcl_wait_set_t * wait_set, size_t index, const rcl_service_t ** service) +{ + if (index < wait_set->size_of_services) + { + *service = wait_set->services[index]; + return 1; + } + return 0; +} + +ROSIDL_GENERATOR_C_EXPORT +uint8_t rclcs_wait_set_get_guard_condition(rcl_wait_set_t * wait_set, size_t index, const rcl_guard_condition_t ** guard_condition) +{ + if (index < wait_set->size_of_guard_conditions) + { + *guard_condition = wait_set->guard_conditions[index]; + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT char * rclcs_get_error_string() { diff --git a/src/ros2cs/ros2cs_core/utils/LockedCollection.cs b/src/ros2cs/ros2cs_core/utils/LockedCollection.cs new file mode 100644 index 00000000..3f91c268 --- /dev/null +++ b/src/ros2cs/ros2cs_core/utils/LockedCollection.cs @@ -0,0 +1,66 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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.Collections; +using System.Collections.Generic; +using System.Linq; + +namespace ROS2 +{ + /// + /// Collection view which locks the the wrapped collection using an object as lock. + /// + internal sealed class LockedCollection : IReadOnlyCollection + { + private readonly IReadOnlyCollection Wrapped; + + private readonly object Lock; + + /// + public int Count + { + get + { + lock (this.Lock) + { + return this.Wrapped.Count; + } + } + } + + public LockedCollection(IReadOnlyCollection wrapped) : this(wrapped, wrapped) + {} + + public LockedCollection(IReadOnlyCollection wrapped, object _lock) + { + this.Wrapped = wrapped; + this.Lock = _lock; + } + + /// + public IEnumerator GetEnumerator() + { + lock (this.Lock) + { + return this.Wrapped.ToArray().AsEnumerable().GetEnumerator(); + } + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs b/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs new file mode 100644 index 00000000..f99dfebe --- /dev/null +++ b/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs @@ -0,0 +1,120 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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.Collections; +using System.Collections.Generic; +using System.Linq; + +namespace ROS2 +{ + /// + /// Dictionary view which locks the the wrapped dictionary using an object as lock. + /// + internal sealed class LockedDictionary : IReadOnlyDictionary + { + private readonly IReadOnlyDictionary Wrapped; + + private readonly object Lock; + + public LockedDictionary(IReadOnlyDictionary wrapped) : this(wrapped, wrapped) + { } + + public LockedDictionary(IReadOnlyDictionary wrapped, object _lock) + { + this.Wrapped = wrapped; + this.Lock = _lock; + } + + /// + public V this[K key] + { + get + { + lock (this.Lock) + { + return this.Wrapped[key]; + } + } + } + + /// + public IEnumerable Keys + { + get + { + lock (this.Lock) + { + return this.Wrapped.Keys.ToArray(); + } + } + } + + /// + public IEnumerable Values + { + get + { + lock (this.Lock) + { + return this.Wrapped.Values.ToArray(); + } + } + } + + /// + public int Count + { + get + { + lock (this.Lock) + { + return this.Wrapped.Count; + } + } + } + + /// + public bool ContainsKey(K key) + { + lock (this.Lock) + { + return this.Wrapped.ContainsKey(key); + } + } + + /// + public bool TryGetValue(K key, out V value) + { + lock (this.Lock) + { + return this.Wrapped.TryGetValue(key, out value); + } + } + + /// + public IEnumerator> GetEnumerator() + { + lock (this.Lock) + { + return this.Wrapped.ToArray().AsEnumerable().GetEnumerator(); + } + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + } +} diff --git a/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs b/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs new file mode 100644 index 00000000..d1c42349 --- /dev/null +++ b/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs @@ -0,0 +1,96 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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; +using System.Collections.Generic; +using System.Linq; + +namespace ROS2 +{ + /// + /// Dictionary view which transforms the values of the wrapped dictionary. + /// + internal sealed class MappedValueDictionary : IReadOnlyDictionary + { + private readonly IReadOnlyDictionary Wrapped; + + private readonly Func Mapper; + + public MappedValueDictionary(IReadOnlyDictionary wrapped, Func mapper) + { + this.Wrapped = wrapped; + this.Mapper = mapper; + } + + /// + public V this[K key] + { + get { return this.Mapper(this.Wrapped[key]); } + } + + /// + public IEnumerable Keys + { + get { return this.Wrapped.Keys; } + } + + /// + public IEnumerable Values + { + get { return this.Wrapped.Values.Select(this.Mapper); } + } + + /// + public int Count + { + get { return this.Wrapped.Count; } + } + + /// + public bool ContainsKey(K key) + { + return this.Wrapped.ContainsKey(key); + } + + /// + public bool TryGetValue(K key, out V value) + { + if (this.Wrapped.TryGetValue(key, out T originalValue)) + { + value = this.Mapper(originalValue); + return true; + } + else + { + value = default(V); + return false; + } + } + + /// + public IEnumerator> GetEnumerator() + { + return this.Wrapped + .Select(pair => new KeyValuePair(pair.Key, this.Mapper(pair.Value))) + .GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + } +} diff --git a/src/ros2cs/ros2cs_core/utils/Utils.cs b/src/ros2cs/ros2cs_core/utils/Utils.cs index 7d747e41..e1f16ee3 100644 --- a/src/ros2cs/ros2cs_core/utils/Utils.cs +++ b/src/ros2cs/ros2cs_core/utils/Utils.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,6 +35,14 @@ internal static void CheckReturnEnum(int ret) throw new InvalidNamespaceException(errorMessage); case RCLReturnEnum.RCL_RET_WAIT_SET_EMPTY: throw new WaitSetEmptyException(errorMessage); + case RCLReturnEnum.RCL_RET_NOT_INIT: + case RCLReturnEnum.RCL_RET_NODE_INVALID: + case RCLReturnEnum.RCL_RET_PUBLISHER_INVALID: + case RCLReturnEnum.RCL_RET_SUBSCRIPTION_INVALID: + case RCLReturnEnum.RCL_RET_CLIENT_INVALID: + case RCLReturnEnum.RCL_RET_SERVICE_INVALID: + case RCLReturnEnum.RCL_RET_WAIT_SET_INVALID: + throw new ObjectDisposedException(errorMessage); default: throw new RuntimeError(errorMessage); } diff --git a/src/ros2cs/ros2cs_examples/ROS2Client.cs b/src/ros2cs/ros2cs_examples/ROS2Client.cs index e9869e65..c804e424 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Client.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Client.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,38 +14,42 @@ using System; +using System.Collections; using System.Threading; +using System.Threading.Tasks; using ROS2; -using std_msgs; -using sensor_msgs; -using example_interfaces; +using ROS2.Executors; +using example_interfaces.srv; namespace Examples { - /// A simple service client class to illustrate Ros2cs in action - public class ROS2Client - { - public static void Main(string[] args) + /// A simple service client class to illustrate Ros2cs in action + public class ROS2Client { - Console.WriteLine("Client start"); - Ros2cs.Init(); - INode node = Ros2cs.CreateNode("talker"); - Client my_client = node.CreateClient("add_two_ints"); + public static void Main(string[] args) + { + Console.WriteLine("Client start"); - example_interfaces.srv.AddTwoInts_Request msg = new example_interfaces.srv.AddTwoInts_Request(); - msg.A = 7; - msg.B = 2; + // everything is disposed when disposing the context + using Context context = new Context(); + using ManualExecutor executor = new ManualExecutor(context); + context.TryCreateNode("client", out INode node); + executor.Add(node); - while (!my_client.IsServiceAvailable()) - { - Thread.Sleep(TimeSpan.FromSeconds(0.25)); - } + IClient my_client = node.CreateClient("add_two_ints"); + AddTwoInts_Request msg = new AddTwoInts_Request(); + msg.A = 7; + msg.B = 2; - example_interfaces.srv.AddTwoInts_Response rsp = my_client.Call(msg); - Console.WriteLine("Sum = " + rsp.Sum); + while (!my_client.IsServiceAvailable()) + { + Thread.Sleep(TimeSpan.FromSeconds(0.25)); + } - Console.WriteLine("Client shutdown"); - Ros2cs.Shutdown(); + Task rsp = my_client.CallAsync(msg); + executor.SpinWhile(() => !rsp.IsCompleted); + + Console.WriteLine("Sum = {0}", rsp.Result.Sum); + } } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2Listener.cs b/src/ros2cs/ros2cs_examples/ROS2Listener.cs index 51e0d376..e7027735 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Listener.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Listener.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,23 +13,30 @@ // limitations under the License. using System; +using System.Collections; using ROS2; +using ROS2.Executors; namespace Examples { - /// A simple listener class to illustrate Ros2cs in action - public class ROS2Listener - { - public static void Main(string[] args) + /// A simple listener class to illustrate Ros2cs in action + public class ROS2Listener { - Ros2cs.Init(); - INode node = Ros2cs.CreateNode("listener"); + public static void Main(string[] args) + { + Console.WriteLine("Listener starting"); - ISubscription chatter_sub = node.CreateSubscription( - "chatter", msg => Console.WriteLine("I heard: [" + msg.Data + "]")); + // everything is disposed when disposing the context + using Context context = new Context(); + using ManualExecutor executor = new ManualExecutor(context); + context.TryCreateNode("listener", out INode node); + executor.Add(node); + ISubscription chatter_sub = node.CreateSubscription( + "chatter", + msg => Console.WriteLine($"I heard: [{msg.Data}]") + ); - Ros2cs.Spin(node); - Ros2cs.Shutdown(); + executor.SpinWhile(() => true); + } } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs b/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs index 304eeb81..a2bc09a3 100644 --- a/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs +++ b/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,113 +13,119 @@ // limitations under the License. using System; -using ROS2; using System.Collections; using System.Collections.Concurrent; +using ROS2; +using ROS2.Executors; +using sensor_msgs.msg; namespace Examples { - public class FixedSizedQueue : ConcurrentQueue - { - public struct InfoStruct + public class FixedSizedQueue : ConcurrentQueue { - public double stdDev; - public double mean; - } + public struct InfoStruct + { + public double stdDev; + public double mean; + } - private readonly object syncObject = new object(); - private InfoStruct result = new InfoStruct(); + private readonly object syncObject = new object(); + private InfoStruct result = new InfoStruct(); - public int Size { get; private set; } + public int Size { get; private set; } - public FixedSizedQueue(int size) - { - Size = size; - } + public FixedSizedQueue(int size) + { + Size = size; + } - public double Avg() - { - double sum = 0.0; - foreach (double diff in this) - { - sum += diff; - } - return (double)(sum/this.Size); - } + public double Avg() + { + double sum = 0.0; + foreach (double diff in this) + { + sum += diff; + } + return (double)(sum / this.Size); + } - public InfoStruct MeanAndStdDev() - { - var variance = 0.0; - lock (syncObject) - { - var mean = this.Avg(); - foreach (double diff in this) + public InfoStruct MeanAndStdDev() { - variance += (diff - mean) * (diff - mean); + var variance = 0.0; + lock (syncObject) + { + var mean = this.Avg(); + foreach (double diff in this) + { + variance += (diff - mean) * (diff - mean); + } + result.mean = mean; + result.stdDev = Math.Sqrt((double)(1.0 / (this.Size - 1)) * variance); + return result; + } } - result.mean = mean; - result.stdDev = Math.Sqrt((double)(1.0/(this.Size-1)) * variance); - return result; - } - } - public new void Enqueue(double obj) - { - base.Enqueue(obj); - lock (syncObject) - { - while (base.Count > Size) + public new void Enqueue(double obj) { - double outObj; - base.TryDequeue(out outObj); + base.Enqueue(obj); + lock (syncObject) + { + while (base.Count > Size) + { + double outObj; + base.TryDequeue(out outObj); + } + } } - } } - } - /// A listener class meant to gauge performance of Ros2cs - public class ROS2PerformanceListener - { - public static void Main(string[] args) + /// A listener class meant to gauge performance of Ros2cs + public class ROS2PerformanceListener { - Ros2cs.Init(); - Clock clock = new Clock(); - INode node = Ros2cs.CreateNode("perf_listener"); - Console.WriteLine("Enter sample size: "); - int sampleSize = Convert.ToInt32(Console.ReadLine()); - Console.Clear(); - Console.WriteLine("Waiting for {0} messages...", sampleSize); - FixedSizedQueue queue = new FixedSizedQueue(sampleSize); + public static void Main(string[] args) + { + using Context context = new Context(); + using ManualExecutor executor = new ManualExecutor(context); + context.TryCreateNode("perf_listener", out INode node); + executor.Add(node); + bool exitScheduled = false; - RosTime timeStamp = new RosTime(); - int counter = 0; + using Clock clock = new Clock(); + Console.WriteLine("Enter sample size: "); + int sampleSize = Convert.ToInt32(Console.ReadLine()); + Console.Clear(); + Console.WriteLine("Waiting for {0} messages...", sampleSize); + FixedSizedQueue queue = new FixedSizedQueue(sampleSize); - QualityOfServiceProfile qos = new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA); - ISubscription chatter_sub = node.CreateSubscription( - "perf_chatter", - msg => - { - RosTime timeNow = clock.Now; - timeStamp.nanosec = msg.Header.Stamp.Nanosec; - timeStamp.sec = msg.Header.Stamp.Sec; - var diff = timeNow.Seconds - timeStamp.Seconds; + RosTime timeStamp = new RosTime(); + int counter = 0; - queue.Enqueue(diff); - counter++; + ISubscription chatter_sub = node.CreateSubscription( + "perf_chatter", + msg => + { + RosTime timeNow = clock.Now; + timeStamp.nanosec = msg.Header.Stamp.Nanosec; + timeStamp.sec = msg.Header.Stamp.Sec; + var diff = timeNow.Seconds - timeStamp.Seconds; - if (counter == queue.Size) - { - counter = 0; - Console.Clear(); - var result = queue.MeanAndStdDev(); - Console.WriteLine("Latency of sample size {0} - avg: {1:F6}s, std dev: {2:F10}s", sampleSize, result.mean, result.stdDev); - Environment.Exit(0); - } - }, - qos); + queue.Enqueue(diff); + counter++; - Ros2cs.Spin(node); - Ros2cs.Shutdown(); + if (counter == queue.Size) + { + counter = 0; + Console.Clear(); + var result = queue.MeanAndStdDev(); + Console.WriteLine("Latency of sample size {0} - avg: {1:F6}s, std dev: {2:F10}s", sampleSize, result.mean, result.stdDev); + exitScheduled = true; + executor.Interrupt(); + } + }, + new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA) + ); + + executor.SpinWhile(() => !exitScheduled); + } } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs b/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs index 7acf5f9d..726877f9 100644 --- a/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs +++ b/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,90 +15,91 @@ using System; using System.Threading; using ROS2; +using sensor_msgs.msg; namespace Examples { - /// A talker class meant to gauge performance of Ros2cs - public class ROS2PerformanceTalker - { - private static Clock clock = new Clock(); - - private static void AssignField(ref sensor_msgs.msg.PointField pf, string n, uint off, byte dt, uint count) + /// A talker class meant to gauge performance of Ros2cs + public class ROS2PerformanceTalker { - pf.Name = n; - pf.Offset = off; - pf.Datatype = dt; - pf.Count = count; - } + private static Clock clock = new Clock(); - private static sensor_msgs.msg.PointCloud2 PrepMessage(int messageSize) - { - uint count = (uint)messageSize; //point per message - uint fieldsSize = 16; - uint rowSize = count * fieldsSize; - sensor_msgs.msg.PointCloud2 message = new sensor_msgs.msg.PointCloud2() - { - Height = 1, - Width = count, - Is_bigendian = false, - Is_dense = true, - Point_step = fieldsSize, - Row_step = rowSize, - Data = new byte[rowSize * 1] - }; - uint pointFieldCount = 4; - message.Fields = new sensor_msgs.msg.PointField[pointFieldCount]; - for (int i = 0; i < pointFieldCount; ++i) - { - message.Fields[i] = new sensor_msgs.msg.PointField(); - } + private static void AssignField(ref PointField pf, string n, uint off, byte dt, uint count) + { + pf.Name = n; + pf.Offset = off; + pf.Datatype = dt; + pf.Count = count; + } - AssignField(ref message.Fields[0], "x", 0, 7, 1); - AssignField(ref message.Fields[1], "y", 4, 7, 1); - AssignField(ref message.Fields[2], "z", 8, 7, 1); - AssignField(ref message.Fields[3], "intensity", 12, 7, 1); - float[] pointsArray = new float[count * message.Fields.Length]; + private static sensor_msgs.msg.PointCloud2 PrepMessage(int messageSize) + { + uint count = (uint)messageSize; //point per message + uint fieldsSize = 16; + uint rowSize = count * fieldsSize; + PointCloud2 message = new PointCloud2() + { + Height = 1, + Width = count, + Is_bigendian = false, + Is_dense = true, + Point_step = fieldsSize, + Row_step = rowSize, + Data = new byte[rowSize * 1] + }; + uint pointFieldCount = 4; + message.Fields = new PointField[pointFieldCount]; + for (int i = 0; i < pointFieldCount; ++i) + { + message.Fields[i] = new PointField(); + } - var floatIndex = 0; - for (int i = 0; i < count; ++i) - { - float intensity = 100; - pointsArray[floatIndex++] = 1; - pointsArray[floatIndex++] = 2; - pointsArray[floatIndex++] = 3; - pointsArray[floatIndex++] = intensity; - } - System.Buffer.BlockCopy(pointsArray, 0, message.Data, 0, message.Data.Length); - message.SetHeaderFrame("pc"); - return message; - } + AssignField(ref message.Fields[0], "x", 0, 7, 1); + AssignField(ref message.Fields[1], "y", 4, 7, 1); + AssignField(ref message.Fields[2], "z", 8, 7, 1); + AssignField(ref message.Fields[3], "intensity", 12, 7, 1); + float[] pointsArray = new float[count * message.Fields.Length]; - public static void Main(string[] args) - { - Ros2cs.Init(); - INode node = Ros2cs.CreateNode("perf_talker"); - QualityOfServiceProfile qos = new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA); - IPublisher pc_pub = node.CreatePublisher("perf_chatter", qos); + var floatIndex = 0; + for (int i = 0; i < count; ++i) + { + float intensity = 100; + pointsArray[floatIndex++] = 1; + pointsArray[floatIndex++] = 2; + pointsArray[floatIndex++] = 3; + pointsArray[floatIndex++] = intensity; + } + System.Buffer.BlockCopy(pointsArray, 0, message.Data, 0, message.Data.Length); + message.SetHeaderFrame("pc"); + return message; + } + + public static void Main(string[] args) + { + using IContext context = new Context(); + context.TryCreateNode("perf_talker", out INode node); - Console.WriteLine("Enter PC2 data size: "); - int messageSize = Convert.ToInt32(Console.ReadLine()); - sensor_msgs.msg.PointCloud2 msg = PrepMessage(messageSize); - // System.Random rand = new System.Random(); + IPublisher pc_pub = node.CreatePublisher( + "perf_chatter", + new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA) + ); - while (Ros2cs.Ok()) - { - var nowTime = clock.Now; - msg.UpdateHeaderTime(nowTime.sec, nowTime.nanosec); + Console.WriteLine("Enter PC2 data size: "); + sensor_msgs.msg.PointCloud2 msg = PrepMessage(Convert.ToInt32(Console.ReadLine())); - // Remove this benchmark if you want to measure maximum throughput for smallest messages - using (var bench = new Benchmark("Publish")) - { - // If we want to test changing sizes: - // msg = PrepMessage(rand.Next() / 1000); - pc_pub.Publish(msg); + while (context.Ok()) + { + var nowTime = clock.Now; + msg.UpdateHeaderTime(nowTime.sec, nowTime.nanosec); + + // Remove this benchmark if you want to measure maximum throughput for smallest messages + using (var bench = new Benchmark("Publish")) + { + // If we want to test changing sizes: + // msg = PrepMessage(rand.Next() / 1000); + pc_pub.Publish(msg); + } + } } - } - Ros2cs.Shutdown(); } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2Service.cs b/src/ros2cs/ros2cs_examples/ROS2Service.cs index daab7297..2e36350d 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Service.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Service.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,34 +14,38 @@ using System; -using System.Collections.Generic; +using System.Collections; using ROS2; +using ROS2.Executors; +using example_interfaces.srv; namespace Examples { - /// A simple service class to illustrate Ros2cs in action - public class ROS2Service - { - public static IService my_service; - - public static void Main(string[] args) + /// A simple service class to illustrate Ros2cs in action + public class ROS2Service { - Console.WriteLine("Service start"); - Ros2cs.Init(); - INode node = Ros2cs.CreateNode("service"); - my_service = node.CreateService( - "add_two_ints", recv_callback); + public static void Main(string[] args) + { + Console.WriteLine("Service start"); - Ros2cs.Spin(node); - Ros2cs.Shutdown(); - } + // everything is disposed when disposing the context + using Context context = new Context(); + using ManualExecutor executor = new ManualExecutor(context); + context.TryCreateNode("service", out INode node); + executor.Add(node); - public static example_interfaces.srv.AddTwoInts_Response recv_callback( example_interfaces.srv.AddTwoInts_Request msg ) - { - Console.WriteLine ("Incoming Service Request A=" + msg.A + " B=" + msg.B); - example_interfaces.srv.AddTwoInts_Response response = new example_interfaces.srv.AddTwoInts_Response(); - response.Sum = msg.A + msg.B; - return response; + IService my_service = node.CreateService( + "add_two_ints", + msg => + { + Console.WriteLine("Incoming Service Request A={0} B={1}", msg.A, msg.B); + AddTwoInts_Response response = new AddTwoInts_Response(); + response.Sum = msg.A + msg.B; + return response; + } + ); + + executor.SpinWhile(() => true); + } } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2Talker.cs b/src/ros2cs/ros2cs_examples/ROS2Talker.cs index afaa93a3..78e08802 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Talker.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Talker.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,32 +16,29 @@ using System; using System.Threading; using ROS2; -using std_msgs; namespace Examples { - /// A simple talker class to illustrate Ros2cs in action - public class ROS2Talker - { - public static void Main(string[] args) + /// A simple talker class to illustrate Ros2cs in action + public class ROS2Talker { - Console.WriteLine("Talker starting"); - Ros2cs.Init(); - INode node = Ros2cs.CreateNode("talker"); - Publisher chatter_pub = node.CreatePublisher("chatter"); - std_msgs.msg.String msg = new std_msgs.msg.String(); + public static void Main(string[] args) + { + Console.WriteLine("Talker starting"); - int i = 1; + // everything is disposed when disposing the context + using IContext context = new Context(); + context.TryCreateNode("talker", out INode node); + IPublisher chatter_pub = node.CreatePublisher("chatter"); + std_msgs.msg.String msg = new std_msgs.msg.String(); - while (Ros2cs.Ok()) - { - Thread.Sleep(1000); //1s - msg.Data = "Hello World: " + i; - i++; - Console.WriteLine(msg.Data); - chatter_pub.Publish(msg); - } - Ros2cs.Shutdown(); + for (int i = 1; context.Ok(); i++) + { + Thread.Sleep(TimeSpan.FromSeconds(1)); + msg.Data = $"Hello World: {i}"; + Console.WriteLine(msg.Data); + chatter_pub.Publish(msg); + } + } } - } } diff --git a/src/ros2cs/ros2cs_tests/CMakeLists.txt b/src/ros2cs/ros2cs_tests/CMakeLists.txt index ca42a0c9..9b84b2a0 100644 --- a/src/ros2cs/ros2cs_tests/CMakeLists.txt +++ b/src/ros2cs/ros2cs_tests/CMakeLists.txt @@ -1,3 +1,17 @@ +# Copyright 2019-2023 Robotec.ai +# +# 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. + cmake_minimum_required(VERSION 3.6) project(ros2cs_tests) @@ -29,8 +43,8 @@ if(BUILD_TESTING) set(TESTS_SRC src/ClockTest.cs - src/CreateNodeTest.cs - src/InitShutdownTest.cs + src/ContextTest.cs + src/PublisherTest.cs src/LargeMessageTest.cs src/MessagesTest.cs src/NodeTest.cs @@ -39,6 +53,10 @@ if(BUILD_TESTING) src/ServiceTest.cs src/SubscriptionTest.cs src/TestUtils.cs + src/WaitSetTest.cs + src/GuardConditionTest.cs + src/ManualExecutorTest.cs + src/TaskExecutorTest.cs ) add_dotnet_test(ros2cs_tests diff --git a/src/ros2cs/ros2cs_tests/src/ClientTest.cs b/src/ros2cs/ros2cs_tests/src/ClientTest.cs index 5e2a0362..e509ef66 100644 --- a/src/ros2cs/ros2cs_tests/src/ClientTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ClientTest.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -26,6 +26,8 @@ public class ClientTest { private static readonly string SERVICE_NAME = "test_service"; + private Context Context; + private INode Node; private IClient Client; @@ -33,16 +35,15 @@ public class ClientTest [SetUp] public void SetUp() { - Ros2cs.Init(); - Node = Ros2cs.CreateNode("service_test_node"); + Context = new Context(); + Context.TryCreateNode("service_test_node", out Node); Client = Node.CreateClient(SERVICE_NAME); } [TearDown] public void TearDown() { - Node.Dispose(); - Ros2cs.Shutdown(); + Context.Dispose(); } private AddTwoInts_Request CreateRequest(int a, int b) @@ -70,7 +71,8 @@ public void ClientCallAsync() var task = Client.CallAsync(CreateRequest(42, 3)); while (!task.IsCompleted) { - Ros2cs.SpinOnce(Node, 0.1); + service.TryProcess(); + Client.TryProcess(); } Assert.That(task.Result.Sum, Is.EqualTo(45)); } @@ -88,7 +90,8 @@ public void ClientCallAsyncConcurrent() .ToArray(); while (!tasks.All(task => task.IsCompleted)) { - Ros2cs.SpinOnce(Node, 0.1); + service.TryProcess(); + Client.TryProcess(); } Assert.That(tasks.Select(task => task.Result.Sum), Is.All.EqualTo(100)); } @@ -96,7 +99,7 @@ public void ClientCallAsyncConcurrent() [Test] public void ClientWaitForService() { - Assert.That(!Client.IsServiceAvailable()); + Assert.That(Client.IsServiceAvailable(), Is.False); { using var service = Node.CreateService( SERVICE_NAME, @@ -104,22 +107,33 @@ public void ClientWaitForService() ); Assert.That(Client.IsServiceAvailable()); } - Assert.That(!Client.IsServiceAvailable()); + Assert.That(Client.IsServiceAvailable(), Is.False); } [Test] public void DisposedClientHandling() { - Assert.That(!Client.IsDisposed); + Assert.That(Client.IsDisposed, Is.False); + + Client.Dispose(); + + Assert.That(Client.IsDisposed); + Assert.That(Node.Clients, Does.Not.Contain(Client)); + Assert.Throws(() => Client.CallAsync(CreateRequest(42, 3))); + } + + [Test] + public void DoubleDisposeClient() + { Client.Dispose(); + Client.Dispose(); + Assert.That(Client.IsDisposed); - Assert.DoesNotThrow(() => { Ros2cs.SpinOnce(Node, 0.1); }); } [Test] public void DisposedClientTasks() { - Ros2cs.SpinOnce(Node, 0.1); using var service = Node.CreateService( SERVICE_NAME, HandleRequest @@ -131,15 +145,6 @@ public void DisposedClientTasks() Assert.Throws(task.Wait); Assert.That(task.IsFaulted); Assert.That(task.Exception.InnerExceptions.Any(e => e is ObjectDisposedException)); - Assert.DoesNotThrow(() => { Ros2cs.SpinOnce(Node, 0.1); }); - } - - [Test] - public void ReinitializeDisposedClient() - { - Client.Dispose(); - Client = Node.CreateClient(SERVICE_NAME); - Assert.DoesNotThrow(() => { Ros2cs.SpinOnce(Node, 0.1); }); } [Test] @@ -158,7 +163,8 @@ public void ClientPendingRequests() while (!tasks.Any(task => task.IsCompleted)) { - Ros2cs.SpinOnce(Node, 0.1); + service.TryProcess(); + Client.TryProcess(); } int completed = tasks.Where(task => task.IsCompletedSuccessfully).Count(); @@ -189,7 +195,8 @@ public void ClientCancel() while (!finishedTask.IsCompleted) { - Ros2cs.SpinOnce(Node, 0.1); + service.TryProcess(); + Client.TryProcess(); } Assert.That(this.Client.Cancel(finishedTask), Is.False); @@ -200,7 +207,25 @@ public void ClientCancel() Assert.That(pendingTask.IsCanceled); Assert.That(this.Client.PendingRequests.Count, Is.Zero); - Assert.DoesNotThrow(() => { Ros2cs.SpinOnce(Node, 0.1); }); + Assert.DoesNotThrow(() => { service.TryProcess(); Client.TryProcess(); }); + } + + [Test] + public void ClientTryProcess() + { + Assert.That(this.Client.TryProcess(), Is.False); + + using var service = Node.CreateService( + SERVICE_NAME, + HandleRequest + ); + Task pendingTask = this.Client.CallAsync(this.CreateRequest(3, 4)); + while (!service.TryProcess()) + {} + while (!this.Client.TryProcess()) + {} + + Assert.That(pendingTask.IsCompletedSuccessfully); } } } diff --git a/src/ros2cs/ros2cs_tests/src/ClockTest.cs b/src/ros2cs/ros2cs_tests/src/ClockTest.cs index db36813a..9eeca09f 100644 --- a/src/ros2cs/ros2cs_tests/src/ClockTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ClockTest.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -20,18 +20,6 @@ namespace ROS2.Test [TestFixture] public class ClockTest { - [SetUp] - public void SetUp() - { - Ros2cs.Init(); - } - - [TearDown] - public void TearDown() - { - Ros2cs.Shutdown(); - } - [Test] public void CreateClock() { diff --git a/src/ros2cs/ros2cs_tests/src/ContextTest.cs b/src/ros2cs/ros2cs_tests/src/ContextTest.cs new file mode 100644 index 00000000..99ce4f0e --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/ContextTest.cs @@ -0,0 +1,160 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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 NUnit.Framework; + +namespace ROS2.Test +{ + [TestFixture] + public class ContextTest + { + private Context Context; + + [SetUp] + public void SetUp() + { + Context = new Context(); + } + + [TearDown] + public void TearDown() + { + Context.Dispose(); + } + + [Test] + public void ContextOk() + { + Assert.That(Context.Ok()); + Assert.That(Context.IsDisposed, Is.False); + + Context.Dispose(); + + Assert.That(Context.Ok(), Is.False); + Assert.That(Context.IsDisposed); + } + + [Test] + public void ContextOnShutdown() + { + int called = 0; + Context.OnShutdown += () => { called += 1; }; + + Context.Dispose(); + + Assert.That(called, Is.EqualTo(1)); + + Context.Dispose(); + + Assert.That(called, Is.EqualTo(1)); + } + + [Test] + public void MultipleContexts() + { + using (Context secondContext = new Context()) + { + Assert.That(Context.IsDisposed, Is.False); + } + Assert.That(Context.IsDisposed, Is.False); + } + + [Test] + public void ContextDoubleDisposal() + { + Context.Dispose(); + Context.Dispose(); + + Assert.That(Context.IsDisposed); + } + + [Test] + public void ContextCreateNode() + { + string name = "test"; + var nodes = Context.Nodes; + + Assert.That(Context.TryCreateNode(name, out INode node)); + Assert.That(nodes, Contains.Item(new KeyValuePair(name, node))); + Assert.That(node.Name, Is.EqualTo(name)); + } + + [Test] + public void ContextRecreateNode() + { + string name = "test"; + Assert.That(Context.TryCreateNode(name, out INode node)); + node.Dispose(); + + Assert.That(Context.TryCreateNode(name, out _)); + } + + [Test] + public void ContextCreateNodeDisposed() + { + Context.Dispose(); + + Assert.That(() => { Context.TryCreateNode("test", out _); }, Throws.TypeOf()); + } + + [Test] + public void ContextRejectDuplicateNode() + { + string name = "test"; + Assert.That(Context.TryCreateNode(name, out _)); + + Assert.That(Context.TryCreateNode(name, out _), Is.False); + Assert.That(Context.Nodes.Count, Is.EqualTo(1)); + Assert.That(Context.TryCreateNode(name + "2", out _)); + } + + [Test] + public void ContextKeepAliveNode() + { + Assert.That(Context.TryCreateNode("test", out INode node)); + var weakRef = new WeakReference(node); + node = null; + GC.Collect(); + + Assert.That(weakRef.TryGetTarget(out _)); + } + + [Test] + public void ContextDisposeNode() + { + Assert.That(Context.TryCreateNode("test", out INode node)); + + Assert.That(Context.Nodes.Values, Contains.Item(node)); + + node.Dispose(); + + Assert.That(Context.Nodes.Values, Does.Not.Contain(node)); + } + + [Test] + public void ContextDisposeNodes() + { + Assert.That(Context.TryCreateNode("test1", out INode node1)); + Assert.That(Context.TryCreateNode("test2", out INode node2)); + + Context.Dispose(); + + Assert.That(node1.IsDisposed); + Assert.That(node2.IsDisposed); + Assert.That(Context.Nodes.Values, Is.Empty); + } + } +} diff --git a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs new file mode 100644 index 00000000..f37b0f3d --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs @@ -0,0 +1,110 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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; +using NUnit.Framework; + +namespace ROS2.Test +{ + [TestFixture] + public class GuardConditionTest + { + private Context Context; + + private GuardCondition GuardCondition; + + [SetUp] + public void SetUp() + { + this.Context = new Context(); + this.GuardCondition = this.Context.CreateGuardCondition( + () => { throw new InvalidOperationException("guard condition was called"); } + ); + } + + [TearDown] + public void TearDown() + { + this.Context.Dispose(); + } + + [Test] + public void DisposedGuardConditionHandling() + { + Assert.That(this.GuardCondition.IsDisposed, Is.False); + + this.Context.Dispose(); + + Assert.That(this.GuardCondition.IsDisposed, Is.True); + Assert.Throws(() => this.GuardCondition.Trigger()); + } + + [Test] + public void DoubleDisposeGuardCondition() + { + this.GuardCondition.Dispose(); + this.GuardCondition.Dispose(); + + Assert.That(this.GuardCondition.IsDisposed, Is.True); + } + + [Test] + public void OnShutdownDisposal() + { + this.Context.OnShutdown += () => Assert.That(this.GuardCondition.IsDisposed, Is.False); + + this.Context.Dispose(); + } + + [Test] + public void DisposedContextHandling() + { + this.Context.Dispose(); + + Assert.Throws(() => this.Context.CreateGuardCondition(() => { })); + } + + [Test] + public void TriggerGuardCondition() + { + using var waitSet = new WaitSet(this.Context); + waitSet.GuardConditions.Add(this.GuardCondition); + + Assert.That(waitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.False); + + using var timer = new Timer( + _ => this.GuardCondition.Trigger(), + null, + TimeSpan.FromSeconds(0.25), + Timeout.InfiniteTimeSpan + ); + + Assert.That(waitSet.TryWait(TimeSpan.FromSeconds(0.5), out var result), Is.True); + Assert.That(result.ReadyGuardConditions, Does.Contain(this.GuardCondition)); + } + + [Test] + public void TriggerGuardConditionNotWaiting() + { + using var waitSet = new WaitSet(this.Context); + waitSet.GuardConditions.Add(this.GuardCondition); + + this.GuardCondition.Trigger(); + + Assert.That(waitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.True); + Assert.That(waitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.False); + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_tests/src/InitShutdownTest.cs b/src/ros2cs/ros2cs_tests/src/InitShutdownTest.cs deleted file mode 100644 index a9d8322c..00000000 --- a/src/ros2cs/ros2cs_tests/src/InitShutdownTest.cs +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) -// Copyright 2019-2021 Robotec.ai -// -// 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 NUnit.Framework; - -namespace ROS2.Test -{ - [TestFixture] - public class InitShutdownTest - { - [Test] - public void Init() - { - Ros2cs.Init(); - try - { - Ros2cs.Shutdown(); - } - catch (RuntimeError) - { - } - } - - [Test] - public void InitShutdown() - { - Ros2cs.Init(); - Ros2cs.Shutdown(); - } - - [Test] - public void InitShutdownSequence() - { - Ros2cs.Init(); - Ros2cs.Shutdown(); - Ros2cs.Init(); - Ros2cs.Shutdown(); - } - - [Test] - public void DoubleInit() - { - Ros2cs.Init(); - Ros2cs.Init(); - Ros2cs.Shutdown(); - } - - [Test] - public void DoubleShutdown() - { - Ros2cs.Init(); - Ros2cs.Shutdown(); - Ros2cs.Shutdown(); - } - - [Test] - public void CreateNodeWithoutInit() - { - Assert.That(() => { Ros2cs.CreateNode("foo"); }, Throws.TypeOf()); - } - - [Test] - public void SpinEmptyNode() - { - Ros2cs.Init(); - try - { - var node = Ros2cs.CreateNode("TestNode"); - Assert.That(Ros2cs.SpinOnce(node), Is.False); - var subscription = node.CreateSubscription( - "subscription_test_topic", - (msg) => { throw new InvalidOperationException("subscription callback was triggered"); } - ); - Assert.That(Ros2cs.SpinOnce(node), Is.True); - } - finally - { - Ros2cs.Shutdown(); - } - } - } -} diff --git a/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs b/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs index 36643d0c..93d492fa 100644 --- a/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs +++ b/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs @@ -1,5 +1,5 @@ -// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai +// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,41 +20,44 @@ namespace ROS2.Test [TestFixture] public class LargeMessageTest { - INode subscriptionNode; - INode publisherNode; - Publisher publisher; + private Context Context; + + private INode SubscriptionNode; + + private INode PublisherNode; + + private IPublisher Publisher; [SetUp] public void SetUp() { - Ros2cs.Init(); + Context = new Context(); - subscriptionNode = Ros2cs.CreateNode("subscription_test_node"); - publisherNode = Ros2cs.CreateNode("publisher_test_node"); + Context.TryCreateNode("subscription_test_node", out SubscriptionNode); + Context.TryCreateNode("publisher_test_node", out PublisherNode); - publisher = publisherNode.CreatePublisher("subscription_test_topic"); + Publisher = PublisherNode.CreatePublisher("subscription_test_topic"); } [TearDown] public void TearDown() { - publisher.Dispose(); - subscriptionNode.Dispose(); - Ros2cs.Shutdown(); + Context.Dispose(); } [Test] - public void SubscriptionTriggerCallback() + public void SubscriptionTryProcess() { bool callbackTriggered = false; - subscriptionNode.CreateSubscription( + using var subscription = SubscriptionNode.CreateSubscription( "subscription_test_topic", (msg) => { callbackTriggered = true; }); std_msgs.msg.Float64MultiArray largeMsg = new std_msgs.msg.Float64MultiArray(); largeMsg.Data = new double[1024]; - publisher.Publish(largeMsg); - Ros2cs.SpinOnce(subscriptionNode, 0.1); + Publisher.Publish(largeMsg); + + Assert.That(subscription.TryProcess()); Assert.That(callbackTriggered, Is.True); } diff --git a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs new file mode 100644 index 00000000..72a828bc --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs @@ -0,0 +1,439 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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 System.Diagnostics; +using System.Threading; +using System.Threading.Tasks; +using NUnit.Framework; +using ROS2.Executors; + +namespace ROS2.Test +{ + [TestFixture] + public class ManualExecutorTest + { + private static readonly string SUBSCRIPTION_TOPIC = "test_executor"; + + private Context Context; + + private ManualExecutor Executor; + + private WaitSet WaitSet; + + [SetUp] + public void SetUp() + { + this.Context = new Context(); + this.WaitSet = new WaitSet(this.Context); + this.Executor = new ManualExecutor( + this.WaitSet, + new GuardCondition(this.Context, () => { }) + ); + } + + [TearDown] + public void TearDown() + { + this.Executor.Dispose(); + this.Context.Dispose(); + } + + [Test] + public void DisposedExecutorHandling() + { + Assert.That(this.Executor.IsDisposed, Is.False); + + this.Context.TryCreateNode("test_node", out var node); + this.Executor.Add(node); + this.Executor.Rescan(); + this.Executor.Dispose(); + + Assert.That(this.Executor.IsDisposed, Is.True); + Assert.That(node.Executor, Is.Null); + Assert.That(this.Executor, Does.Not.Contain(node)); + Assert.Throws(() => { this.Executor.TrySpin(TimeSpan.FromSeconds(0.1)); }); + } + + [Test] + public void DoubleDisposeExecutor() + { + Assert.That(this.Executor.IsDisposed, Is.False); + + this.Executor.Dispose(); + this.Executor.Dispose(); + + Assert.That(this.Executor.IsDisposed, Is.True); + } + + [Test] + public void RemoveNodeAfterContextDispose() + { + this.Context.TryCreateNode("test_node", out var node); + this.Executor.Add(node); + + this.Context.Dispose(); + + Assert.That(node.Executor, Is.Null); + Assert.That(this.Executor, Does.Not.Contain(node)); + Assert.That(node.IsDisposed, Is.True); + } + + [Test] + public void ScheduleRescan() + { + Assert.That(this.Executor.RescanScheduled, Is.False); + + this.Executor.ScheduleRescan(); + + Assert.That(this.Executor.RescanScheduled, Is.True); + } + + [Test] + public void RescheduleRescan() + { + Assert.That(this.Executor.RescanScheduled, Is.False); + + this.Executor.ScheduleRescan(); + this.Executor.ScheduleRescan(); + + Assert.That(this.Executor.RescanScheduled, Is.True); + } + + [Test] + public void Wait() + { + this.Executor.Wait(); + this.Executor.ScheduleRescan(); + this.Executor.Wait(); + } + + [Test] + public void TryWaitScheduled() + { + this.Executor.ScheduleRescan(); + + Assert.That(this.Executor.TryWait(TimeSpan.Zero), Is.True); + this.Executor.Wait(); + Assert.That(this.Executor.RescanScheduled, Is.True); + } + + [Test] + public void TryWaitUnscheduled() + { + Assert.That(this.Executor.TryWait(TimeSpan.Zero), Is.True); + this.Executor.Wait(); + Assert.That(this.Executor.RescanScheduled, Is.False); + } + + [Test] + public void TryWaitInterrupt() + { + Thread spinThread = new Thread(() => { this.Executor.TrySpin(TimeSpan.FromMinutes(1)); }); + spinThread.Start(); + // wait for spin to start + while (!this.Executor.IsSpinning) + { } + this.Executor.ScheduleRescan(); + + Assert.That(this.Executor.TryWait(TimeSpan.FromSeconds(10)), Is.True); + Assert.That(spinThread.Join(TimeSpan.FromSeconds(10)), Is.True); + } + + [Test] + public void TryWaitTimeout() + { + using ManualResetEventSlim isTriggered = new ManualResetEventSlim(false); + this.Context.TryCreateNode("test_node", out var node); + this.Executor.Add(node); + using var publisher = node.CreatePublisher( + SUBSCRIPTION_TOPIC + ); + using var subscription = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => + { + isTriggered.Set(); + Thread.Sleep(TimeSpan.FromSeconds(2)); + } + ); + this.Executor.Rescan(); + Thread spinThread = new Thread(() => + { + this.Executor.TrySpin(TimeSpan.FromMinutes(0.5)); + }); + spinThread.Start(); + publisher.Publish(new std_msgs.msg.Int32()); + isTriggered.Wait(TimeSpan.FromSeconds(1)); + this.Executor.ScheduleRescan(); + + Assert.That(this.Executor.TryWait(TimeSpan.FromSeconds(0.5)), Is.False); + Assert.That(spinThread.Join(TimeSpan.FromSeconds(10)), Is.True); + } + + [Test] + public void InterruptWaiting() + { + Thread spinThread = new Thread(() => { this.Executor.TrySpin(TimeSpan.FromMinutes(1)); }); + spinThread.Start(); + // wait for spin to start + while (!this.Executor.IsSpinning) + { } + + this.Executor.Interrupt(); + + Assert.That(spinThread.Join(TimeSpan.FromSeconds(10)), Is.True); + } + + [Test] + public void InterruptNotWaiting() + { + Assert.That(this.Executor.IsSpinning, Is.False); + + this.Executor.Interrupt(); + } + + [Test] + public void TrySpin() + { + std_msgs.msg.Int32 received = null; + this.Context.TryCreateNode("test_node", out var node); + this.Executor.Add(node); + using var publisher = node.CreatePublisher( + SUBSCRIPTION_TOPIC + ); + using var subscription = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => { received = msg; } + ); + this.Executor.Rescan(); + + Assert.That(this.Executor.TrySpin(TimeSpan.FromSeconds(0.1)), Is.True); + Assert.That(received, Is.Null); + + publisher.Publish(new std_msgs.msg.Int32()); + + Assert.That(this.Executor.TrySpin(TimeSpan.FromSeconds(0.1)), Is.True); + Assert.That(received, Is.Not.Null); + } + + [Test] + public void TrySpinRescanScheduled() + { + this.Executor.ScheduleRescan(); + Assert.That(this.Executor.TrySpin(TimeSpan.FromSeconds(0)), Is.False); + } + + [Test] + public void TrySpinEmpty() + { + Assert.That(this.Executor.TrySpin(TimeSpan.FromSeconds(0)), Is.True); + } + + [Test] + public void Rescan() + { + this.Executor.ScheduleRescan(); + this.Executor.Rescan(); + + Assert.That(this.Executor.RescanScheduled, Is.False); + } + + [Test] + public void RescanEmpty() + { + this.Executor.Rescan(); + Assert.That(this.Executor.RescanScheduled, Is.False); + } + + [Test] + public void AddNoExecutor() + { + this.Context.TryCreateNode("test_node", out var node); + this.Executor.Add(node); + + Assert.That(node.Executor, Is.SameAs(this.Executor)); + Assert.That(this.Executor, Does.Contain(node)); + Assert.That(this.Executor.RescanScheduled, Is.True); + } + + [Test] + public void AddExecutor() + { + this.Context.TryCreateNode("test_node", out var node); + + node.Executor = new DummyExecutor(); + + Assert.Throws(() => { this.Executor.Add(node); }); + Assert.That(node.Executor, Is.Not.SameAs(this.Executor)); + Assert.That(this.Executor, Does.Not.Contain(node)); + Assert.That(this.Executor.RescanScheduled, Is.False); + } + + [Test] + public void RemoveContains() + { + this.Context.TryCreateNode("test_node", out var node); + this.Executor.Add(node); + this.Executor.Rescan(); + + Assert.That(this.Executor.Remove(node), Is.True); + Assert.That(node.Executor, Is.Null); + Assert.That(this.Executor, Does.Not.Contain(node)); + Assert.That(this.Executor.RescanScheduled, Is.True); + } + + [Test] + public void RemoveNotContains() + { + this.Context.TryCreateNode("test_node", out var node); + + Assert.That(this.Executor.Remove(node), Is.False); + Assert.That(this.Executor.RescanScheduled, Is.False); + } + + [Test] + public void Clear() + { + this.Context.TryCreateNode("test_node1", out var node1); + this.Executor.Add(node1); + this.Context.TryCreateNode("test_node2", out var node2); + this.Executor.Add(node2); + this.Executor.Rescan(); + + this.Executor.Clear(); + + Assert.That(node1.Executor, Is.Null); + Assert.That(node2.Executor, Is.Null); + Assert.That(this.Executor, Does.Not.Contain(node1)); + Assert.That(this.Executor, Does.Not.Contain(node2)); + Assert.That(this.Executor.Count, Is.Zero); + Assert.That(this.Executor.RescanScheduled, Is.True); + } + + [Test] + public void SpinInTask() + { + using ManualResetEventSlim wasSpun = new ManualResetEventSlim(false); + using var guardCondition = this.Context.CreateGuardCondition(wasSpun.Set); + this.WaitSet.GuardConditions.Add(guardCondition); + + using var cancellationSource = new CancellationTokenSource(); + using Task spinTask = this.Executor.CreateSpinTask(TimeSpan.FromSeconds(0.5), cancellationSource.Token); + + Assert.That(spinTask.Status, Is.EqualTo(TaskStatus.Created)); + + spinTask.Start(); + try + { + while (spinTask.Status != TaskStatus.Running) + { + Thread.Yield(); // wait for task to be scheduled + } + Assert.That(wasSpun.Wait(TimeSpan.FromSeconds(1)), Is.False); + guardCondition.Trigger(); + Assert.That(wasSpun.Wait(TimeSpan.FromSeconds(1)), Is.True); + wasSpun.Reset(); + } + finally + { + cancellationSource.Cancel(); + try + { + spinTask.Wait(); + } + catch (AggregateException e) + { + e.Handle(inner => inner is TaskCanceledException); + } + } + + Assert.That(spinTask.Status, Is.EqualTo(TaskStatus.Canceled)); + guardCondition.Trigger(); + Assert.That(wasSpun.Wait(TimeSpan.FromSeconds(1)), Is.False); + } + + [Test] + public void ExceptionWhileSpinningInTask() + { + using var guardCondition = this.Context.CreateGuardCondition(() => + { + throw new SimulatedException("simulating runtime exception"); + }); + this.WaitSet.GuardConditions.Add(guardCondition); + + using var cancellationSource = new CancellationTokenSource(); + using Task spinTask = this.Executor.CreateSpinTask(TimeSpan.FromSeconds(0.5), cancellationSource.Token); + + spinTask.Start(); + try + { + while (spinTask.Status != TaskStatus.Running) + { + Thread.Yield(); // wait for task to be scheduled + } + guardCondition.Trigger(); + var exception = Assert.Throws(() => spinTask.Wait(TimeSpan.FromSeconds(1))); + Assert.That(exception.InnerExceptions, Has.Some.Matches(new Predicate(e => e is SimulatedException))); + Assert.That(spinTask.Status, Is.EqualTo(TaskStatus.Faulted)); + } + finally + { + cancellationSource.Cancel(); + try + { + spinTask.Wait(); + } + catch (AggregateException e) + { + e.Handle(inner => inner is TaskCanceledException || inner is SimulatedException); + } + } + } + + private sealed class SimulatedException : Exception + { + public SimulatedException(string msg) : base(msg) + { } + } + + private sealed class DummyExecutor : HashSet, IExecutor + { + public bool IsDisposed + { + get { return false; } + } + + public void ScheduleRescan() + { } + + public bool TryScheduleRescan(INode node) + { + return true; + } + + public void Wait() + { } + + public bool TryWait(TimeSpan timeout) + { + return true; + } + + public void Dispose() + { } + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs b/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs index 056d52cc..c18bd265 100644 --- a/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs +++ b/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs @@ -1,5 +1,5 @@ -// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai +// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,17 +13,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -using NUnit.Framework; using System; -using ROS2.Test; +using NUnit.Framework; using ROS2.Internal; +using ROS2.Test; namespace ROS2.TestNativeMethods { [TestFixture] public class RCLInitialize { - public static void InitRcl(ref rcl_context_t context) + internal static IntPtr InitRcl() { NativeRcl.rcl_reset_error(); rcl_init_options_t init_options = NativeRcl.rcl_get_zero_initialized_init_options(); @@ -31,54 +31,36 @@ public static void InitRcl(ref rcl_context_t context) var ret = (RCLReturnEnum)NativeRcl.rcl_init_options_init(ref init_options, allocator); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); - context = NativeRcl.rcl_get_zero_initialized_context(); - - ret = (RCLReturnEnum)NativeRcl.rcl_init(0, null, ref init_options, ref context); + IntPtr context = NativeRclInterface.rclcs_get_zero_initialized_context(); + ret = (RCLReturnEnum)NativeRcl.rcl_init(0, null, ref init_options, context); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK), Utils.PopRclErrorString()); - Assert.That(NativeRcl.rcl_context_is_valid(ref context), Is.True); + Assert.That(NativeRclInterface.rclcs_context_is_valid(context), Is.True); + return context; } - public static void ShutdownRcl(ref rcl_context_t context) + internal static void ShutdownRcl(IntPtr context) { - var ret = (RCLReturnEnum)NativeRcl.rcl_shutdown(ref context); + var ret = (RCLReturnEnum)NativeRcl.rcl_shutdown(context); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); + Assert.That(NativeRclInterface.rclcs_context_is_valid(context), Is.False); - ret = (RCLReturnEnum)NativeRcl.rcl_context_fini(ref context); + ret = (RCLReturnEnum)NativeRcl.rcl_context_fini(context); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); + + NativeRclInterface.rclcs_free_context(context); } [Test] public void InitShutdownFinalize() { - rcl_context_t context = new rcl_context_t(); - InitRcl(ref context); - ShutdownRcl(ref context); + var context = InitRcl(); + ShutdownRcl(context); } } [TestFixture] public class RCL { - rcl_context_t context = new rcl_context_t(); - - [SetUp] - public void SetUp() - { - RCLInitialize.InitRcl(ref context); - } - - [TearDown] - public void TearDown() - { - RCLInitialize.ShutdownRcl(ref context); - } - - [Test] - public void GetZeroInitializedContext() - { - rcl_context_t context = NativeRcl.rcl_get_zero_initialized_context(); - } - [Test] public void GetDefaultAllocator() { @@ -120,42 +102,32 @@ public void InitValidArgs() rcl_init_options_t init_options = NativeRcl.rcl_get_zero_initialized_init_options(); rcl_allocator_t allocator = NativeRcl.rcutils_get_default_allocator(); NativeRcl.rcl_init_options_init(ref init_options, allocator); - rcl_context_t context = NativeRcl.rcl_get_zero_initialized_context(); + IntPtr context = NativeRclInterface.rclcs_get_zero_initialized_context(); var ret = (RCLReturnEnum)NativeRcl.rcl_init( - 2, new string[] { "foo", "bar" }, ref init_options, ref context); - Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); - - Assert.That(NativeRcl.rcl_context_is_valid(ref context), Is.True); - ret = (RCLReturnEnum)NativeRcl.rcl_shutdown(ref context); + 2, new string[] { "foo", "bar" }, ref init_options, context); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); + Assert.That(NativeRclInterface.rclcs_context_is_valid(context), Is.True); - ret = (RCLReturnEnum)NativeRcl.rcl_context_fini(ref context); - Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); + RCLInitialize.ShutdownRcl(context); } } [TestFixture] public class NodeInitialize { - rcl_context_t context = new rcl_context_t(); + private IntPtr Context = IntPtr.Zero; [SetUp] public void SetUp() { - RCLInitialize.InitRcl(ref context); + this.Context = RCLInitialize.InitRcl(); } [TearDown] public void TearDown() { - RCLInitialize.ShutdownRcl(ref context); - } - - [Test] - public void GetZeroInitializedNode() - { - rcl_node_t node = NativeRcl.rcl_get_zero_initialized_node(); + RCLInitialize.ShutdownRcl(this.Context); } [Test] @@ -165,68 +137,80 @@ public void NodeGetDefaultOptions() NativeRclInterface.rclcs_node_dispose_options(defaultNodeOptions); } - public static void InitNode(ref rcl_node_t node, IntPtr nodeOptions, ref rcl_context_t context) + internal static IntPtr InitOptions() { - node = NativeRcl.rcl_get_zero_initialized_node(); + return NativeRclInterface.rclcs_node_create_default_options(); + } + + internal static void ShutdownOptions(IntPtr options) + { + NativeRclInterface.rclcs_node_dispose_options(options); + } - nodeOptions = NativeRclInterface.rclcs_node_create_default_options(); + internal static IntPtr InitNode(IntPtr options, IntPtr context) + { string name = "node_test"; string nodeNamespace = "/ns"; + IntPtr node = NativeRclInterface.rclcs_get_zero_initialized_node(); var ret = (RCLReturnEnum)NativeRcl.rcl_node_init( - ref node, name, nodeNamespace, ref context, nodeOptions); + node, name, nodeNamespace, context, options); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); + return node; } - public static void ShutdownNode(ref rcl_node_t node, IntPtr nodeOptions) + internal static void ShutdownNode(IntPtr node) { - NativeRcl.rcl_node_fini(ref node); - NativeRclInterface.rclcs_node_dispose_options(nodeOptions); + var ret = (RCLReturnEnum)NativeRcl.rcl_node_fini(node); + Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); } [Test] public void NodeInitShutdown() { - rcl_node_t node = new rcl_node_t(); - IntPtr nodeOptions = new IntPtr(); - - InitNode(ref node, nodeOptions, ref context); - ShutdownNode(ref node, nodeOptions); + var options = InitOptions(); + var node = InitNode(options, this.Context); + ShutdownNode(node); + ShutdownOptions(options); } } [TestFixture] - public class Node + public class NodeTest { - rcl_context_t context; - rcl_node_t node; - IntPtr nodeOptions = new IntPtr(); + private IntPtr Context = IntPtr.Zero; + + private IntPtr Options = IntPtr.Zero; + + private IntPtr Node = IntPtr.Zero; [SetUp] public void SetUp() { - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); + this.Context = RCLInitialize.InitRcl(); + this.Options = NodeInitialize.InitOptions(); + this.Node = NodeInitialize.InitNode(this.Options, this.Context); } [TearDown] public void TearDown() { - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); + NodeInitialize.ShutdownNode(this.Node); + NodeInitialize.ShutdownOptions(this.Options); + RCLInitialize.ShutdownRcl(this.Context); } [Test] public void NodeGetNamespace() { - string nodeNameFromRcl = Utils.PtrToString(NativeRcl.rcl_node_get_name(ref node)); + string nodeNameFromRcl = Utils.PtrToString(NativeRcl.rcl_node_get_name(this.Node)); Assert.That("node_test", Is.EqualTo(nodeNameFromRcl)); } [Test] public void NodeGetName() { - string nodeNamespaceFromRcl = Utils.PtrToString(NativeRcl.rcl_node_get_namespace(ref node)); + string nodeNamespaceFromRcl = Utils.PtrToString(NativeRcl.rcl_node_get_namespace(this.Node)); Assert.That("/ns", Is.EqualTo(nodeNamespaceFromRcl)); } } @@ -234,22 +218,26 @@ public void NodeGetName() [TestFixture] public class PublisherInitialize { - rcl_context_t context; - rcl_node_t node; - IntPtr nodeOptions = new IntPtr(); + private IntPtr Context = IntPtr.Zero; + + private IntPtr NodeOptions = IntPtr.Zero; + + private IntPtr Node = IntPtr.Zero; [SetUp] public void SetUp() { - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); + this.Context = RCLInitialize.InitRcl(); + this.NodeOptions = NodeInitialize.InitOptions(); + this.Node = NodeInitialize.InitNode(this.NodeOptions, this.Context); } [TearDown] public void TearDown() { - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); + NodeInitialize.ShutdownNode(this.Node); + NodeInitialize.ShutdownOptions(this.NodeOptions); + RCLInitialize.ShutdownRcl(this.Context); } [Test] @@ -259,85 +247,112 @@ public void PublisherCreateOptions() IntPtr publisherOptions = NativeRclInterface.rclcs_publisher_create_options(qos.handle); } - [Test] - public void GetZeroInitializedPublisher() + internal static IntPtr InitOptions() { - rcl_publisher_t publisher = NativeRcl.rcl_get_zero_initialized_publisher(); + QualityOfServiceProfile qos = new QualityOfServiceProfile(); + return NativeRclInterface.rclcs_publisher_create_options(qos.handle); } - public static void InitPublisher( - ref rcl_publisher_t publisher, ref rcl_node_t node, IntPtr publisherOptions) + internal static void ShutdownOptions(IntPtr options) { - publisher = NativeRcl.rcl_get_zero_initialized_publisher(); - QualityOfServiceProfile qos = new QualityOfServiceProfile(); - publisherOptions = NativeRclInterface.rclcs_publisher_create_options(qos.handle); + NativeRclInterface.rclcs_publisher_dispose_options(options); + } + + internal static IntPtr InitPublisher(IntPtr node, IntPtr options) + { + IntPtr publisher = NativeRclInterface.rclcs_get_zero_initialized_publisher(); MessageInternals msg = new std_msgs.msg.Bool(); IntPtr typeSupportHandle = msg.TypeSupportHandle; var ret = (RCLReturnEnum)NativeRcl.rcl_publisher_init( - ref publisher, ref node, typeSupportHandle, "publisher_test_topic", publisherOptions); + publisher, node, typeSupportHandle, "publisher_test_topic", options); + Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK), Utils.PopRclErrorString()); + return publisher; } - public static void ShutdownPublisher( - ref rcl_publisher_t publisher, ref rcl_node_t node, IntPtr publisherOptions) + public static void ShutdownPublisher(IntPtr publisher, IntPtr node) { - var ret = (RCLReturnEnum)NativeRcl.rcl_publisher_fini(ref publisher, ref node); + var ret = (RCLReturnEnum)NativeRcl.rcl_publisher_fini(publisher, node); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK), Utils.PopRclErrorString()); - NativeRclInterface.rclcs_publisher_dispose_options(publisherOptions); + NativeRclInterface.rclcs_free_publisher(publisher); } [Test] public void PublisherInit() { - rcl_publisher_t publisher = new rcl_publisher_t(); - IntPtr publisherOptions = new IntPtr(); - InitPublisher(ref publisher, ref node, publisherOptions); - ShutdownPublisher(ref publisher, ref node, publisherOptions); + var options = InitOptions(); + var publisher = InitPublisher(this.Node, options); + ShutdownPublisher(publisher, this.Node); + ShutdownOptions(options); } } [TestFixture] - public class Publisher + public class PublisherTest { - rcl_context_t context; - rcl_node_t node; - IntPtr nodeOptions = new IntPtr(); + private IntPtr Context = IntPtr.Zero; + + private IntPtr NodeOptions = IntPtr.Zero; + + private IntPtr Node = IntPtr.Zero; + + private IntPtr PublisherOptions = IntPtr.Zero; + + private IntPtr Publisher = IntPtr.Zero; [SetUp] public void SetUp() { - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); + this.Context = RCLInitialize.InitRcl(); + this.NodeOptions = NodeInitialize.InitOptions(); + this.Node = NodeInitialize.InitNode(this.NodeOptions, this.Context); + this.PublisherOptions = PublisherInitialize.InitOptions(); + this.Publisher = PublisherInitialize.InitPublisher(this.Node, this.PublisherOptions); } [TearDown] public void TearDown() { - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); + PublisherInitialize.ShutdownPublisher(this.Publisher, this.Node); + PublisherInitialize.ShutdownOptions(this.PublisherOptions); + NodeInitialize.ShutdownNode(this.Node); + NodeInitialize.ShutdownOptions(this.NodeOptions); + RCLInitialize.ShutdownRcl(this.Context); } [Test] public void PublisherPublish() { - rcl_publisher_t publisher = new rcl_publisher_t(); - IntPtr publisherOptions = new IntPtr(); - PublisherInitialize.InitPublisher(ref publisher, ref node, publisherOptions); MessageInternals msg = new std_msgs.msg.Bool(); rcl_allocator_t allocator = NativeRcl.rcutils_get_default_allocator(); - var ret = (RCLReturnEnum)NativeRcl.rcl_publish(ref publisher, msg.Handle, allocator.allocate); + var ret = (RCLReturnEnum)NativeRcl.rcl_publish(this.Publisher, msg.Handle, allocator.allocate); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK), Utils.PopRclErrorString()); - PublisherInitialize.ShutdownPublisher(ref publisher, ref node, publisherOptions); } } [TestFixture] public class SubscriptionInitialize { - [Test] - public void GetZeroInitializedSubscription() + private IntPtr Context = IntPtr.Zero; + + private IntPtr NodeOptions = IntPtr.Zero; + + private IntPtr Node = IntPtr.Zero; + + [SetUp] + public void SetUp() { - rcl_subscription_t subscription = NativeRcl.rcl_get_zero_initialized_subscription(); + this.Context = RCLInitialize.InitRcl(); + this.NodeOptions = NodeInitialize.InitOptions(); + this.Node = NodeInitialize.InitNode(this.NodeOptions, this.Context); + } + + [TearDown] + public void TearDown() + { + NodeInitialize.ShutdownNode(this.Node); + NodeInitialize.ShutdownOptions(this.NodeOptions); + RCLInitialize.ShutdownRcl(this.Context); } [Test] @@ -348,76 +363,82 @@ public void SubscriptionCreateOptions() NativeRclInterface.rclcs_subscription_dispose_options(subscriptionOptions); } - public static void InitSubscription( - ref rcl_subscription_t subscription, IntPtr subscriptionOptions, ref rcl_node_t node) + internal static IntPtr InitOptions() { - subscription = NativeRcl.rcl_get_zero_initialized_subscription(); QualityOfServiceProfile qos = new QualityOfServiceProfile(); - subscriptionOptions = NativeRclInterface.rclcs_subscription_create_options(qos.handle); + return NativeRclInterface.rclcs_subscription_create_options(qos.handle); + } + + internal static void ShutdownOptions(IntPtr options) + { + NativeRclInterface.rclcs_subscription_dispose_options(options); + } + + internal static IntPtr InitSubscription(IntPtr node, IntPtr options) + { + IntPtr subscription = NativeRclInterface.rclcs_get_zero_initialized_subscription(); MessageInternals msg = new std_msgs.msg.Bool(); IntPtr typeSupportHandle = msg.TypeSupportHandle; var ret = (RCLReturnEnum)NativeRcl.rcl_subscription_init( - ref subscription, ref node, typeSupportHandle, "/subscriber_test_topic", subscriptionOptions); + subscription, node, typeSupportHandle, "/subscriber_test_topic", options); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK), Utils.PopRclErrorString()); + return subscription; } - public static void ShutdownSubscription( - ref rcl_subscription_t subscription, IntPtr subscriptionOptions, ref rcl_node_t node) + internal static void ShutdownSubscription(IntPtr subscription, IntPtr node) { - var ret = (RCLReturnEnum)NativeRcl.rcl_subscription_fini(ref subscription, ref node); - NativeRclInterface.rclcs_subscription_dispose_options(subscriptionOptions); + var ret = (RCLReturnEnum)NativeRcl.rcl_subscription_fini(subscription, node); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK), Utils.PopRclErrorString()); + NativeRclInterface.rclcs_free_subscription(subscription); } [Test] public void SubscriptionInit() { - rcl_context_t context = new rcl_context_t(); - rcl_node_t node = new rcl_node_t(); - IntPtr nodeOptions = new IntPtr(); - - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); - - rcl_subscription_t subscription = new rcl_subscription_t(); - IntPtr subscriptionOptions = new IntPtr(); - - InitSubscription(ref subscription, subscriptionOptions, ref node); - ShutdownSubscription(ref subscription, subscriptionOptions, ref node); - - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); + var options = InitOptions(); + var subscription = InitSubscription(this.Node, options); + ShutdownSubscription(subscription, this.Node); + ShutdownOptions(options); } } [TestFixture] - public class Subscription + public class SubscriptionTest { - rcl_context_t context; - rcl_node_t node; - IntPtr nodeOptions = new IntPtr(); - rcl_subscription_t subscription; - IntPtr subscriptionOptions = new IntPtr(); + private IntPtr Context = IntPtr.Zero; + + private IntPtr NodeOptions = IntPtr.Zero; + + private IntPtr Node = IntPtr.Zero; + + private IntPtr SubscriptionOptions = IntPtr.Zero; + + private IntPtr Subscription = IntPtr.Zero; [SetUp] public void SetUp() { - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); - SubscriptionInitialize.InitSubscription(ref subscription, subscriptionOptions, ref node); + this.Context = RCLInitialize.InitRcl(); + this.NodeOptions = NodeInitialize.InitOptions(); + this.Node = NodeInitialize.InitNode(this.NodeOptions, this.Context); + this.SubscriptionOptions = SubscriptionInitialize.InitOptions(); + this.Subscription = SubscriptionInitialize.InitSubscription(this.Node, this.SubscriptionOptions); } [TearDown] public void TearDown() { - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); + SubscriptionInitialize.ShutdownSubscription(this.Subscription, this.Node); + SubscriptionInitialize.ShutdownOptions(this.SubscriptionOptions); + NodeInitialize.ShutdownNode(this.Node); + NodeInitialize.ShutdownOptions(this.NodeOptions); + RCLInitialize.ShutdownRcl(this.Context); } [Test] public void SubscriptionIsValid() { - Assert.That(NativeRcl.rcl_subscription_is_valid(ref subscription), Is.True); + Assert.That(NativeRclInterface.rclcs_subscription_is_valid(this.Subscription), Is.True); } [Test] @@ -426,163 +447,135 @@ public void WaitSetAddSubscription() NativeRcl.rcl_reset_error(); rcl_allocator_t allocator = NativeRcl.rcutils_get_default_allocator(); - rcl_wait_set_t waitSet = NativeRcl.rcl_get_zero_initialized_wait_set(); + IntPtr handle = NativeRclInterface.rclcs_get_zero_initialized_wait_set(); TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_init( - ref waitSet, + handle, (UIntPtr)1, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, - ref context, + this.Context, allocator )); - TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(ref waitSet)); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(handle)); - Assert.That(NativeRcl.rcl_subscription_is_valid(ref subscription), Is.True); + Assert.That(NativeRclInterface.rclcs_subscription_is_valid(this.Subscription), Is.True); UIntPtr index = (UIntPtr)42; - TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_add_subscription(ref waitSet, ref subscription, ref index)); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_add_subscription(handle, this.Subscription, out index)); Assert.That(index.ToUInt64(), Is.EqualTo(0)); long timeout_ns = 10*1000*1000; - var ret = (RCLReturnEnum)NativeRcl.rcl_wait(ref waitSet, timeout_ns); + var ret = (RCLReturnEnum)NativeRcl.rcl_wait(handle, timeout_ns); Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_TIMEOUT)); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_fini(handle)); + NativeRclInterface.rclcs_free_wait_set(handle); } } [TestFixture] public class WaitSet { - rcl_context_t context; - rcl_node_t node; - IntPtr nodeOptions = new IntPtr(); + private IntPtr Context = IntPtr.Zero; [SetUp] public void SetUp() { - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); + this.Context = RCLInitialize.InitRcl(); } [TearDown] public void TearDown() { - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); - } - - [Test] - public void GetZeroInitializedWaitSet() - { - // NOTE: The struct rcl_wait_set_t contains size_t - // fields that are set to UIntPtr in C# declaration, - // not guaranteed to work for all C implemenations/platforms. - rcl_wait_set_t waitSet = NativeRcl.rcl_get_zero_initialized_wait_set(); + RCLInitialize.ShutdownRcl(this.Context); } [Test] public void WaitSetInit() { rcl_allocator_t allocator = NativeRcl.rcutils_get_default_allocator(); - rcl_wait_set_t waitSet = NativeRcl.rcl_get_zero_initialized_wait_set(); + IntPtr handle = NativeRclInterface.rclcs_get_zero_initialized_wait_set(); TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_init( - ref waitSet, + handle, (UIntPtr)1, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, - ref context, + this.Context, allocator )); - TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_fini(ref waitSet)); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_fini(handle)); + NativeRclInterface.rclcs_free_wait_set(handle); } [Test] public void WaitSetClear() { rcl_allocator_t allocator = NativeRcl.rcutils_get_default_allocator(); - rcl_wait_set_t waitSet = NativeRcl.rcl_get_zero_initialized_wait_set(); - NativeRcl.rcl_wait_set_init( - ref waitSet, + IntPtr handle = NativeRclInterface.rclcs_get_zero_initialized_wait_set(); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_init( + handle, (UIntPtr)1, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, - ref context, + this.Context, allocator - ); - TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(ref waitSet)); - TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_fini(ref waitSet)); + )); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(handle)); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_fini(handle)); + NativeRclInterface.rclcs_free_wait_set(handle); } } [TestFixture] public class QualityOfService { - rcl_context_t context; - rcl_node_t node; - IntPtr nodeOptions = new IntPtr(); + private IntPtr Context = IntPtr.Zero; + + private IntPtr Options = IntPtr.Zero; + + private IntPtr Node = IntPtr.Zero; [SetUp] public void SetUp() { - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); + this.Context = RCLInitialize.InitRcl(); + this.Options = NodeInitialize.InitOptions(); + this.Node = NodeInitialize.InitNode(this.Options, this.Context); } [TearDown] public void TearDown() { - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); + NodeInitialize.ShutdownNode(this.Node); + NodeInitialize.ShutdownOptions(this.Options); + RCLInitialize.ShutdownRcl(this.Context); } [Test] public void SetSubscriptionQosProfile() { - rcl_subscription_t subscription = NativeRcl.rcl_get_zero_initialized_subscription(); - QualityOfServiceProfile qos = new QualityOfServiceProfile(); - IntPtr subscriptionOptions = NativeRclInterface.rclcs_subscription_create_options(qos.handle); + IntPtr options = NativeRclInterface.rclcs_subscription_create_options(qos.handle); - MessageInternals msg = new std_msgs.msg.Bool(); - IntPtr typeSupportHandle = msg.TypeSupportHandle; - NativeRcl.rcl_subscription_init( - ref subscription, ref node, typeSupportHandle, "/subscriber_test_topic", subscriptionOptions); + var subscription = SubscriptionInitialize.InitSubscription(this.Node, options); - Assert.That(NativeRcl.rcl_subscription_is_valid(ref subscription), Is.True); + Assert.That(NativeRclInterface.rclcs_subscription_is_valid(subscription), Is.True); - NativeRcl.rcl_subscription_fini(ref subscription, ref node); - NativeRclInterface.rclcs_subscription_dispose_options(subscriptionOptions); + SubscriptionInitialize.ShutdownSubscription(subscription, this.Node); + SubscriptionInitialize.ShutdownOptions(options); } } [TestFixture] public class Clock { - rcl_context_t context; - rcl_node_t node; - IntPtr nodeOptions = new IntPtr(); - - [SetUp] - public void SetUp() - { - RCLInitialize.InitRcl(ref context); - NodeInitialize.InitNode(ref node, nodeOptions, ref context); - } - - [TearDown] - public void TearDown() - { - NodeInitialize.ShutdownNode(ref node, nodeOptions); - RCLInitialize.ShutdownRcl(ref context); - } - [Test] public void CreateClock() { @@ -597,7 +590,7 @@ public void ClockGetNow() rcl_allocator_t allocator = NativeRcl.rcutils_get_default_allocator(); IntPtr clockHandle = NativeRclInterface.rclcs_ros_clock_create(ref allocator); long queryNow = 0; - NativeRcl.rcl_clock_get_now(clockHandle, ref queryNow); + NativeRcl.rcl_clock_get_now(clockHandle, out queryNow); Assert.That(queryNow, Is.Not.EqualTo(0)); diff --git a/src/ros2cs/ros2cs_tests/src/NodeTest.cs b/src/ros2cs/ros2cs_tests/src/NodeTest.cs index 99cd1593..86370f52 100644 --- a/src/ros2cs/ros2cs_tests/src/NodeTest.cs +++ b/src/ros2cs/ros2cs_tests/src/NodeTest.cs @@ -1,5 +1,5 @@ -// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai +// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,121 +14,394 @@ // limitations under the License. using System; -using NUnit.Framework; using example_interfaces.srv; +using NUnit.Framework; +using ROS2.Executors; namespace ROS2.Test { [TestFixture] public class NodeTest { - INode node; - string TEST_NODE = "my_node"; + private static readonly string TEST_NODE = "my_node"; + + private Context Context; + + private INode Node; [SetUp] public void SetUp() { - Ros2cs.Init(); - node = Ros2cs.CreateNode(TEST_NODE); + Context = new Context(); + Context.TryCreateNode(TEST_NODE, out Node); } [TearDown] public void TearDown() { - node.Dispose(); - Ros2cs.Shutdown(); + Context.Dispose(); } [Test] - public void Accessors() + public void NameProperty() { - Assert.That(node.Name, Is.EqualTo("my_node")); + Assert.That(Node.Name, Is.EqualTo(TEST_NODE)); } [Test] - public void CreatePublisher() + public void ContextProperty() { - Publisher publisher = node.CreatePublisher("test_topic"); - publisher.Dispose(); + Assert.That(Object.ReferenceEquals(Node.Context, Context)); + } + + [Test] + public void DefaultExecutor() + { + Assert.That(Node.Executor, Is.Null); + } + + [Test] + public void IsDisposed() + { + Assert.That(Node.IsDisposed, Is.False); + + Node.Dispose(); + + Assert.That(Node.IsDisposed); + } + + [Test] + public void DisposedPublisherCreation() + { + Node.Dispose(); - using (publisher = node.CreatePublisher("test_topic")) + Assert.Throws(() => { - } + Node.CreatePublisher("test_publisher"); + }); + Assert.That(Node.Publishers.Count, Is.Zero); } [Test] - public void Publish() + public void DisposedSubscriptionCreation() { - using (Publisher publisher = node.CreatePublisher("test_topic")) + Node.Dispose(); + + Assert.Throws(() => + { + Node.CreateSubscription( + "test_subscription", + msg => { throw new InvalidOperationException($"subscription called with {msg}"); } + ); + }); + Assert.That(Node.Subscriptions.Count, Is.Zero); + } + + [Test] + public void DisposedServiceCreation() + { + Node.Dispose(); + + Assert.Throws(() => { - publisher.Publish(new std_msgs.msg.Bool()); - } + Node.CreateService( + "service_test", + request => { throw new InvalidOperationException($"received request {request}"); } + ); + }); + Assert.That(Node.Services.Count, Is.Zero); } [Test] - public void PublishChangingSize() + public void DisposedClientCreation() { - using (Publisher publisher = node.CreatePublisher("test_topic")) - { - string[] setStringSequence = new string[2] + Node.Dispose(); + + Assert.Throws(() => { - "First", - "Second string to send, has to be a bit longer for the test" - }; + Node.CreateClient("client_test"); + }); + Assert.That(Node.Clients.Count, Is.Zero); + } + + [Test] + public void DoubleDisposal() + { + Node.Dispose(); + Node.Dispose(); + + Assert.That(Node.IsDisposed); + } + + [Test] + public void DisposeAllOnDispose() + { + var publisher = Node.CreatePublisher("publisher_topic"); + var subscription = Node.CreateSubscription( + "publisher_topic", + msg => { throw new InvalidOperationException($"received message {msg}"); } + ); + var service = Node.CreateService( + "service_topic", + request => { throw new InvalidOperationException($"received request {request}"); } + ); + var client = Node.CreateClient( + "service_topic" + ); + + Node.Dispose(); + + Assert.That(publisher.IsDisposed); + Assert.That(subscription.IsDisposed); + Assert.That(service.IsDisposed); + Assert.That(client.IsDisposed); + Assert.That(Node.Publishers, Is.Empty); + Assert.That(Node.Subscriptions, Is.Empty); + Assert.That(Node.Services, Is.Empty); + Assert.That(Node.Clients, Is.Empty); + } + + [Test] + public void RemoveExecutorOnDispose() + { + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + executor.Rescan(); + + this.Node.Dispose(); + + Assert.That(executor, Is.Empty); + Assert.That(this.Node.Executor, Is.Null); + } + + [Test] + public void CreatePublisher() + { + string topic = "publisher_topic"; + var publishers = Node.Publishers; + using IPublisher publisher = Node.CreatePublisher(topic); + + Assert.That(publishers, Contains.Item(publisher)); + Assert.That(publisher.Topic, Is.EqualTo(topic)); + } + + [Test] + public void CreatePublisherWithExecutor() + { + string topic = "publisher_topic"; + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + executor.Rescan(); + + using IPublisher publisher = Node.CreatePublisher(topic); + + // publisher not in executor + Assert.That(executor.RescanScheduled, Is.False); + } - test_msgs.msg.UnboundedSequences msg3 = new test_msgs.msg.UnboundedSequences(); - msg3.String_values = setStringSequence; - publisher.Publish(msg3); + [Test] + public void DisposePublisher() + { + IPublisher publisher = Node.CreatePublisher("test_topic"); + + Assert.That(Node.Publishers, Contains.Item(publisher)); - msg3.Int32_values = new int[2] { 1, 2 }; - msg3.String_values[0] = "A string that is longer than the previous one"; - msg3.String_values[1] = "shorter than previous one"; + publisher.Dispose(); - // Publish reusing the message - publisher.Publish(msg3); + Assert.That(Node.Publishers, Does.Not.Contain(publisher)); + } - msg3.String_values = new string[5] { "1", "2", "3", "4", "5" }; - msg3.Int32_values = new int[10] { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 }; - publisher.Publish(msg3); + [Test] + public void DisposePublisherWithExecutor() + { + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + IPublisher publisher = Node.CreatePublisher("test_topic"); + executor.Rescan(); + + publisher.Dispose(); - msg3.String_values = new string[1] { "hello" }; - msg3.Int32_values = new int[1] { 1 }; - publisher.Publish(msg3); - } + // publisher not in executor + Assert.That(executor.RescanScheduled, Is.False); } [Test] public void CreateSubscription() { - Subscription subscription = node.CreateSubscription( - "/subscription_topic", msg => Console.WriteLine("I heard: [" + msg.Data + "]")); + string topic = "subscription_topic"; + var subscriptions = Node.Subscriptions; + using ISubscription subscription = Node.CreateSubscription( + topic, + msg => { throw new InvalidOperationException($"received message {msg}"); } + ); + + Assert.That(subscriptions, Contains.Item(subscription)); + Assert.That(subscription.Topic, Is.EqualTo(topic)); + } + + [Test] + public void CreateSubscriptionWithExecutor() + { + string topic = "subscription_topic"; + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + executor.Rescan(); + + using ISubscription subscription = Node.CreateSubscription( + topic, + msg => { throw new InvalidOperationException($"received message {msg}"); } + ); + + Assert.That(executor.RescanScheduled, Is.True); + } + + [Test] + public void DisposeSubscription() + { + ISubscription subscription = Node.CreateSubscription( + "test_topic", + msg => { throw new InvalidOperationException($"received message {msg}"); } + ); + + Assert.That(Node.Subscriptions, Contains.Item(subscription)); + subscription.Dispose(); - using (subscription = node.CreateSubscription( - "test_topic", msg => Console.WriteLine("Got message"))) - { - } + Assert.That(Node.Subscriptions, Does.Not.Contain(subscription)); + } + + [Test] + public void DisposeSubscriptionWithExecutor() + { + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + ISubscription subscription = Node.CreateSubscription( + "test_topic", + msg => { throw new InvalidOperationException($"received message {msg}"); } + ); + executor.Rescan(); + + subscription.Dispose(); + + Assert.That(executor.RescanScheduled, Is.True); + } + + [Test] + public void CreateService() + { + string topic = "service_topic"; + var services = Node.Services; + using IService service = Node.CreateService( + topic, + request => { throw new InvalidOperationException($"received request {request}"); } + ); + + Assert.That(services, Contains.Item(service)); + Assert.That(service.Topic, Is.EqualTo(topic)); } [Test] - public void RemoveService() + public void CreateServiceWithExecutor() { - var service = node.CreateService( - "/test", - request => { throw new InvalidOperationException("service should not be called"); } + string topic = "service_topic"; + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + executor.Rescan(); + + using IService service = Node.CreateService( + topic, + request => { throw new InvalidOperationException($"received request {request}"); } + ); + + Assert.That(executor.RescanScheduled, Is.True); + } + + [Test] + public void DisposeService() + { + IService service = Node.CreateService( + "test_topic", + request => { throw new InvalidOperationException($"received request {request}"); } ); - Assert.That(node.RemoveService(service)); - Assert.That(service.IsDisposed); + Assert.That(Node.Services, Contains.Item(service)); + + service.Dispose(); + + Assert.That(Node.Services, Does.Not.Contain(service)); + } + + [Test] + public void DisposeServiceWithExecutor() + { + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + IService service = Node.CreateService( + "test_topic", + request => { throw new InvalidOperationException($"received request {request}"); } + ); + executor.Rescan(); + + service.Dispose(); + + Assert.That(executor.RescanScheduled, Is.True); } [Test] - public void RemoveClient() + public void CreateClient() { - var client = node.CreateClient("/test"); + string topic = "client_topic"; + var clients = Node.Clients; + using IClient client = Node.CreateClient( + topic + ); + + Assert.That(clients, Contains.Item(client)); + Assert.That(client.Topic, Is.EqualTo(topic)); + } + + [Test] + public void CreateClientWithExecutor() + { + string topic = "client_topic"; + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + executor.Rescan(); + + using IClient client = Node.CreateClient( + topic + ); + + Assert.That(executor.RescanScheduled, Is.True); + } + + [Test] + public void DisposeClient() + { + IClient client = Node.CreateClient( + "test_topic" + ); - Assert.That(node.RemoveClient(client)); - Assert.That(client.IsDisposed); + Assert.That(Node.Clients, Contains.Item(client)); + + client.Dispose(); + + Assert.That(Node.Clients, Does.Not.Contain(client)); + } + + [Test] + public void DisposeClientWithExecutor() + { + using var executor = new ManualExecutor(this.Context); + executor.Add(this.Node); + IClient client = Node.CreateClient( + "test_topic" + ); + executor.Rescan(); + + client.Dispose(); + + Assert.That(executor.RescanScheduled, Is.True); } } } diff --git a/src/ros2cs/ros2cs_tests/src/PublisherTest.cs b/src/ros2cs/ros2cs_tests/src/PublisherTest.cs new file mode 100644 index 00000000..a84c4e1d --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/PublisherTest.cs @@ -0,0 +1,75 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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 NUnit.Framework; + +namespace ROS2.Test +{ + [TestFixture] + public class PublisherTest + { + private static readonly string TOPIC = "test_publisher"; + + private Context Context; + + private INode Node; + + private IPublisher Publisher; + + [SetUp] + public void SetUp() + { + Context = new Context(); + Context.TryCreateNode("publisher_test_node", out Node); + Publisher = Node.CreatePublisher(TOPIC); + } + + [TearDown] + public void TearDown() + { + Context.Dispose(); + } + + [Test] + public void DisposedPublisherHandling() + { + Assert.That(Publisher.IsDisposed, Is.False); + + Publisher.Dispose(); + + Assert.That(Publisher.IsDisposed); + Assert.That(Node.Publishers, Does.Not.Contain(Publisher)); + } + + [Test] + public void DoubleDisposePublisher() + { + Publisher.Dispose(); + Publisher.Dispose(); + + Assert.That(Publisher.IsDisposed); + } + + [Test] + public void PublishDisposed() + { + var msg = new std_msgs.msg.Int32(); + msg.Data = 42; + Publisher.Dispose(); + + Assert.Throws(() => Publisher.Publish(msg)); + } + } +} diff --git a/src/ros2cs/ros2cs_tests/src/ServiceTest.cs b/src/ros2cs/ros2cs_tests/src/ServiceTest.cs index 1a4be4f0..a3a64274 100644 --- a/src/ros2cs/ros2cs_tests/src/ServiceTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ServiceTest.cs @@ -1,4 +1,4 @@ -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,43 +23,53 @@ public class ServiceTest { private static readonly string SERVICE_NAME = "test_service"; + private Context Context; + private INode Node; private IService Service; - private Func OnRequest = - msg => throw new InvalidOperationException("callback not set"); - [SetUp] public void SetUp() { - Ros2cs.Init(); - Node = Ros2cs.CreateNode("service_test_node"); - Service = Node.CreateService(SERVICE_NAME, OnRequest); + Context = new Context(); + Context.TryCreateNode("service_test_node", out Node); + Service = Node.CreateService( + SERVICE_NAME, + request => { throw new InvalidOperationException($"received request ${request}"); } + ); } [TearDown] public void TearDown() { - Node.Dispose(); - Ros2cs.Shutdown(); + Context.Dispose(); } [Test] public void DisposedServiceHandling() { - Assert.That(!Service.IsDisposed); + Assert.That(Service.IsDisposed, Is.False); + Service.Dispose(); + Assert.That(Service.IsDisposed); - Assert.DoesNotThrow(() => { Ros2cs.SpinOnce(Node, 0.1); }); + Assert.That(Node.Services, Does.Not.Contain(Service)); } [Test] - public void ReinitializeDisposedService() + public void DoubleDisposeService() { Service.Dispose(); - Service = Node.CreateService(SERVICE_NAME, OnRequest); - Assert.DoesNotThrow(() => { Ros2cs.SpinOnce(Node, 0.1); }); + Service.Dispose(); + + Assert.That(Service.IsDisposed); + } + + [Test] + public void ServiceTryProcess() + { + Assert.That(Service.TryProcess(), Is.False); } } } diff --git a/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs b/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs index 4cec7897..c3ab2e05 100644 --- a/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs +++ b/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs @@ -1,5 +1,5 @@ -// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) -// Copyright 2019-2021 Robotec.ai +// Copyright 2019-2023 Robotec.ai +// Copyright 2019 Dyno Robotics (by Samuel Lindgren samuel@dynorobotics.se) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,40 +13,51 @@ // See the License for the specific language governing permissions and // limitations under the License. +using System; using NUnit.Framework; -using System.Collections.Generic; namespace ROS2.Test { [TestFixture] public class SubscriptionTest { - INode node; - Publisher publisher; + private static readonly string TOPIC = "test_subscription"; + + private Context Context; + + private INode Node; [SetUp] public void SetUp() { - Ros2cs.Init(); - node = Ros2cs.CreateNode("subscription_test_node"); - publisher = node.CreatePublisher("subscription_test_topic"); + Context = new Context(); + Context.TryCreateNode("subscription_test_node", out Node); } [TearDown] public void TearDown() { - publisher.Dispose(); - node.Dispose(); - Ros2cs.Shutdown(); + Context.Dispose(); + } + + private std_msgs.msg.Int32 CreateMessage(int data) + { + var msg = new std_msgs.msg.Int32(); + msg.Data = data; + return msg; } [Test] public void SubscriptionTriggerCallback() { bool callbackTriggered = false; - node.CreateSubscription("subscription_test_topic", (msg) => { callbackTriggered = true; }); - publisher.Publish(new std_msgs.msg.Int32()); - Ros2cs.SpinOnce(node, 0.1); + using var subscription = Node.CreateSubscription( + TOPIC, + (msg) => { callbackTriggered = true; } + ); + Node.CreatePublisher(TOPIC).Publish(CreateMessage(0)); + + Assert.That(subscription.TryProcess()); Assert.That(callbackTriggered, Is.True); } @@ -55,11 +66,13 @@ public void SubscriptionTriggerCallback() public void SubscriptionCallbackMessageData() { int messageData = 12345; - node.CreateSubscription("subscription_test_topic", (msg) => { messageData = msg.Data; }); - std_msgs.msg.Int32 published_msg = new std_msgs.msg.Int32(); - published_msg.Data = 42; - publisher.Publish(published_msg); - Ros2cs.SpinOnce(node, 0.1); + using var subscription = Node.CreateSubscription( + TOPIC, + (msg) => { messageData = msg.Data; } + ); + Node.CreatePublisher(TOPIC).Publish(CreateMessage(42)); + + Assert.That(subscription.TryProcess()); Assert.That(messageData, Is.EqualTo(42)); } @@ -67,60 +80,54 @@ public void SubscriptionCallbackMessageData() [Test] public void DisposedSubscriptionHandling() { - ISubscription subscriber = - node.CreateSubscription("subscription_test_topic", (msg) => { }); - subscriber.Dispose(); - Assert.DoesNotThrow( () => { Ros2cs.SpinOnce(node, 0.1); }); - } + var subscription = Node.CreateSubscription( + TOPIC, + (msg) => { throw new InvalidOperationException($"received message {msg}"); } + ); + + Assert.That(subscription.IsDisposed, Is.False); - [Test] - public void MultipleDisposedSubscriptionsHandling() - { - int numberOfSubscribers = 10; - List> subscriptions = new List>(); - for(int i = 0; i < numberOfSubscribers; i++) - { - subscriptions.Add( - node.CreateSubscription("subscription_test_topic", (msg) => { })); - } - Ros2cs.SpinOnce(node, 0.1); - subscriptions.ForEach(delegate(Subscription subscription) - { - subscription.Dispose(); - }); - Assert.DoesNotThrow( () => { Ros2cs.SpinOnce(node, 0.1); }); + subscription.Dispose(); + + Assert.That(subscription.IsDisposed); + Assert.That(Node.Subscriptions, Does.Not.Contain(subscription)); } [Test] - public void ReinitializeDisposedSubscriber() + public void DoubleDisposeSubscription() { - ISubscription subscriber = - node.CreateSubscription("subscription_test_topic", (msg) => { }); - subscriber.Dispose(); - subscriber = - node.CreateSubscription("subscription_test_topic", (msg) => { }); - Assert.DoesNotThrow( () => { Ros2cs.SpinOnce(node, 0.1); }); + var subscription = Node.CreateSubscription( + TOPIC, + (msg) => { throw new InvalidOperationException($"received message {msg}"); } + ); + + subscription.Dispose(); + subscription.Dispose(); + + Assert.That(subscription.IsDisposed); } [Test] public void SubscriptionQosDefaultDepth() { int count = 0; - node.CreateSubscription("subscription_test_topic", - (msg) => { count += 1; }); - - std_msgs.msg.Int32 published_msg = new std_msgs.msg.Int32(); - published_msg.Data = 42; + using var subscription = Node.CreateSubscription( + TOPIC, + (msg) => { count += 1; } + ); + using var publisher = Node.CreatePublisher(TOPIC); + var msg = CreateMessage(42); for (int i = 0; i < 10; i++) { - publisher.Publish(published_msg); + publisher.Publish(msg); } - for (int i = 0; i < 11; i++) + for (int i = 0; i < 10; i++) { - Ros2cs.SpinOnce(node, 0.1); + Assert.That(subscription.TryProcess()); } + Assert.That(subscription.TryProcess(), Is.False); Assert.That(count, Is.EqualTo(10)); } @@ -129,25 +136,24 @@ public void SubscriptionQosDefaultDepth() public void SubscriptionQosSensorDataDepth() { int count = 0; - QualityOfServiceProfile qosProfile = - new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA); - - node.CreateSubscription("subscription_test_topic", - (msg) => { count += 1; }, - qosProfile); - - std_msgs.msg.Int32 published_msg = new std_msgs.msg.Int32(); - published_msg.Data = 42; + using var subscription = Node.CreateSubscription( + TOPIC, + (msg) => { count += 1; }, + new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA) + ); + using var publisher = Node.CreatePublisher(TOPIC); + var msg = CreateMessage(42); for (int i = 0; i < 6; i++) { - publisher.Publish(published_msg); + publisher.Publish(msg); } - for (int i = 0; i < 11; i++) + for (int i = 0; i < 5; i++) { - Ros2cs.SpinOnce(node, 0.1); + Assert.That(subscription.TryProcess()); } + Assert.That(subscription.TryProcess(), Is.False); Assert.That(count, Is.EqualTo(5)); } diff --git a/src/ros2cs/ros2cs_tests/src/TaskExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/TaskExecutorTest.cs new file mode 100644 index 00000000..4d12b28b --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/TaskExecutorTest.cs @@ -0,0 +1,120 @@ +using System; +using System.Threading; +using System.Threading.Tasks; +using NUnit.Framework; +using ROS2.Executors; + +namespace ROS2.Test +{ + [TestFixture] + public class TaskExecutorTest + { + private readonly static string TOPIC = "/task_executor_test"; + + private Context Context; + + private TaskExecutor Executor; + + [SetUp] + public void SetUp() + { + this.Context = new Context(); + this.Executor = new TaskExecutor(this.Context, TimeSpan.FromSeconds(0.5)); + } + + [TearDown] + public void TearDown() + { + this.Executor.Dispose(); + this.Context.Dispose(); + } + + [Test] + public void DoubleDisposeExecutor() + { + Assert.That(this.Executor.IsDisposed, Is.False); + + this.Executor.Dispose(); + this.Executor.Dispose(); + + Assert.That(this.Executor.IsDisposed, Is.True); + } + + [Test] + public void StopTask() + { + this.Executor.Dispose(); + + Assert.Throws(() => this.Executor.Task.Wait(TimeSpan.FromSeconds(1))); + Assert.That(this.Executor.Task.Status, Is.EqualTo(TaskStatus.Canceled)); + } + + [Test] + public void DisposeContext() + { + this.Context.Dispose(); + + Assert.Throws(() => this.Executor.Task.Wait(TimeSpan.FromSeconds(1))); + Assert.That(this.Executor.Task.Status, Is.EqualTo(TaskStatus.Canceled)); + } + + + [Test] + public void ExceptionWhileSpinning() + { + if (this.Context.TryCreateNode("task_executor_test_node", out INode node)) + { + using (node) + { + this.Executor.Add(node); + using var publisher = node.CreatePublisher(TOPIC); + using var subscription = node.CreateSubscription( + TOPIC, + _ => { throw new Exception("simulated runtime exception"); } + ); + + publisher.Publish(new std_msgs.msg.Int32()); + + Assert.Throws(() => this.Executor.Task.Wait(TimeSpan.FromSeconds(1))); + Assert.That(this.Executor.Task.Status, Is.EqualTo(TaskStatus.Faulted)); + + this.Executor.Dispose(); + Assert.That(this.Executor.IsDisposed, Is.True); + Assert.That(node.Executor, Is.Null); + } + } + else + { + throw new ArgumentException("node already exists"); + } + } + + [Test] + public void SpinningInBackground() + { + if (this.Context.TryCreateNode("task_executor_test_node", out INode node)) + { + using (node) + { + this.Executor.Add(node); + using var msgReceived = new ManualResetEventSlim(false); + using var publisher = node.CreatePublisher(TOPIC); + using var subscription = node.CreateSubscription(TOPIC, _ => msgReceived.Set()); + + publisher.Publish(new std_msgs.msg.Int32()); + + Assert.That(msgReceived.Wait(TimeSpan.FromSeconds(1)), Is.True); + Assert.That(this.Executor.Task.IsCompleted, Is.False); + + this.Executor.Dispose(); + Assert.That(this.Executor.IsDisposed, Is.True); + Assert.That(node.Executor, Is.Null); + } + } + else + { + throw new ArgumentException("node already exists"); + } + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs new file mode 100644 index 00000000..a6fb0b25 --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs @@ -0,0 +1,290 @@ +// Copyright 2023 ADVITEC Informatik GmbH - www.advitec.de +// +// 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 System.Linq; +using System.Threading.Tasks; +using example_interfaces.srv; +using NUnit.Framework; + +namespace ROS2.Test +{ + [TestFixture] + public class WaitSetTest + { + private static readonly string SUBSCRIPTION_TOPIC = "test_subscription"; + + private static readonly string SERVICE_TOPIC = "test_service"; + + private Context Context; + + private WaitSet WaitSet; + + [SetUp] + public void SetUp() + { + this.Context = new Context(); + this.WaitSet = this.Context.CreateWaitSet(); + } + + [TearDown] + public void TearDown() + { + this.Context.Dispose(); + } + + [Test] + public void DisposedWaitSetHandling() + { + Assert.That(this.WaitSet.IsDisposed, Is.False); + + this.Context.Dispose(); + + Assert.That(this.WaitSet.IsDisposed, Is.True); + Assert.Throws(() => this.WaitSet.TryWait(TimeSpan.Zero, out _)); + } + + [Test] + public void DoubleDisposeWaitSet() + { + this.WaitSet.Dispose(); + this.WaitSet.Dispose(); + + Assert.That(this.WaitSet.IsDisposed, Is.True); + } + + [Test] + public void OnShutdownDisposal() + { + this.Context.OnShutdown += () => Assert.That(this.WaitSet.IsDisposed, Is.False); + + this.Context.Dispose(); + } + + [Test] + public void TestSubscriptionCollection() + { + Assert.That(this.WaitSet.Count, Is.Zero); + + this.Context.TryCreateNode("TestNode", out var node); + using var subscription = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => { throw new InvalidOperationException($"callback was triggered with {msg}"); } + ); + + this.WaitSet.Subscriptions.Add(subscription); + + Assert.That(this.WaitSet.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.Subscriptions.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.Subscriptions, Does.Contain(subscription)); + + Assert.That(this.WaitSet.Subscriptions.Remove(subscription), Is.True); + + Assert.That(this.WaitSet.Subscriptions, Does.Not.Contain(subscription)); + } + + [Test] + public void TestClientCollection() + { + Assert.That(this.WaitSet.Count, Is.Zero); + + this.Context.TryCreateNode("TestNode", out var node); + using var client = node.CreateClient(SERVICE_TOPIC); + + this.WaitSet.Clients.Add(client); + + Assert.That(this.WaitSet.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.Clients.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.Clients, Does.Contain(client)); + + Assert.That(this.WaitSet.Clients.Remove(client), Is.True); + + Assert.That(this.WaitSet.Clients, Does.Not.Contain(client)); + } + + [Test] + public void TestServiceCollection() + { + Assert.That(this.WaitSet.Count, Is.Zero); + + this.Context.TryCreateNode("TestNode", out var node); + using var service = node.CreateService( + SERVICE_TOPIC, + request => { throw new InvalidOperationException($"received request ${request}"); } + ); + + this.WaitSet.Services.Add(service); + + Assert.That(this.WaitSet.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.Services.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.Services, Does.Contain(service)); + + Assert.That(this.WaitSet.Services.Remove(service), Is.True); + + Assert.That(this.WaitSet.Services, Does.Not.Contain(service)); + } + + [Test] + public void TestGuardConditionCollection() + { + Assert.That(this.WaitSet.Count, Is.Zero); + + using var guard_condition = new GuardCondition( + this.Context, + () => throw new InvalidOperationException("guard condition was triggered!") + ); + this.WaitSet.GuardConditions.Add(guard_condition); + + Assert.That(this.WaitSet.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.GuardConditions.Count, Is.EqualTo(1)); + Assert.That(this.WaitSet.GuardConditions, Does.Contain(guard_condition)); + + Assert.That(this.WaitSet.GuardConditions.Remove(guard_condition), Is.True); + + Assert.That(this.WaitSet.GuardConditions, Does.Not.Contain(guard_condition)); + } + + [Test] + public void TestTryWait() + { + this.Context.TryCreateNode("TestNode", out var node); + + using var publisher = node.CreatePublisher(SUBSCRIPTION_TOPIC); + using var subscription = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => { } + ); + using var client = node.CreateClient(SERVICE_TOPIC); + using var service = node.CreateService( + SERVICE_TOPIC, + request => new AddTwoInts_Response() + ); + + this.WaitSet.Subscriptions.Add(subscription); + this.WaitSet.Clients.Add(client); + this.WaitSet.Services.Add(service); + + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.False); + + publisher.Publish(new std_msgs.msg.Int32()); + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.True); + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.True); + Assert.That(subscription.TryProcess(), Is.True); + + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.False); + + using var task = client.CallAsync(new AddTwoInts_Request()); + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.True); + Assert.That(service.TryProcess(), Is.True); + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.True); + Assert.That(client.TryProcess(), Is.True); + Assert.That(task.IsCompletedSuccessfully, Is.True); + + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.False); + } + + [Test] + public void TestTryWaitEmpty() + { + Assert.Throws(() => { this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _); }); + + this.Context.TryCreateNode("TestNode", out var node); + using var subscription = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => { throw new InvalidOperationException($"callback was triggered with {msg}"); } + ); + this.WaitSet.Subscriptions.Add(subscription); + + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _), Is.False); + + Assert.That(this.WaitSet.Subscriptions.Remove(subscription), Is.True); + Assert.Throws(() => { this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out _); }); + } + + [Test] + public void TestResult() + { + this.Context.TryCreateNode("TestNode", out var node); + + using var publisher = node.CreatePublisher(SUBSCRIPTION_TOPIC); + using var subscription = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => { } + ); + using var client = node.CreateClient(SERVICE_TOPIC); + using var service = node.CreateService( + SERVICE_TOPIC, + request => new AddTwoInts_Response() + ); + this.WaitSet.Subscriptions.Add(subscription); + this.WaitSet.Clients.Add(client); + this.WaitSet.Services.Add(service); + + publisher.Publish(new std_msgs.msg.Int32()); + client.CallAsync(new AddTwoInts_Request()); + + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out var result1), Is.True); + Assert.That(result1.ReadySubscriptions, Does.Contain(subscription).And.Exactly(1).Items); + Assert.That(result1.ReadyServices, Does.Contain(service).And.Exactly(1).Items); + Assert.That(result1, Has.Exactly(2).Items); + + Assert.That(subscription.TryProcess(), Is.True); + Assert.That(service.TryProcess(), Is.True); + + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out var result2), Is.True); + Assert.Catch(() => { _ = result1.ReadySubscriptions.GetEnumerator().MoveNext(); }); + Assert.Catch(() => { _ = result1.ReadyClients.GetEnumerator().MoveNext(); }); + Assert.Catch(() => { _ = result1.ReadyServices.GetEnumerator().MoveNext(); }); + Assert.That(result2.ReadyClients, Does.Contain(client).And.Exactly(1).Items); + Assert.That(result2, Has.Exactly(1).Items); + + this.WaitSet.Dispose(); + Assert.Catch(() => { _ = result2.ReadySubscriptions.GetEnumerator().MoveNext(); }); + Assert.Catch(() => { _ = result2.ReadyClients.GetEnumerator().MoveNext(); }); + Assert.Catch(() => { _ = result2.ReadyServices.GetEnumerator().MoveNext(); }); + } + + [Test] + public void TestResultMultiple() + { + this.Context.TryCreateNode("TestNode", out var node); + using var publisher1 = node.CreatePublisher(SUBSCRIPTION_TOPIC + "1"); + using var publisher2 = node.CreatePublisher(SUBSCRIPTION_TOPIC + "2"); + using var publisher3 = node.CreatePublisher(SUBSCRIPTION_TOPIC + "3"); + using var subscription1 = node.CreateSubscription( + SUBSCRIPTION_TOPIC + "1", + msg => { } + ); + using var subscription2 = node.CreateSubscription( + SUBSCRIPTION_TOPIC + "2", + msg => { } + ); + using var subscription3 = node.CreateSubscription( + SUBSCRIPTION_TOPIC + "3", + msg => { } + ); + this.WaitSet.Subscriptions.Add(subscription1); + this.WaitSet.Subscriptions.Add(subscription2); + this.WaitSet.Subscriptions.Add(subscription3); + + publisher1.Publish(new std_msgs.msg.Int32()); + publisher3.Publish(new std_msgs.msg.Int32()); + + Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out var result1), Is.True); + Assert.That(result1.ReadySubscriptions, Is.EquivalentTo(new ISubscriptionBase[]{ subscription1, subscription3 })); + Assert.That(result1, Has.Exactly(2).Items); + } + } +} \ No newline at end of file