From 13a4d04b697b74c82f821a1f222c5cf0eb03a50f Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 12:41:56 +0100 Subject: [PATCH 01/46] add executor interface Executors should allow for more flexibility and encapsulation. create mode 100644 src/ros2cs/ros2cs_core/interfaces/IExecutor.cs --- .../ros2cs_core/interfaces/IExecutor.cs | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 src/ros2cs/ros2cs_core/interfaces/IExecutor.cs diff --git a/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs new file mode 100644 index 00000000..6438ee6b --- /dev/null +++ b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs @@ -0,0 +1,43 @@ +// 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.Collections.Generic; + +namespace ROS2 +{ + /// + /// Executor controlling the processing of callbacks of some nodes. + /// + /// Adding and removing Nodes should be done by calling . + 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. + /// The executor has to guarantee that removed entities can be + /// disposed after this method returns. + /// + void Wake(); + + /// + /// Notify the instance that a node changed or got added or removed. + /// + /// Node which changed and is handled by the executor. + /// If the node is handled by the executor. + /// + bool Wake(INode node); + } +} From 8c580c2fefec3a96411fc9aba9c6a22843243354 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 12:49:31 +0100 Subject: [PATCH 02/46] refactor Node and create Context - Context is a non-static version of Ros2cs without executor functionality - Context and Node are now sealed - Node is now internal and is preventing its Context from being collected by the GC - not calling Dispose may now leak resources since internal collections may be finalized in the finalizer - sizes of rcl_node_t and rcl_conext_t are now invisible to C# code create mode 100644 src/ros2cs/ros2cs_core/Context.cs delete mode 100644 src/ros2cs/ros2cs_core/Ros2cs.cs create mode 100644 src/ros2cs/ros2cs_core/interfaces/IContext.cs create mode 100644 src/ros2cs/ros2cs_core/utils/MappingValueView.cs --- src/ros2cs/ros2cs_core/Context.cs | 158 +++++++ src/ros2cs/ros2cs_core/Node.cs | 388 ++++++++---------- src/ros2cs/ros2cs_core/Ros2cs.cs | 277 ------------- src/ros2cs/ros2cs_core/interfaces/IContext.cs | 51 +++ src/ros2cs/ros2cs_core/interfaces/INode.cs | 137 ++++--- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 63 ++- .../ros2cs_core/native/NativeRclInterface.cs | 39 +- src/ros2cs/ros2cs_core/native/NativeTypes.cs | 13 - .../ros2cs_core/native/rcl_native_interface.c | 29 ++ .../ros2cs_core/utils/MappingValueView.cs | 82 ++++ 10 files changed, 629 insertions(+), 608 deletions(-) create mode 100644 src/ros2cs/ros2cs_core/Context.cs delete mode 100644 src/ros2cs/ros2cs_core/Ros2cs.cs create mode 100644 src/ros2cs/ros2cs_core/interfaces/IContext.cs create mode 100644 src/ros2cs/ros2cs_core/utils/MappingValueView.cs diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs new file mode 100644 index 00000000..da62d3f0 --- /dev/null +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -0,0 +1,158 @@ +// 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.Collections.Generic; + +namespace ROS2 +{ + /// + /// ROS Context encapsulating the non-global state of an init/shutdown cycle. + /// + /// Can be disposed by the garbage collector + public sealed class Context : IContext + { + /// + public IReadOnlyDictionary Nodes { get; private set; } + + /// + public bool IsDisposed { get { return !this.Ok(); } } + + /// + public event Action OnShutdown; + + internal IntPtr Handle { get; private set; } = IntPtr.Zero; + + private Dictionary ROSNodes = new Dictionary(); + + private rcl_allocator_t ROSAllocator; + + /// + /// Get the current RMW implementation. + /// + /// The current implementation as string. + public static string GetRMWImplementation() + { + return Utils.PtrToString(NativeRmwInterface.rmw_native_interface_get_implementation_identifier()); + } + + public Context() + { + this.Nodes = new MappingValueView(this.ROSNodes, node => node); + this.ROSAllocator = NativeRcl.rcutils_get_default_allocator(); + this.Handle = NativeRclInterface.rclcs_get_zero_initialized_context(); + int ret = NativeRclInterface.rclcs_init(this.Handle, this.ROSAllocator); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } + } + + /// + /// Assert that the context has not been disposed. + /// + private void AssertOk() + { + if (this.IsDisposed) + { + throw new ObjectDisposedException("current ROS context"); + } + } + + /// + public bool Ok() + { + return NativeRcl.rcl_context_is_valid(this.Handle); + } + + /// + public bool TryCreateNode(string name, out INode node) + { + this.AssertOk(); + 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. + /// + /// Name of the node. + /// If the node existed in this context and has been removed. + internal bool RemoveNode(string name) + { + return this.ROSNodes.Remove(name); + } + + /// + 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 ROSNodes has not been finalized + if (disposing) + { + this.OnShutdown?.Invoke(); + foreach (var node in this.ROSNodes.Values) + { + node.DisposeFromContext(); + } + this.ROSNodes.Clear(); + // only safe when all nodes are gone, not calling Dispose() will leak the Handle + Utils.CheckReturnEnum(NativeRcl.rcl_context_fini(this.Handle)); + this.FreeHandles(); + } + } + + private void FreeHandles() + { + NativeRclInterface.rclcs_free_context(this.Handle); + this.Handle = IntPtr.Zero; + } + + ~Context() + { + this.Dispose(false); + } + } +} diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 6911c2d1..26c83739 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -13,271 +13,209 @@ // limitations under the License. using System; -using System.Linq; using System.Collections.Generic; 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 + /// + internal sealed class Node : INode, IExtendedDisposable { - get - { - lock (mutex) - { - return subscriptions.ToList(); - } - } - } + /// + public string Name { get; private set; } - internal List Clients - { - get - { - lock (mutex) - { - return clients.ToList(); - } - } - } + /// + public IContext Context { get { return this.ROSContext; } } - internal List Services - { - get - { - lock (mutex) + /// + public IExecutor Executor { get; private set; } + + /// + public bool IsDisposed { - return services.ToList(); + get { return !NativeRcl.rcl_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"); - } + internal IntPtr Handle = IntPtr.Zero; - /// Finalizer supporting IDisposable model - ~Node() - { - DestroyNode(); - } + private Context ROSContext; - /// Release managed and native resources. IDisposable implementation - public void Dispose() - { - DestroyNode(); - } + private IntPtr Options; - /// "Destructor" supporting IDisposable model - /// Disposes all subscriptions and publishers and clients before finilizing node - internal void DestroyNode() - { - lock (mutex) - { - if (!disposed) + /// + public IReadOnlyCollection Publishers { get { return this.CurrentPublishers; } } + + private HashSet CurrentPublishers = new HashSet(); + + /// + public IReadOnlyCollection Subscriptions { get { return this.CurrentSubscriptions; } } + + private HashSet CurrentSubscriptions = new HashSet(); + + /// + public IReadOnlyCollection Services { get { return this.CurrentServices; } } + + private HashSet CurrentServices = new HashSet(); + + /// + public IReadOnlyCollection Clients { get { return this.CurrentClients; } } + + private HashSet CurrentClients = new HashSet(); + + 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.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 + + ); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + { + this.FreeHandles(); + Utils.CheckReturnEnum(ret); + } } - } - } - /// 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()) + /// + /// Assert that the instance has not been disposed. + /// + private void AssertOk() { - logger.LogWarning("Cannot create client as the class is already disposed or shutdown was called"); - return null; + if (this.IsDisposed) + { + throw new ObjectDisposedException($"ROS2 node '{this.Name}'"); + } } - 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)) + /// + public bool TrySetExecutor(IExecutor executor) { - logger.LogInfo("Removing client for topic " + client.Topic); - client.Dispose(); - return clients.Remove(client); + return this.TrySetExecutor(executor, out _); } - 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()) + /// + public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) { - logger.LogWarning("Cannot create service as the class is already disposed or shutdown was called"); - return null; + oldExecutor = default(IExecutor); + if (this.Executor != null && !this.Executor.Remove(this)) + { + return false; + } + // prevent invalid executor if a failure occurs + (oldExecutor, this.Executor) = (this.Executor, oldExecutor); + oldExecutor?.Wake(this); + executor?.Add(this); + this.Executor = executor; + executor?.Wake(this); + return true; } - Service service = new Service(topic, this, callback, qos); - services.Add(service); - logger.LogInfo("Created service for topic " + topic); - return service; - } - } - - /// Remove a service - /// - public bool RemoveService(IServiceBase service) - { - lock (mutex) - { - if (services.Contains(service)) + /// + public IPublisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new() { - logger.LogInfo("Removing service for topic " + service.Topic); - service.Dispose(); - return services.Remove(service); + this.AssertOk(); + Publisher publisher = new Publisher(topic, this, qos); + this.CurrentPublishers.Add(publisher); + return publisher; } - 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()) + /// + public ISubscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new() { - logger.LogWarning("Cannot create publisher as the class is already disposed or shutdown was called"); - return null; + this.AssertOk(); + Subscription subscription = new Subscription(topic, this, callback, qos); + this.CurrentSubscriptions.Add(subscription); + this.Executor?.Wake(this); + return subscription; } - Publisher publisher = new Publisher(topic, this, qos); - publishers.Add(publisher); - logger.LogInfo("Created Publisher for topic " + topic); - return publisher; - } - } + /// + public IClient CreateClient(string topic, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() + { + this.AssertOk(); + Client client = new Client(topic, this, qos); + this.CurrentClients.Add(client); + this.Executor?.Wake(this); + return client; + } - /// 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()) + /// + public IService CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() { - logger.LogWarning("Cannot create subscription as the class is already disposed or shutdown was called"); - return null; + this.AssertOk(); + Service service = new Service(topic, this, callback, qos); + this.CurrentServices.Add(service); + this.Executor?.Wake(this); + return service; } - Subscription subscription = new Subscription(topic, this, callback, qos); - subscriptions.Add(subscription); - logger.LogInfo("Created subscription for topic " + topic); - return subscription; - } - } + /// + public void Dispose() + { + if (this.Handle == IntPtr.Zero) + { + return; + } + // no finalizer since the hash sets may have been finalized + this.DisposeFromContext(); + this.ROSContext.RemoveNode(this.Name); + } - /// Remove a publisher - /// - public bool RemovePublisher(IPublisherBase publisher) - { - lock (mutex) - { - if (publishers.Contains(publisher)) + /// + /// Dispose this node without modifying the context. + /// + internal void DisposeFromContext() { - logger.LogInfo("Removing publisher for topic " + publisher.Topic); - publisher.Dispose(); - return publishers.Remove(publisher); + if (this.Handle == IntPtr.Zero) + { + return; + } + + if (!this.TrySetExecutor(null)) + { + throw new RuntimeError("removing the node from the current executor failed"); + } + + foreach (IDisposable disposable in this.CurrentPublishers) + { + disposable.Dispose(); + } + this.CurrentPublishers.Clear(); + + foreach (IDisposable disposable in this.CurrentSubscriptions) + { + disposable.Dispose(); + } + this.CurrentSubscriptions.Clear(); + + foreach (IDisposable disposable in this.CurrentServices) + { + disposable.Dispose(); + } + this.CurrentServices.Clear(); + + foreach (IDisposable disposable in this.CurrentClients) + { + disposable.Dispose(); + } + this.CurrentClients.Clear(); + + Utils.CheckReturnEnum(NativeRcl.rcl_node_fini(this.Handle)); + this.FreeHandles(); } - return false; - } - } - /// Remove a subscription - /// - public bool RemoveSubscription(ISubscriptionBase subscription) - { - lock (mutex) - { - if (subscriptions.Contains(subscription)) + 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/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/interfaces/IContext.cs b/src/ros2cs/ros2cs_core/interfaces/IContext.cs new file mode 100644 index 00000000..c649d21d --- /dev/null +++ b/src/ros2cs/ros2cs_core/interfaces/IContext.cs @@ -0,0 +1,51 @@ +// 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.Collections.Generic; + +namespace ROS2 +{ + /// + /// ROS Context encapsulating the non-global state of an init/shutdown cycle. + /// + /// + /// Instances should be disposed with which is NOT automatically performed completely on shutdown. + /// + 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/INode.cs b/src/ros2cs/ros2cs_core/interfaces/INode.cs index 90b59d4c..f9e57674 100644 --- a/src/ros2cs/ros2cs_core/interfaces/INode.cs +++ b/src/ros2cs/ros2cs_core/interfaces/INode.cs @@ -17,68 +17,93 @@ 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. + /// + /// + /// A node has to guarantee that it is associated with at most one executor at any given time + /// to prevent undefined behaviour when multithreading is used. + /// + IExecutor Executor { get; } - /// 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(); + /// + /// Try to change the executor of this node. + /// + /// + /// A node has to guarantee that it is associated with at most one executor at any given time + /// to prevent undefined behaviour when multithreading is used. + /// + /// The new executor + /// If the change was successful + bool TrySetExecutor(IExecutor executor); - /// 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); + /// + /// The old executor + bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor); - /// 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 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(); - /// 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(); + /// + /// Publishers created on this node. + /// + IReadOnlyCollection Publishers { 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 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(); - /// 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); - } + /// + /// Subscriptions created on this node. + /// + IReadOnlyCollection Subscriptions { get; } + + /// 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(); + + /// + /// 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/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index 4fbea984..ce41c919 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -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 @@ -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,7 +54,7 @@ internal static ShutdownType typeof(ShutdownType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool ContextIsValidType(ref rcl_context_t context); + internal delegate bool ContextIsValidType(IntPtr context); internal static ContextIsValidType rcl_context_is_valid = (ContextIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -72,7 +63,7 @@ internal static ContextIsValidType 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, ref rcl_init_options_t option, IntPtr context); internal static InitType rcl_init = (InitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -81,7 +72,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 +81,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 +90,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 +99,16 @@ internal static NodeFiniType typeof(NodeFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate IntPtr NodeGetNameType(ref rcl_node_t node); + internal delegate bool NodeValidType(IntPtr node); + internal static NodeValidType + rcl_node_is_valid = + (NodeValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeRCL, + "rcl_node_is_valid"), + typeof(NodeValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr NodeGetNameType(IntPtr node); internal static NodeGetNameType rcl_node_get_name = (NodeGetNameType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -126,7 +117,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( @@ -153,7 +144,7 @@ internal static GetZeroInitiazizedClientType 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(ref rcl_client_t 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 +153,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(ref rcl_client_t client, IntPtr node); internal static ClientFiniType rcl_client_fini = (ClientFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -189,7 +180,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, ref rcl_client_t client, ref bool is_available); internal static ServiceIsAvailableType rcl_service_server_is_available = (ServiceIsAvailableType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -216,7 +207,7 @@ internal static GetZeroInitiazizedServiceType 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(ref rcl_service_t 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 +216,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(ref rcl_service_t client, IntPtr node); internal static ServiceFiniType rcl_service_fini = (ServiceFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -271,7 +262,7 @@ internal static GetZeroInitiazizedPublisherType 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(ref rcl_publisher_t 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 +271,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(ref rcl_publisher_t publisher, IntPtr node); internal static PublisherFiniType rcl_publisher_fini = (PublisherFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -307,7 +298,7 @@ internal static GetZeroInitializedSubcriptionType 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(ref rcl_subscription_t 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 +307,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(ref rcl_subscription_t subscription, IntPtr node); internal static SubscriptionFiniType rcl_subscription_fini = (SubscriptionFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -373,7 +364,7 @@ internal delegate int WaitSetInitType(ref rcl_wait_set_t wait_set, 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 = diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index 5d97aa31..c417d8f5 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -28,7 +28,26 @@ 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)] + internal delegate int RCLCSInitType(IntPtr context, rcl_allocator_t allocator); internal static RCLCSInitType rclcs_init = (RCLCSInitType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -36,6 +55,24 @@ internal static RCLCSInitType "rclcs_init"), typeof(RCLCSInitType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate IntPtr GetZeroInitializedNodeType(); + internal static GetZeroInitializedContextType + 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 FreeContextType + rclcs_free_node = + (FreeNodeType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_free_node"), + typeof(FreeNodeType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr GetErrorStringType(); internal static GetErrorStringType diff --git a/src/ros2cs/ros2cs_core/native/NativeTypes.cs b/src/ros2cs/ros2cs_core/native/NativeTypes.cs index 3a06787f..20911c2b 100644 --- a/src/ros2cs/ros2cs_core/native/NativeTypes.cs +++ b/src/ros2cs/ros2cs_core/native/NativeTypes.cs @@ -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,12 +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; diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index 7a774642..6658d383 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -21,6 +22,20 @@ #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 int rclcs_init(rcl_context_t *context, rcl_allocator_t allocator) { @@ -41,6 +56,20 @@ 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 rcl_node_options_t * rclcs_node_create_default_options() { diff --git a/src/ros2cs/ros2cs_core/utils/MappingValueView.cs b/src/ros2cs/ros2cs_core/utils/MappingValueView.cs new file mode 100644 index 00000000..f7611a22 --- /dev/null +++ b/src/ros2cs/ros2cs_core/utils/MappingValueView.cs @@ -0,0 +1,82 @@ +using System; +using System.Linq; +using System.Collections; +using System.Collections.Generic; + +namespace ROS2 +{ + /// + /// Dictionary view wich transforms the values of the wrapped dictionary. + /// + internal class MappingValueView : IReadOnlyDictionary + { + private readonly IReadOnlyDictionary Wrapped; + + private readonly Func Mapper; + + public MappingValueView(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(); + } + } +} \ No newline at end of file From 3f53085c8ee9d152550546613b79b31b0133ab07 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 12:54:48 +0100 Subject: [PATCH 03/46] improve description of IExtendedDisposable.IsDisposed This should prevent misunderstandings regarding being in an disposed state and being disposed successfully. --- src/ros2cs/ros2cs_common/interfaces/IExtendedDisposable.cs | 5 +++++ 1 file changed, 5 insertions(+) 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; } } From 51495c2e600e05e19e3e2584c5c7515edaaf4a17 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 15:03:17 +0100 Subject: [PATCH 04/46] refactor Publisher - is now sealed and internal - rcl_publisher_t size is now invisible to C# code - removes himself from node on dispose - prevents Node from being collected by the GC --- src/ros2cs/ros2cs_core/Node.cs | 6 +- src/ros2cs/ros2cs_core/Publisher.cs | 172 +++++++++++------- .../ros2cs_core/interfaces/IPublisher.cs | 48 +++-- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 24 +-- .../ros2cs_core/native/NativeRclInterface.cs | 18 ++ src/ros2cs/ros2cs_core/native/NativeTypes.cs | 5 - .../ros2cs_core/native/rcl_native_interface.c | 14 ++ 7 files changed, 183 insertions(+), 104 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 26c83739..728593a1 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -44,7 +44,7 @@ public bool IsDisposed /// public IReadOnlyCollection Publishers { get { return this.CurrentPublishers; } } - private HashSet CurrentPublishers = new HashSet(); + internal HashSet CurrentPublishers = new HashSet(); /// public IReadOnlyCollection Subscriptions { get { return this.CurrentSubscriptions; } } @@ -182,9 +182,9 @@ internal void DisposeFromContext() throw new RuntimeError("removing the node from the current executor failed"); } - foreach (IDisposable disposable in this.CurrentPublishers) + foreach (IRawPublisher publisher in this.CurrentPublishers) { - disposable.Dispose(); + publisher.DisposeFromNode(); } this.CurrentPublishers.Clear(); diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index cfe9cb68..4d34fe20 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -13,84 +13,124 @@ // limitations under the License. using System; -using System.Diagnostics; using ROS2.Internal; 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. + /// + internal 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 { return !NativeRcl.rcl_publisher_is_valid(this.Handle); } + } - publisherOptions = NativeRclInterface.rclcs_publisher_create_options(qualityOfServiceProfile.handle); + private IntPtr Handle = IntPtr.Zero; - IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); + 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)); - } + private readonly Node Node; - ~Publisher() - { - Dispose(); - } + 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); + } + } + + /// + /// Assert that the publisher has not been disposed. + /// + private void AssertOk() + { + if (this.IsDisposed) + { + throw new ObjectDisposedException($"publisher for topic '{this.Topic}'"); + } + } + + /// + public void Publish(T msg) + { + this.AssertOk(); + MessageInternals msgInternals = msg as MessageInternals; + msgInternals.WriteNativeMessage(); + Utils.CheckReturnEnum(NativeRcl.rcl_publish(this.Handle, msgInternals.Handle, IntPtr.Zero)); + } + + /// + 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) + { + this.Node.CurrentPublishers.Remove(this); + } + + this.DisposeFromNode(); + } + + /// + public void DisposeFromNode() + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + Utils.CheckReturnEnum(NativeRcl.rcl_publisher_fini(this.Handle, this.Node.Handle)); + this.FreeHandles(); + } + + 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/interfaces/IPublisher.cs b/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs index f58e526d..aa3d46ae 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs @@ -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/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index ce41c919..e5f716ce 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -253,16 +253,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, IntPtr 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( @@ -271,7 +262,16 @@ internal static PublisherInitType typeof(PublisherInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int PublisherFiniType(ref rcl_publisher_t publisher, IntPtr node); + internal delegate bool PublisherValidType(IntPtr publisher); + internal static PublisherValidType + rcl_publisher_is_valid = + (PublisherValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeRCL, + "rcl_publisher_is_valid"), + typeof(PublisherValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int PublisherFiniType(IntPtr publisher, IntPtr node); internal static PublisherFiniType rcl_publisher_fini = (PublisherFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -280,7 +280,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( diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index c417d8f5..ccfa491c 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -127,6 +127,24 @@ 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)] internal delegate IntPtr PublisherCreateOptionsType(IntPtr qos); internal static PublisherCreateOptionsType diff --git a/src/ros2cs/ros2cs_core/native/NativeTypes.cs b/src/ros2cs/ros2cs_core/native/NativeTypes.cs index 20911c2b..473094ab 100644 --- a/src/ros2cs/ros2cs_core/native/NativeTypes.cs +++ b/src/ros2cs/ros2cs_core/native/NativeTypes.cs @@ -52,11 +52,6 @@ public struct rcl_init_options_t private IntPtr impl; } - public struct rcl_publisher_t - { - private IntPtr impl; - } - public struct rcl_subscription_t { private IntPtr impl; diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index 6658d383..e98d377b 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -99,6 +99,20 @@ 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 rcl_publisher_options_t *rclcs_publisher_create_options(rmw_qos_profile_t * qos) { From 5ce3a792d081726c68fda9943410f08d3ef8cc67 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 17:11:07 +0100 Subject: [PATCH 05/46] add IWaitable This interface contains all methods necessary for executors to process work. The async version is equivalent to the non-async version and an optimization opportunity for task based executors. create mode 100644 src/ros2cs/ros2cs_core/interfaces/IWaitable.cs --- .../ros2cs_core/interfaces/IWaitable.cs | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 src/ros2cs/ros2cs_core/interfaces/IWaitable.cs diff --git a/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs new file mode 100644 index 00000000..dbc4f2c0 --- /dev/null +++ b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs @@ -0,0 +1,24 @@ +using System.Threading.Tasks; + +namespace ROS2 +{ + /// + /// Object which can process becoming ready. + /// + public interface IWaitable + { + /// + /// Try to process if this instance is ready. + /// + /// Both variants of this method are equivalent. + /// If the instance was ready. + bool TryProcess(); + + /// + /// Try to process preferably asynchronously if this instance is ready. + /// + /// indicating if the instance was ready. + /// + Task TryProcessAsync(); + } +} From a2c32d537aca2bf87974641c74104ce8f55d99f6 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 17:15:14 +0100 Subject: [PATCH 06/46] refactor Subscription - is now sealed and internal - rcl_subscription_t size is now invisible to C# code - removes himself from node on dispose - prevents Node from being collected by the GC --- src/ros2cs/ros2cs_core/Node.cs | 6 +- src/ros2cs/ros2cs_core/Subscription.cs | 210 ++++++++++-------- .../ros2cs_core/interfaces/ISubscription.cs | 38 ++-- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 19 +- .../ros2cs_core/native/NativeRclInterface.cs | 18 ++ src/ros2cs/ros2cs_core/native/NativeTypes.cs | 5 - .../ros2cs_core/native/rcl_native_interface.c | 14 ++ 7 files changed, 173 insertions(+), 137 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 728593a1..597ff981 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -49,7 +49,7 @@ public bool IsDisposed /// public IReadOnlyCollection Subscriptions { get { return this.CurrentSubscriptions; } } - private HashSet CurrentSubscriptions = new HashSet(); + internal HashSet CurrentSubscriptions = new HashSet(); /// public IReadOnlyCollection Services { get { return this.CurrentServices; } } @@ -188,9 +188,9 @@ internal void DisposeFromContext() } this.CurrentPublishers.Clear(); - foreach (IDisposable disposable in this.CurrentSubscriptions) + foreach (IRawSubscription subscription in this.CurrentSubscriptions) { - disposable.Dispose(); + subscription.DisposeFromNode(); } this.CurrentSubscriptions.Clear(); diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 19885f97..75e9dd5c 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -13,122 +13,140 @@ // limitations under the License. using System; +using System.Threading.Tasks; 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; + /// Subscription to a topic with a given type. + /// + internal sealed class Subscription : ISubscription, IRawSubscription where T : Message, new() + { + /// + public string Topic { get; private set; } - public string Topic { get { return topic; } } - private string topic; + /// + public bool IsDisposed + { + get { return !NativeRcl.rcl_subscription_is_valid(this.Handle); } + } - public bool IsDisposed { get { return disposed; } } - private bool disposed = false; + private IntPtr Handle = IntPtr.Zero; - private rcl_node_t nodeHandle; - private readonly Action callback; - private IntPtr subscriptionOptions; + private IntPtr Options = IntPtr.Zero; - public object Mutex { get { return mutex; } } - private object mutex = new object(); + private readonly Node Node; - /// 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() - { - RCLReturnEnum ret; - MessageInternals message; - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + private readonly Action Callback; + + internal Subscription(string topic, Node node, Action callback, QualityOfServiceProfile qos = null) { - return; + 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); + } } - message = CreateMessage(); - ret = (RCLReturnEnum)NativeRcl.rcl_take(ref subscriptionHandle, message.Handle, IntPtr.Zero, IntPtr.Zero); - } - - bool gotMessage = ret == RCLReturnEnum.RCL_RET_OK; + /// + public bool TryProcess() + { + if (this.IsDisposed) + { + return false; + } + + T message = new T(); + int ret = NativeRcl.rcl_take( + this.Handle, + (message as MessageInternals).Handle, + IntPtr.Zero, + IntPtr.Zero + ); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_SUBSCRIPTION_TAKE_FAILED) + { + Utils.CheckReturnEnum(ret); + (message as MessageInternals).ReadNativeMessage(); + this.Callback(message); + return true; + } + return false; + } - if (gotMessage) - { - TriggerCallback(message); - } - } + /// + public Task TryProcessAsync() + { + return Task.FromResult(this.TryProcess()); + } - /// Construct a message of the subscription type - private MessageInternals CreateMessage() - { - return new T() as MessageInternals; - } + /// + 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 + if (disposing) + { + this.Node.CurrentSubscriptions.Remove(this); + this.Node.Executor?.Wake(this.Node); + } + + this.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)); - } + /// + public void 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(); - } + 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/interfaces/ISubscription.cs b/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs index f18a732c..319329b3 100644 --- a/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs +++ b/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs @@ -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_core/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index e5f716ce..e92b23fa 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -289,16 +289,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, IntPtr 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( @@ -307,7 +298,7 @@ internal static SubscriptionInitType typeof(SubscriptionInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int SubscriptionFiniType(ref rcl_subscription_t subscription, IntPtr node); + internal delegate int SubscriptionFiniType(IntPtr subscription, IntPtr node); internal static SubscriptionFiniType rcl_subscription_fini = (SubscriptionFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -316,7 +307,7 @@ internal static SubscriptionFiniType typeof(SubscriptionFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool SubscriptionIsValidType(ref rcl_subscription_t subscription); + internal delegate bool SubscriptionIsValidType(IntPtr subscription); internal static SubscriptionIsValidType rcl_subscription_is_valid = (SubscriptionIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -325,7 +316,7 @@ internal static SubscriptionIsValidType 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( @@ -392,7 +383,7 @@ 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 delegate int WaitSetAddSubscriptionType(ref rcl_wait_set_t wait_set, IntPtr subscription, ref UIntPtr index); internal static WaitSetAddSubscriptionType rcl_wait_set_add_subscription = (WaitSetAddSubscriptionType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index ccfa491c..cbe699be 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -109,6 +109,24 @@ 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)] internal delegate IntPtr SubscriptionCreateOptionsType(IntPtr qos); internal static SubscriptionCreateOptionsType diff --git a/src/ros2cs/ros2cs_core/native/NativeTypes.cs b/src/ros2cs/ros2cs_core/native/NativeTypes.cs index 473094ab..70377909 100644 --- a/src/ros2cs/ros2cs_core/native/NativeTypes.cs +++ b/src/ros2cs/ros2cs_core/native/NativeTypes.cs @@ -52,11 +52,6 @@ public struct rcl_init_options_t private IntPtr impl; } - public struct rcl_subscription_t - { - private IntPtr impl; - } - public struct rcl_client_t { private IntPtr impl; diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index e98d377b..0b618f34 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -84,6 +84,20 @@ 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 rcl_subscription_options_t *rclcs_subscription_create_options(rmw_qos_profile_t * qos) { From 40f53d9033b400f68d057121b0374b47964d1708 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 18:16:13 +0100 Subject: [PATCH 07/46] refactor Service - is now sealed and internal - rcl_service_t size is now invisible to C# code - removes himself from node on dispose - prevents Node from being collected by the GC --- src/ros2cs/ros2cs_core/Node.cs | 6 +- src/ros2cs/ros2cs_core/Service.cs | 278 ++++++++++-------- src/ros2cs/ros2cs_core/interfaces/IService.cs | 56 ++-- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 29 +- .../ros2cs_core/native/NativeRclInterface.cs | 18 ++ src/ros2cs/ros2cs_core/native/NativeTypes.cs | 5 - .../ros2cs_core/native/rcl_native_interface.c | 14 + 7 files changed, 224 insertions(+), 182 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 597ff981..9ce9e90a 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -54,7 +54,7 @@ public bool IsDisposed /// public IReadOnlyCollection Services { get { return this.CurrentServices; } } - private HashSet CurrentServices = new HashSet(); + internal HashSet CurrentServices = new HashSet(); /// public IReadOnlyCollection Clients { get { return this.CurrentClients; } } @@ -194,9 +194,9 @@ internal void DisposeFromContext() } this.CurrentSubscriptions.Clear(); - foreach (IDisposable disposable in this.CurrentServices) + foreach (IRawService service in this.CurrentServices) { - disposable.Dispose(); + service.DisposeFromNode(); } this.CurrentServices.Clear(); diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 73b84a6d..889237c7 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -13,150 +13,174 @@ // limitations under the License. using System; +using System.Threading.Tasks; 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 - /// - private readonly Func callback; - private IntPtr serviceOptions; - - /// - public object Mutex { get { return mutex; } } - private object mutex = new object(); - - /// - /// Internal constructor for Service - /// - /// Use to construct new Instances - internal Service(string subTopic, Node node, Func cb, QualityOfServiceProfile qos = null) + /// Service with a topic and Types for Messages. + /// + internal sealed class Service : IService, IRawService + where I : Message, new() + where O : Message, new() { - 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)); - } + /// + public string Topic { get; private set; } - /// - /// 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); - } + /// + public bool IsDisposed + { + get { return !NativeRcl.rcl_service_is_valid(this.Handle); } + } - /// - // 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; + private IntPtr Handle = IntPtr.Zero; + + private IntPtr Options = IntPtr.Zero; + + private readonly Node Node; - lock (mutex) - { - if (disposed || !Ros2cs.Ok()) + /// + /// Callback to be called to process incoming requests + /// + private readonly Func Callback; + + internal Service(string topic, Node node, Func callback, QualityOfServiceProfile qos = null) { - return; + this.Topic = topic; + this.Node = node; + this.Callback = callback; + + QualityOfServiceProfile qualityOfServiceProfile = qos ?? new QualityOfServiceProfile(); + + 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); + } } - message = new I() as MessageInternals; - ret = (RCLReturnEnum)NativeRcl.rcl_take_request(ref serviceHandle, ref header, message.Handle); - } + /// + public bool TryProcess() + { + if (this.IsDisposed) + { + return false; + } + + 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 + ); + + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_SERIVCE_TAKE_FAILD) + { + Utils.CheckReturnEnum(ret); + this.ProcessRequest(header, message); + return true; + } + return false; + } - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_OK) - { - ProcessRequest(header, message); - } - } + /// + public Task TryProcessAsync() + { + return Task.FromResult(this.TryProcess()); + } - /// - /// 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); - } + /// + /// 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)); + } - ~Service() - { - DestroyService(); - } + /// + /// 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) + { + MessageInternals msgInternals = msg as MessageInternals; + msgInternals.WriteNativeMessage(); + Utils.CheckReturnEnum(NativeRcl.rcl_send_response( + this.Handle, + ref header, + msgInternals.Handle + )); + } - public void Dispose() - { - DestroyService(); - } + /// + public void Dispose() + { + this.Dispose(true); + // finalizer not needed when we disposed successfully + GC.SuppressFinalize(this); + } - /// "Destructor" supporting disposable model - private void DestroyService() - { - lock (mutex) - { - if (!disposed) + /// 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 + if (disposing) + { + this.Node.CurrentServices.Remove(this); + this.Node.Executor?.Wake(this.Node); + } + + this.DisposeFromNode(); + } + + /// + public void DisposeFromNode() + { + if (this.Handle == IntPtr.Zero) + { + return; + } + + Utils.CheckReturnEnum(NativeRcl.rcl_service_fini(this.Handle, this.Node.Handle)); + this.FreeHandles(); + } + + private void FreeHandles() + { + NativeRclInterface.rclcs_free_service(this.Handle); + this.Handle = IntPtr.Zero; + NativeRclInterface.rclcs_service_dispose_options(this.Options); + this.Options = IntPtr.Zero; + } + + ~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/interfaces/IService.cs b/src/ros2cs/ros2cs_core/interfaces/IService.cs index 51275ef4..a8bfad20 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IService.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IService.cs @@ -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/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index e92b23fa..66437745 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -198,16 +198,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, IntPtr 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( @@ -216,7 +207,16 @@ internal static ServiceInitType typeof(ServiceInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ServiceFiniType(ref rcl_service_t client, IntPtr node); + internal delegate bool ServiceIsValidType(IntPtr service); + internal static ServiceIsValidType + rcl_service_is_valid = + (ServiceIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeRCL, + "rcl_service_is_valid"), + typeof(ServiceIsValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int ServiceFiniType(IntPtr service, IntPtr node); internal static ServiceFiniType rcl_service_fini = (ServiceFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -225,7 +225,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, ref rcl_rmw_request_id_t request_header, IntPtr message_handle); internal static TakeRequestType rcl_take_request = (TakeRequestType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -234,8 +234,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, ref rcl_rmw_request_id_t request_header, IntPtr responce_info); internal static SendResponceType rcl_send_response = (SendResponceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -401,7 +400,7 @@ internal static WaitSetAddClientType typeof(WaitSetAddClientType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetAddServiceType(ref rcl_wait_set_t wait_set, ref rcl_service_t service, ref UIntPtr index); + internal delegate int WaitSetAddServiceType(ref rcl_wait_set_t wait_set, IntPtr service, ref UIntPtr index); internal static WaitSetAddServiceType rcl_wait_set_add_service = (WaitSetAddServiceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index cbe699be..1e69d833 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -199,6 +199,24 @@ 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)] 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 70377909..83d329c4 100644 --- a/src/ros2cs/ros2cs_core/native/NativeTypes.cs +++ b/src/ros2cs/ros2cs_core/native/NativeTypes.cs @@ -57,11 +57,6 @@ 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 { diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index 0b618f34..b0b8db2c 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -157,6 +157,20 @@ void rclcs_client_dispose_options(rcl_client_options_t * client_options_handle) free(client_options_handle); } +ROSIDL_GENERATOR_C_EXPORT +rcl_subscription_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 rcl_service_options_t *rclcs_service_create_options(rmw_qos_profile_t * qos) { From 7858e3df9910e533c6ab850e7f36f299f4abb2fb Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 29 Dec 2022 18:21:48 +0100 Subject: [PATCH 08/46] fix missing includes in rcl_native_interface.c --- src/ros2cs/ros2cs_core/native/rcl_native_interface.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index b0b8db2c..9a521ffd 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -15,6 +15,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -158,7 +161,7 @@ void rclcs_client_dispose_options(rcl_client_options_t * client_options_handle) } ROSIDL_GENERATOR_C_EXPORT -rcl_subscription_t * rclcs_get_zero_initialized_service() +rcl_service_t * rclcs_get_zero_initialized_service() { rcl_service_t * service = malloc(sizeof(rcl_service_t)); *service = rcl_get_zero_initialized_service(); From d314def7d8362489e9ab696e84c51e64cd4ad23c Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 30 Dec 2022 13:13:52 +0100 Subject: [PATCH 09/46] rename MappingValueView -> MappedValueDictionary rename src/ros2cs/ros2cs_core/utils/{MappingValueView.cs => MappedValueDictionary.cs} (87%) --- src/ros2cs/ros2cs_core/Context.cs | 2 +- .../{MappingValueView.cs => MappedValueDictionary.cs} | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) rename src/ros2cs/ros2cs_core/utils/{MappingValueView.cs => MappedValueDictionary.cs} (87%) diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs index da62d3f0..997218a3 100644 --- a/src/ros2cs/ros2cs_core/Context.cs +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -49,7 +49,7 @@ public static string GetRMWImplementation() public Context() { - this.Nodes = new MappingValueView(this.ROSNodes, node => node); + this.Nodes = new MappedValueDictionary(this.ROSNodes, node => node); this.ROSAllocator = NativeRcl.rcutils_get_default_allocator(); this.Handle = NativeRclInterface.rclcs_get_zero_initialized_context(); int ret = NativeRclInterface.rclcs_init(this.Handle, this.ROSAllocator); diff --git a/src/ros2cs/ros2cs_core/utils/MappingValueView.cs b/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs similarity index 87% rename from src/ros2cs/ros2cs_core/utils/MappingValueView.cs rename to src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs index f7611a22..2ddb14cd 100644 --- a/src/ros2cs/ros2cs_core/utils/MappingValueView.cs +++ b/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs @@ -1,20 +1,20 @@ using System; -using System.Linq; using System.Collections; using System.Collections.Generic; +using System.Linq; namespace ROS2 { /// - /// Dictionary view wich transforms the values of the wrapped dictionary. + /// Dictionary view which transforms the values of the wrapped dictionary. /// - internal class MappingValueView : IReadOnlyDictionary + internal sealed class MappedValueDictionary : IReadOnlyDictionary { private readonly IReadOnlyDictionary Wrapped; private readonly Func Mapper; - public MappingValueView(IReadOnlyDictionary wrapped, Func mapper) + public MappedValueDictionary(IReadOnlyDictionary wrapped, Func mapper) { this.Wrapped = wrapped; this.Mapper = mapper; @@ -79,4 +79,4 @@ IEnumerator IEnumerable.GetEnumerator() return this.GetEnumerator(); } } -} \ No newline at end of file +} From c3fbc8048bba49c87c45748afa3da09250b7b75e Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 30 Dec 2022 13:15:31 +0100 Subject: [PATCH 10/46] add LockedDictionary create mode 100644 src/ros2cs/ros2cs_core/utils/LockedDictionary.cs --- .../ros2cs_core/utils/LockedDictionary.cs | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 src/ros2cs/ros2cs_core/utils/LockedDictionary.cs diff --git a/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs b/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs new file mode 100644 index 00000000..fe784369 --- /dev/null +++ b/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs @@ -0,0 +1,106 @@ +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(); + } + } +} From 33d3be0ac3d082c268d8c528ec75aa257f259fce Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 30 Dec 2022 13:16:21 +0100 Subject: [PATCH 11/46] fix service default qos --- src/ros2cs/ros2cs_core/Service.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 889237c7..6e0e3265 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -50,7 +50,7 @@ internal Service(string topic, Node node, Func callback, QualityOfServiceP this.Node = node; this.Callback = callback; - QualityOfServiceProfile qualityOfServiceProfile = qos ?? new QualityOfServiceProfile(); + QualityOfServiceProfile qualityOfServiceProfile = qos ?? new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT); this.Options = NativeRclInterface.rclcs_service_create_options(qualityOfServiceProfile.handle); From 4a24200960723293f67a24298c92ae256193076a Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 30 Dec 2022 13:19:40 +0100 Subject: [PATCH 12/46] refactor Client - is now sealed and internal - rcl_client_t size is now invisible to C# code - removes himself from node on dispose - prevents Node from being collected by the GC - reduced code duplication --- src/ros2cs/ros2cs_core/Client.cs | 578 ++++++++---------- src/ros2cs/ros2cs_core/Node.cs | 6 +- src/ros2cs/ros2cs_core/interfaces/IClient.cs | 127 ++-- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 30 +- .../ros2cs_core/native/NativeRclInterface.cs | 18 + src/ros2cs/ros2cs_core/native/NativeTypes.cs | 5 - .../ros2cs_core/native/rcl_native_interface.c | 15 + 7 files changed, 354 insertions(+), 425 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index fd39ab16..538bcc49 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -13,9 +13,7 @@ // limitations under the License. using System; -using System.Collections; using System.Collections.Generic; -using System.Diagnostics; using System.Linq; using System.Threading.Tasks; using ROS2.Internal; @@ -23,393 +21,303 @@ 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 + /// Client with a topic and Types for Messages. + /// + internal sealed class Client : IClient, IRawClient 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 . - /// - /// - /// The is stored separately to allow - /// to work even if the source returns multiple tasks. - /// - 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; + /// + public string Topic { get; private set; } - QualityOfServiceProfile qualityOfServiceProfile = qos; - if (qualityOfServiceProfile == null) - qualityOfServiceProfile = new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT); + /// + public IReadOnlyDictionary> PendingRequests { get; private set; } - Requests = new Dictionary, Task)>(); - PendingRequests = new PendingTasksView(Requests); + /// + IReadOnlyDictionary IClientBase.PendingRequests { get { return this.UntypedPendingRequests; } } - serviceOptions = NativeRclInterface.rclcs_client_create_options(qualityOfServiceProfile.handle); + private readonly IReadOnlyDictionary UntypedPendingRequests; - IntPtr typeSupportHandle = MessageTypeSupportHelper.GetTypeSupportHandle(); + /// + public bool IsDisposed + { + get { return !NativeRcl.rcl_client_is_valid(this.Handle); } + } - serviceHandle = NativeRcl.rcl_get_zero_initialized_client(); - Utils.CheckReturnEnum(NativeRcl.rcl_client_init( - ref serviceHandle, - ref nodeHandle, - typeSupportHandle, - topic, - serviceOptions)); - } + private IntPtr Handle = IntPtr.Zero; - ~Client() - { - Dispose(); - } + private IntPtr Options = IntPtr.Zero; - public void Dispose() - { - DestroyClient(); - } + private readonly Node Node; - /// "Destructor" supporting disposable model - private void DestroyClient() - { - lock (mutex) - { - if (!disposed) + /// + /// Mapping from request id without Response to . + /// + /// + /// The is stored separately to allow + /// to work even if the source returns multiple tasks. + /// + private readonly Dictionary, Task)> Requests = new Dictionary, Task)>(); + + /// + /// Internal constructor for Client + /// + /// Use to construct new Instances + public Client(string topic, Node node, QualityOfServiceProfile qos = null) { - lock (Requests) - { - foreach (var source in Requests.Values) + 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) { - bool success = source.Item1.TrySetException(new ObjectDisposedException("client has been disposed")); - Debug.Assert(success); + this.FreeHandles(); + Utils.CheckReturnEnum(ret); } - 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()) + /// + /// Assert that the client has not been disposed. + /// + private void AssertOk() { - return; + if (this.IsDisposed) + { + throw new ObjectDisposedException($"client for topic '{this.Topic}'"); + } } - 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)) + /// + public bool IsServiceAvailable() { - exists = true; - Requests.Remove(sequence_number); + this.AssertOk(); + bool available = false; + Utils.CheckReturnEnum(NativeRcl.rcl_service_server_is_available( + this.Node.Handle, + this.Handle, + ref available + )); + 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 bool TryProcess() + { + if (this.IsDisposed) + { + return false; + } - /// - public O Call(I msg) - { - var task = CallAsync(msg); - task.Wait(); - return task.Result; - } + 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) - { - return CallAsync(msg, TaskCreationOptions.None); - } + lock (this.Requests) + { + // prevent taking responses before RegisterSource was called + int ret = NativeRcl.rcl_take_response( + this.Handle, + ref header, + (message as MessageInternals).Handle + ); + + if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_CLIENT_TAKE_FAILED) + { + return false; + } + Utils.CheckReturnEnum(ret); + 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); + } + return true; + } - /// - 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); - } - } + /// + public Task TryProcessAsync() + { + return Task.FromResult(this.TryProcess()); + } - /// - public bool Cancel(Task task) - { - var pair = default(KeyValuePair, Task)>); - try - { - lock(this.Requests) + /// + 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 + /// + 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]; } - } + /// + public Task CallAsync(I msg, TaskCreationOptions options) + { + this.AssertOk(); + 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, + ref sequence_number + ) + ); + 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; } - } + /// + public bool Cancel(Task task) + { + var pair = default(KeyValuePair, Task)>); + try + { + lock (this.Requests) + { + pair = this.Requests.First(entry => entry.Value.Item2 == task); + // has to be true + this.Requests.Remove(pair.Key); + } + } + catch (InvalidOperationException) + { + return false; + } + pair.Value.Item1.SetCanceled(); + return true; + } - public int Count - { - get + /// + 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 + if (disposing) + { + this.Node.CurrentClients.Remove(this); + this.Node.Executor?.Wake(this.Node); + 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) + /// + public void 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) + 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) + + 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/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 9ce9e90a..e36c807b 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -59,7 +59,7 @@ public bool IsDisposed /// public IReadOnlyCollection Clients { get { return this.CurrentClients; } } - private HashSet CurrentClients = new HashSet(); + internal HashSet CurrentClients = new HashSet(); internal Node(string name, Context context) { @@ -200,9 +200,9 @@ internal void DisposeFromContext() } this.CurrentServices.Clear(); - foreach (IDisposable disposable in this.CurrentClients) + foreach (IRawClient client in this.CurrentClients) { - disposable.Dispose(); + client.DisposeFromNode(); } this.CurrentClients.Clear(); diff --git a/src/ros2cs/ros2cs_core/interfaces/IClient.cs b/src/ros2cs/ros2cs_core/interfaces/IClient.cs index 96a4e1a2..56a6a9cb 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IClient.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IClient.cs @@ -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 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; } - /// - /// 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/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index 66437745..7aacd311 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -135,16 +135,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, IntPtr 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( @@ -153,7 +144,16 @@ internal static ClientInitType typeof(ClientInitType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ClientFiniType(ref rcl_client_t client, IntPtr node); + internal delegate bool ClientIsValidType(IntPtr client); + internal static ClientIsValidType + rcl_client_is_valid = + (ClientIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeRCL, + "rcl_client_is_valid"), + typeof(ClientIsValidType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int ClientFiniType(IntPtr client, IntPtr node); internal static ClientFiniType rcl_client_fini = (ClientFiniType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -162,7 +162,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, ref long sequence_number); internal static SendRequestType rcl_send_request = (SendRequestType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -171,7 +171,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, ref rcl_rmw_request_id_t request_header, IntPtr ros_response); internal static TakeResponceType rcl_take_response = (TakeResponceType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -180,7 +180,7 @@ internal static TakeResponceType typeof(TakeResponceType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ServiceIsAvailableType(IntPtr node, ref rcl_client_t client, ref bool is_available); + internal delegate int ServiceIsAvailableType(IntPtr node, IntPtr client, ref bool is_available); internal static ServiceIsAvailableType rcl_service_server_is_available = (ServiceIsAvailableType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -391,7 +391,7 @@ internal static WaitSetAddSubscriptionType typeof(WaitSetAddSubscriptionType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetAddClientType(ref rcl_wait_set_t wait_set, ref rcl_client_t client, ref UIntPtr index); + internal delegate int WaitSetAddClientType(ref rcl_wait_set_t wait_set, IntPtr client, ref UIntPtr index); internal static WaitSetAddClientType rcl_wait_set_add_client = (WaitSetAddClientType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index 1e69d833..d642a540 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -181,6 +181,24 @@ 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)] internal delegate IntPtr ClientCreateOptionsType(IntPtr qos); internal static ClientCreateOptionsType diff --git a/src/ros2cs/ros2cs_core/native/NativeTypes.cs b/src/ros2cs/ros2cs_core/native/NativeTypes.cs index 83d329c4..f16d6b28 100644 --- a/src/ros2cs/ros2cs_core/native/NativeTypes.cs +++ b/src/ros2cs/ros2cs_core/native/NativeTypes.cs @@ -52,11 +52,6 @@ public struct rcl_init_options_t private IntPtr impl; } - public struct rcl_client_t - { - private IntPtr impl; - } - [StructLayout(LayoutKind.Sequential)] public struct rcl_rmw_request_id_t { diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index 9a521ffd..c4a5fa92 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -145,6 +146,20 @@ 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 rcl_client_options_t *rclcs_client_create_options(rmw_qos_profile_t * qos) { From dabf904bf8e537c7b8074734e900efe14f43dceb Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 30 Dec 2022 14:36:32 +0100 Subject: [PATCH 13/46] add debug assertions These assertions are only enabled in debug mode and should make finding bugs easier. --- src/ros2cs/ros2cs_core/Client.cs | 25 ++++++++++++++++--------- src/ros2cs/ros2cs_core/Node.cs | 18 +++++++++++++----- src/ros2cs/ros2cs_core/Publisher.cs | 4 +++- src/ros2cs/ros2cs_core/Service.cs | 4 +++- src/ros2cs/ros2cs_core/Subscription.cs | 4 +++- 5 files changed, 38 insertions(+), 17 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index 538bcc49..7f0e7f0b 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -14,6 +14,7 @@ using System; using System.Collections.Generic; +using System.Diagnostics; using System.Linq; using System.Threading.Tasks; using ROS2.Internal; @@ -162,6 +163,10 @@ public bool TryProcess() (message as MessageInternals).ReadNativeMessage(); source.Item1.SetResult(message); } + else + { + Debug.Print("received request which was not pending, maybe canceled"); + } return true; } @@ -236,18 +241,19 @@ private Task RegisterSource(TaskCompletionSource source, long sequence_num public bool Cancel(Task task) { var pair = default(KeyValuePair, Task)>); - try + lock (this.Requests) { - lock (this.Requests) + try { pair = this.Requests.First(entry => entry.Value.Item2 == task); - // has to be true - this.Requests.Remove(pair.Key); } - } - catch (InvalidOperationException) - { - return false; + 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; @@ -273,7 +279,8 @@ private void Dispose(bool disposing) // only do if Node.CurrentClients and this.Requests have not been finalized if (disposing) { - this.Node.CurrentClients.Remove(this); + bool success = this.Node.CurrentClients.Remove(this); + Debug.Assert(success, "failed to remove client"); this.Node.Executor?.Wake(this.Node); this.DisposeAllTasks(); } diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index e36c807b..ac8fc9a7 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -14,6 +14,7 @@ using System; using System.Collections.Generic; +using System.Diagnostics; namespace ROS2 { @@ -121,7 +122,8 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) { this.AssertOk(); Publisher publisher = new Publisher(topic, this, qos); - this.CurrentPublishers.Add(publisher); + bool success = this.CurrentPublishers.Add(publisher); + Debug.Assert(success, "publisher already exists"); return publisher; } @@ -130,7 +132,8 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) { this.AssertOk(); Subscription subscription = new Subscription(topic, this, callback, qos); - this.CurrentSubscriptions.Add(subscription); + bool success = this.CurrentSubscriptions.Add(subscription); + Debug.Assert(success, "subscription already exists"); this.Executor?.Wake(this); return subscription; } @@ -140,7 +143,8 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) { this.AssertOk(); Client client = new Client(topic, this, qos); - this.CurrentClients.Add(client); + bool success = this.CurrentClients.Add(client); + Debug.Assert(success, "client already exists"); this.Executor?.Wake(this); return client; } @@ -150,7 +154,8 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) { this.AssertOk(); Service service = new Service(topic, this, callback, qos); - this.CurrentServices.Add(service); + bool success = this.CurrentServices.Add(service); + Debug.Assert(success, "service already exists"); this.Executor?.Wake(this); return service; } @@ -162,9 +167,12 @@ public void Dispose() { 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(); - this.ROSContext.RemoveNode(this.Name); } /// diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index 4d34fe20..568313cd 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -13,6 +13,7 @@ // limitations under the License. using System; +using System.Diagnostics; using ROS2.Internal; namespace ROS2 @@ -102,7 +103,8 @@ private void Dispose(bool disposing) // only do if Node.CurrentPublishers has not been finalized if (disposing) { - this.Node.CurrentPublishers.Remove(this); + bool success = this.Node.CurrentPublishers.Remove(this); + Debug.Assert(success, "failed to remove publisher"); } this.DisposeFromNode(); diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 6e0e3265..d2bb12f5 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -13,6 +13,7 @@ // limitations under the License. using System; +using System.Diagnostics; using System.Threading.Tasks; using ROS2.Internal; @@ -151,7 +152,8 @@ private void Dispose(bool disposing) // only do if Node.CurrentServices has not been finalized if (disposing) { - this.Node.CurrentServices.Remove(this); + bool success = this.Node.CurrentServices.Remove(this); + Debug.Assert(success, "failed to remove service"); this.Node.Executor?.Wake(this.Node); } diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 75e9dd5c..b060823c 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -13,6 +13,7 @@ // limitations under the License. using System; +using System.Diagnostics; using System.Threading.Tasks; using ROS2.Internal; @@ -117,7 +118,8 @@ private void Dispose(bool disposing) // only do if Node.CurrentSubscriptions has not been finalized if (disposing) { - this.Node.CurrentSubscriptions.Remove(this); + bool success = this.Node.CurrentSubscriptions.Remove(this); + Debug.Assert(success, "failed to remove subscription"); this.Node.Executor?.Wake(this.Node); } From d6b5e728232d17f3d374da66ca66ae20ebf6dd4f Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Sat, 31 Dec 2022 18:52:38 +0100 Subject: [PATCH 14/46] add calls to `GC.KeepAlive(this)` This should prevent the GC from starting finalization while a native call is running. --- src/ros2cs/ros2cs_core/Client.cs | 10 +++++++++- src/ros2cs/ros2cs_core/Publisher.cs | 8 +++++++- src/ros2cs/ros2cs_core/Service.cs | 9 ++++++++- src/ros2cs/ros2cs_core/Subscription.cs | 9 ++++++++- 4 files changed, 32 insertions(+), 4 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index 7f0e7f0b..71fce56d 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -42,7 +42,12 @@ internal sealed class Client : IClient, IRawClient /// public bool IsDisposed { - get { return !NativeRcl.rcl_client_is_valid(this.Handle); } + get + { + bool ok = NativeRcl.rcl_client_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } } private IntPtr Handle = IntPtr.Zero; @@ -122,6 +127,7 @@ public bool IsServiceAvailable() this.Handle, ref available )); + GC.KeepAlive(this); return available; } @@ -146,6 +152,7 @@ public bool TryProcess() ref header, (message as MessageInternals).Handle ); + GC.KeepAlive(this); if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_CLIENT_TAKE_FAILED) { @@ -220,6 +227,7 @@ private long SendRequest(I msg) ref sequence_number ) ); + GC.KeepAlive(this); return sequence_number; } diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index 568313cd..528cc39b 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -28,7 +28,12 @@ namespace ROS2 /// public bool IsDisposed { - get { return !NativeRcl.rcl_publisher_is_valid(this.Handle); } + get + { + bool ok = NativeRcl.rcl_publisher_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } } private IntPtr Handle = IntPtr.Zero; @@ -81,6 +86,7 @@ public void Publish(T msg) MessageInternals msgInternals = msg as MessageInternals; msgInternals.WriteNativeMessage(); Utils.CheckReturnEnum(NativeRcl.rcl_publish(this.Handle, msgInternals.Handle, IntPtr.Zero)); + GC.KeepAlive(this); } /// diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index d2bb12f5..e6153012 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -31,7 +31,12 @@ internal sealed class Service : IService, IRawService /// public bool IsDisposed { - get { return !NativeRcl.rcl_service_is_valid(this.Handle); } + get + { + bool ok = NativeRcl.rcl_service_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } } private IntPtr Handle = IntPtr.Zero; @@ -88,6 +93,7 @@ public bool TryProcess() ref header, (message as MessageInternals).Handle ); + GC.KeepAlive(this); if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_SERIVCE_TAKE_FAILD) { @@ -130,6 +136,7 @@ private void SendResp(rcl_rmw_request_id_t header, O msg) ref header, msgInternals.Handle )); + GC.KeepAlive(this); } /// diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index b060823c..98884bdc 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -29,7 +29,12 @@ namespace ROS2 /// public bool IsDisposed { - get { return !NativeRcl.rcl_subscription_is_valid(this.Handle); } + get + { + bool ok = NativeRcl.rcl_subscription_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } } private IntPtr Handle = IntPtr.Zero; @@ -82,6 +87,8 @@ public bool TryProcess() IntPtr.Zero, IntPtr.Zero ); + GC.KeepAlive(this); + if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_SUBSCRIPTION_TAKE_FAILED) { Utils.CheckReturnEnum(ret); From 83a2a2b09145b78d3bd5532dec3b55dc4fda594a Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 4 Jan 2023 19:04:45 +0100 Subject: [PATCH 15/46] add internal Removal methods to `Node` This enforces more encapsulation than direct internal access. --- src/ros2cs/ros2cs_core/Client.cs | 2 +- src/ros2cs/ros2cs_core/Node.cs | 64 +++++++++++++++++++++++--- src/ros2cs/ros2cs_core/Publisher.cs | 2 +- src/ros2cs/ros2cs_core/Service.cs | 2 +- src/ros2cs/ros2cs_core/Subscription.cs | 2 +- 5 files changed, 62 insertions(+), 10 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index 71fce56d..c8bb8efd 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -287,7 +287,7 @@ private void Dispose(bool disposing) // only do if Node.CurrentClients and this.Requests have not been finalized if (disposing) { - bool success = this.Node.CurrentClients.Remove(this); + bool success = this.Node.RemoveClient(this); Debug.Assert(success, "failed to remove client"); this.Node.Executor?.Wake(this.Node); this.DisposeAllTasks(); diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index ac8fc9a7..98d95c47 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -38,29 +38,29 @@ public bool IsDisposed internal IntPtr Handle = IntPtr.Zero; - private Context ROSContext; - private IntPtr Options; + private readonly Context ROSContext; + /// public IReadOnlyCollection Publishers { get { return this.CurrentPublishers; } } - internal HashSet CurrentPublishers = new HashSet(); + private readonly HashSet CurrentPublishers = new HashSet(); /// public IReadOnlyCollection Subscriptions { get { return this.CurrentSubscriptions; } } - internal HashSet CurrentSubscriptions = new HashSet(); + private readonly HashSet CurrentSubscriptions = new HashSet(); /// public IReadOnlyCollection Services { get { return this.CurrentServices; } } - internal HashSet CurrentServices = new HashSet(); + private readonly HashSet CurrentServices = new HashSet(); /// public IReadOnlyCollection Clients { get { return this.CurrentClients; } } - internal HashSet CurrentClients = new HashSet(); + private readonly HashSet CurrentClients = new HashSet(); internal Node(string name, Context context) { @@ -127,6 +127,19 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) return publisher; } + /// + /// Remove a publisher. + /// + /// + /// This method is intended to be used by and does not dispose the publisher. + /// + /// Publisher to be removed. + /// If the publisher existed on this node and has been removed. + internal bool RemovePublisher(IRawPublisher publisher) + { + return this.CurrentPublishers.Remove(publisher); + } + /// public ISubscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new() { @@ -138,6 +151,19 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) return subscription; } + /// + /// Remove a subscription. + /// + /// + /// This method is intended to be used by and does not dispose the subscription. + /// + /// Subscription to be removed. + /// If the subscription existed on this node and has been removed. + internal bool RemoveSubscription(IRawSubscription subscription) + { + return this.CurrentSubscriptions.Remove(subscription); + } + /// public IClient CreateClient(string topic, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() { @@ -149,6 +175,19 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) return client; } + /// + /// Remove a client. + /// + /// + /// This method is intended to be used by and does not dispose the client. + /// + /// Client to be removed. + /// If the client existed on this node and has been removed. + internal bool RemoveClient(IRawClient client) + { + return this.CurrentClients.Remove(client); + } + /// public IService CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() { @@ -160,6 +199,19 @@ public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) return service; } + /// + /// Remove a service. + /// + /// + /// This method is intended to be used by and does not dispose the service. + /// + /// Service to be removed. + /// If the service existed on this node and has been removed. + internal bool RemoveService(IRawService service) + { + return this.CurrentServices.Remove(service); + } + /// public void Dispose() { diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index 528cc39b..a1ebbd39 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -109,7 +109,7 @@ private void Dispose(bool disposing) // only do if Node.CurrentPublishers has not been finalized if (disposing) { - bool success = this.Node.CurrentPublishers.Remove(this); + bool success = this.Node.RemovePublisher(this); Debug.Assert(success, "failed to remove publisher"); } diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index e6153012..de3f4d72 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -159,7 +159,7 @@ private void Dispose(bool disposing) // only do if Node.CurrentServices has not been finalized if (disposing) { - bool success = this.Node.CurrentServices.Remove(this); + bool success = this.Node.RemoveService(this); Debug.Assert(success, "failed to remove service"); this.Node.Executor?.Wake(this.Node); } diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 98884bdc..f18554e1 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -125,7 +125,7 @@ private void Dispose(bool disposing) // only do if Node.CurrentSubscriptions has not been finalized if (disposing) { - bool success = this.Node.CurrentSubscriptions.Remove(this); + bool success = this.Node.RemoveSubscription(this); Debug.Assert(success, "failed to remove subscription"); this.Node.Executor?.Wake(this.Node); } From 0fd008acc4e5b32245a8003ffd931e1ffa4ff383 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 4 Jan 2023 01:05:56 +0100 Subject: [PATCH 16/46] refactor `IExecutor` interface Make setting `INode.Executor` the task of the executor since `INode.SwapExecutor` is performing two actions at once (adding and removing) which can complicate error handling. Furthermore, `IExecutor.Wake` is split to allow notifying executors that changes occurred without being forced to wait. --- src/ros2cs/ros2cs_core/Client.cs | 3 +- src/ros2cs/ros2cs_core/Node.cs | 59 ++++++++----------- src/ros2cs/ros2cs_core/Service.cs | 3 +- src/ros2cs/ros2cs_core/Subscription.cs | 3 +- .../ros2cs_core/interfaces/IExecutor.cs | 37 ++++++++---- src/ros2cs/ros2cs_core/interfaces/INode.cs | 22 ++----- 6 files changed, 63 insertions(+), 64 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index c8bb8efd..5ae0703f 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -289,7 +289,8 @@ private void Dispose(bool disposing) { bool success = this.Node.RemoveClient(this); Debug.Assert(success, "failed to remove client"); - this.Node.Executor?.Wake(this.Node); + // not required if we are being finalized since the executor cant be running + this.Node.Executor?.Wait(); this.DisposeAllTasks(); } diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 98d95c47..14086be5 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -19,7 +19,7 @@ namespace ROS2 { /// - internal sealed class Node : INode, IExtendedDisposable + internal sealed class Node : INode { /// public string Name { get; private set; } @@ -28,7 +28,7 @@ internal sealed class Node : INode, IExtendedDisposable public IContext Context { get { return this.ROSContext; } } /// - public IExecutor Executor { get; private set; } + public IExecutor Executor { get; set; } /// public bool IsDisposed @@ -94,29 +94,6 @@ private void AssertOk() } } - /// - public bool TrySetExecutor(IExecutor executor) - { - return this.TrySetExecutor(executor, out _); - } - - /// - public bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor) - { - oldExecutor = default(IExecutor); - if (this.Executor != null && !this.Executor.Remove(this)) - { - return false; - } - // prevent invalid executor if a failure occurs - (oldExecutor, this.Executor) = (this.Executor, oldExecutor); - oldExecutor?.Wake(this); - executor?.Add(this); - this.Executor = executor; - executor?.Wake(this); - return true; - } - /// public IPublisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new() { @@ -147,7 +124,7 @@ internal bool RemovePublisher(IRawPublisher publisher) Subscription subscription = new Subscription(topic, this, callback, qos); bool success = this.CurrentSubscriptions.Add(subscription); Debug.Assert(success, "subscription already exists"); - this.Executor?.Wake(this); + this.Executor?.TryScheduleRescan(this); return subscription; } @@ -161,7 +138,12 @@ internal bool RemovePublisher(IRawPublisher publisher) /// If the subscription existed on this node and has been removed. internal bool RemoveSubscription(IRawSubscription subscription) { - return this.CurrentSubscriptions.Remove(subscription); + if (this.CurrentSubscriptions.Remove(subscription)) + { + this.Executor?.TryScheduleRescan(this); + return true; + } + return false; } /// @@ -171,7 +153,7 @@ internal bool RemoveSubscription(IRawSubscription subscription) Client client = new Client(topic, this, qos); bool success = this.CurrentClients.Add(client); Debug.Assert(success, "client already exists"); - this.Executor?.Wake(this); + this.Executor?.TryScheduleRescan(this); return client; } @@ -185,7 +167,12 @@ internal bool RemoveSubscription(IRawSubscription subscription) /// If the client existed on this node and has been removed. internal bool RemoveClient(IRawClient client) { - return this.CurrentClients.Remove(client); + if (this.CurrentClients.Remove(client)) + { + this.Executor?.TryScheduleRescan(this); + return true; + } + return false; } /// @@ -195,7 +182,7 @@ internal bool RemoveClient(IRawClient client) Service service = new Service(topic, this, callback, qos); bool success = this.CurrentServices.Add(service); Debug.Assert(success, "service already exists"); - this.Executor?.Wake(this); + this.Executor?.TryScheduleRescan(this); return service; } @@ -209,7 +196,12 @@ internal bool RemoveClient(IRawClient client) /// If the service existed on this node and has been removed. internal bool RemoveService(IRawService service) { - return this.CurrentServices.Remove(service); + if (this.CurrentServices.Remove(service)) + { + this.Executor?.TryScheduleRescan(this); + return true; + } + return false; } /// @@ -237,9 +229,10 @@ internal void DisposeFromContext() return; } - if (!this.TrySetExecutor(null)) + if (!(this.Executor is null)) { - throw new RuntimeError("removing the node from the current executor failed"); + bool success = this.Executor.Remove(this); + Debug.Assert(success, "node was not added to its old executor"); } foreach (IRawPublisher publisher in this.CurrentPublishers) diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index de3f4d72..0cceacb4 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -161,7 +161,8 @@ private void Dispose(bool disposing) { bool success = this.Node.RemoveService(this); Debug.Assert(success, "failed to remove service"); - this.Node.Executor?.Wake(this.Node); + // not required if we are being finalized since the executor cant be running + this.Node.Executor?.Wait(); } this.DisposeFromNode(); diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index f18554e1..2b12cb10 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -127,7 +127,8 @@ private void Dispose(bool disposing) { bool success = this.Node.RemoveSubscription(this); Debug.Assert(success, "failed to remove subscription"); - this.Node.Executor?.Wake(this.Node); + // not required if we are being finalized since the executor cant be running + this.Node.Executor?.Wait(); } this.DisposeFromNode(); diff --git a/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs index 6438ee6b..3091bc70 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs @@ -12,14 +12,18 @@ // 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 some nodes. + /// Executor controlling the processing of callbacks of a set of nodes. /// - /// Adding and removing Nodes should be done by calling . + /// + /// 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 { /// @@ -27,17 +31,30 @@ public interface IExecutor: IExtendedDisposable, ICollection /// /// /// This is used to tell the executor when entities are created or destroyed. - /// The executor has to guarantee that removed entities can be - /// disposed after this method returns. /// - void Wake(); + void ScheduleRescan(); /// - /// Notify the instance that a node changed or got added or removed. + /// Notify the instance that a node changed. /// - /// Node which changed and is handled by the executor. - /// If the node is handled by the executor. - /// - bool Wake(INode node); + /// 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 f9e57674..656623c3 100644 --- a/src/ros2cs/ros2cs_core/interfaces/INode.cs +++ b/src/ros2cs/ros2cs_core/interfaces/INode.cs @@ -37,28 +37,14 @@ public interface INode : IExtendedDisposable IContext Context { get; } /// - /// Executor handling callbacks of this node. + /// Executor handling callbacks of this node, may be null. /// /// - /// A node has to guarantee that it is associated with at most one executor at any given time + /// 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; } - - /// - /// Try to change the executor of this node. - /// - /// - /// A node has to guarantee that it is associated with at most one executor at any given time - /// to prevent undefined behaviour when multithreading is used. - /// - /// The new executor - /// If the change was successful - bool TrySetExecutor(IExecutor executor); - - /// - /// The old executor - bool TrySetExecutor(IExecutor executor, out IExecutor oldExecutor); + IExecutor Executor { get; set; } /// 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 From 667f68f41d3627b5846ab1ce1ef79611bf09377e Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Tue, 10 Jan 2023 13:48:30 +0100 Subject: [PATCH 17/46] update tests and fix C interop Returning false could fail since the size of a bool is different between C, C++ and C#. This is fixed by adding wrappers which assure that a byte is returned containing 1 for true and 0 for false. Furthermore this commit adds missing attributes to ref parameters in NativeRcl or turns them into out parameters. --- src/ros2cs/ros2cs_core/Client.cs | 6 +- src/ros2cs/ros2cs_core/Clock.cs | 2 +- src/ros2cs/ros2cs_core/Context.cs | 2 +- src/ros2cs/ros2cs_core/Node.cs | 2 +- src/ros2cs/ros2cs_core/Publisher.cs | 2 +- src/ros2cs/ros2cs_core/Service.cs | 2 +- src/ros2cs/ros2cs_core/Subscription.cs | 2 +- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 70 +--- .../ros2cs_core/native/NativeRclInterface.cs | 64 ++- .../ros2cs_core/native/rcl_native_interface.c | 68 +++ src/ros2cs/ros2cs_tests/CMakeLists.txt | 4 +- src/ros2cs/ros2cs_tests/src/ClientTest.cs | 86 ++-- src/ros2cs/ros2cs_tests/src/ClockTest.cs | 12 - src/ros2cs/ros2cs_tests/src/ContextTest.cs | 146 +++++++ src/ros2cs/ros2cs_tests/src/CreateNodeTest.cs | 42 -- .../ros2cs_tests/src/InitShutdownTest.cs | 95 ----- .../ros2cs_tests/src/LargeMessageTest.cs | 31 +- .../ros2cs_tests/src/NativeMetodsTest.cs | 386 +++++++++--------- src/ros2cs/ros2cs_tests/src/NodeTest.cs | 199 ++++++--- src/ros2cs/ros2cs_tests/src/PublisherTest.cs | 61 +++ src/ros2cs/ros2cs_tests/src/ServiceTest.cs | 51 ++- .../ros2cs_tests/src/SubscriptionTest.cs | 153 ++++--- 22 files changed, 895 insertions(+), 591 deletions(-) create mode 100644 src/ros2cs/ros2cs_tests/src/ContextTest.cs delete mode 100644 src/ros2cs/ros2cs_tests/src/CreateNodeTest.cs delete mode 100644 src/ros2cs/ros2cs_tests/src/InitShutdownTest.cs create mode 100644 src/ros2cs/ros2cs_tests/src/PublisherTest.cs diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index 5ae0703f..cc37f0ef 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -44,7 +44,7 @@ public bool IsDisposed { get { - bool ok = NativeRcl.rcl_client_is_valid(this.Handle); + bool ok = NativeRclInterface.rclcs_client_is_valid(this.Handle); GC.KeepAlive(this); return !ok; } @@ -125,7 +125,7 @@ public bool IsServiceAvailable() Utils.CheckReturnEnum(NativeRcl.rcl_service_server_is_available( this.Node.Handle, this.Handle, - ref available + out available )); GC.KeepAlive(this); return available; @@ -224,7 +224,7 @@ private long SendRequest(I msg) NativeRcl.rcl_send_request( this.Handle, msgInternals.Handle, - ref sequence_number + out sequence_number ) ); GC.KeepAlive(this); 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 index 997218a3..993e5938 100644 --- a/src/ros2cs/ros2cs_core/Context.cs +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -74,7 +74,7 @@ private void AssertOk() /// public bool Ok() { - return NativeRcl.rcl_context_is_valid(this.Handle); + return NativeRclInterface.rclcs_context_is_valid(this.Handle); } /// diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 14086be5..263c214c 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -33,7 +33,7 @@ internal sealed class Node : INode /// public bool IsDisposed { - get { return !NativeRcl.rcl_node_is_valid(this.Handle); } + get { return !NativeRclInterface.rclcs_node_is_valid(this.Handle); } } internal IntPtr Handle = IntPtr.Zero; diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index a1ebbd39..a63867b8 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -30,7 +30,7 @@ public bool IsDisposed { get { - bool ok = NativeRcl.rcl_publisher_is_valid(this.Handle); + bool ok = NativeRclInterface.rclcs_publisher_is_valid(this.Handle); GC.KeepAlive(this); return !ok; } diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 0cceacb4..642b95eb 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -33,7 +33,7 @@ public bool IsDisposed { get { - bool ok = NativeRcl.rcl_service_is_valid(this.Handle); + bool ok = NativeRclInterface.rclcs_service_is_valid(this.Handle); GC.KeepAlive(this); return !ok; } diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 2b12cb10..13a4698f 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -31,7 +31,7 @@ public bool IsDisposed { get { - bool ok = NativeRcl.rcl_subscription_is_valid(this.Handle); + bool ok = NativeRclInterface.rclcs_subscription_is_valid(this.Handle); GC.KeepAlive(this); return !ok; } diff --git a/src/ros2cs/ros2cs_core/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index 7aacd311..cec90d8f 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -36,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,16 +54,7 @@ internal static ShutdownType typeof(ShutdownType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool ContextIsValidType(IntPtr 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, IntPtr 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( @@ -98,15 +89,6 @@ internal static NodeFiniType "rcl_node_fini"), typeof(NodeFiniType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NodeValidType(IntPtr node); - internal static NodeValidType - rcl_node_is_valid = - (NodeValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_node_is_valid"), - typeof(NodeValidType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr NodeGetNameType(IntPtr node); internal static NodeGetNameType @@ -143,15 +125,6 @@ internal static ClientInitType "rcl_client_init"), typeof(ClientInitType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool ClientIsValidType(IntPtr client); - internal static ClientIsValidType - rcl_client_is_valid = - (ClientIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_client_is_valid"), - typeof(ClientIsValidType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate int ClientFiniType(IntPtr client, IntPtr node); internal static ClientFiniType @@ -162,7 +135,7 @@ internal static ClientFiniType typeof(ClientFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int SendRequestType(IntPtr 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( @@ -171,7 +144,7 @@ internal static SendRequestType typeof(SendRequestType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int TakeResponceType(IntPtr 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( @@ -180,7 +153,7 @@ internal static TakeResponceType typeof(TakeResponceType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int ServiceIsAvailableType(IntPtr node, IntPtr 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( @@ -206,15 +179,6 @@ internal static ServiceInitType "rcl_service_init"), typeof(ServiceInitType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool ServiceIsValidType(IntPtr service); - internal static ServiceIsValidType - rcl_service_is_valid = - (ServiceIsValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_service_is_valid"), - typeof(ServiceIsValidType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate int ServiceFiniType(IntPtr service, IntPtr node); internal static ServiceFiniType @@ -225,7 +189,7 @@ internal static ServiceFiniType typeof(ServiceFiniType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int TakeRequestType(IntPtr 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( @@ -234,7 +198,7 @@ internal static TakeRequestType typeof(TakeRequestType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int SendResponceType(IntPtr service, ref rcl_rmw_request_id_t request_header, 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( @@ -260,15 +224,6 @@ internal static PublisherInitType "rcl_publisher_init"), typeof(PublisherInitType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool PublisherValidType(IntPtr publisher); - internal static PublisherValidType - rcl_publisher_is_valid = - (PublisherValidType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeRCL, - "rcl_publisher_is_valid"), - typeof(PublisherValidType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate int PublisherFiniType(IntPtr publisher, IntPtr node); internal static PublisherFiniType @@ -305,15 +260,6 @@ internal static SubscriptionFiniType "rcl_subscription_fini"), typeof(SubscriptionFiniType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool SubscriptionIsValidType(IntPtr 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(IntPtr subscription, IntPtr message_handle, IntPtr message_info, IntPtr allocation); internal static TakeType @@ -418,7 +364,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 d642a540..42e1ab7b 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -46,6 +46,16 @@ internal static FreeContextType "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 @@ -57,7 +67,7 @@ internal static RCLCSInitType [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr GetZeroInitializedNodeType(); - internal static GetZeroInitializedContextType + internal static GetZeroInitializedNodeType rclcs_get_zero_initialized_node = (GetZeroInitializedNodeType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( nativeROS2CS, @@ -66,13 +76,23 @@ internal static GetZeroInitializedContextType [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate void FreeNodeType(IntPtr node); - internal static FreeContextType + 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 @@ -127,6 +147,16 @@ internal static FreeSubscriptionType "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 @@ -163,6 +193,16 @@ internal static FreePublisherType "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 @@ -199,6 +239,16 @@ internal static FreeClientType "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 @@ -235,6 +285,16 @@ internal static FreeServiceType "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 IntPtr ServiceCreateOptionsType(IntPtr qos); internal static ServiceCreateOptionsType diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index c4a5fa92..d10b183a 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -19,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -40,6 +42,17 @@ 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) { @@ -74,6 +87,17 @@ 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() { @@ -102,6 +126,17 @@ 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) { @@ -131,6 +166,17 @@ 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) { @@ -160,6 +206,17 @@ 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) { @@ -189,6 +246,17 @@ 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) { diff --git a/src/ros2cs/ros2cs_tests/CMakeLists.txt b/src/ros2cs/ros2cs_tests/CMakeLists.txt index ca42a0c9..0515dc33 100644 --- a/src/ros2cs/ros2cs_tests/CMakeLists.txt +++ b/src/ros2cs/ros2cs_tests/CMakeLists.txt @@ -29,8 +29,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 diff --git a/src/ros2cs/ros2cs_tests/src/ClientTest.cs b/src/ros2cs/ros2cs_tests/src/ClientTest.cs index 5e2a0362..664d6e7f 100644 --- a/src/ros2cs/ros2cs_tests/src/ClientTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ClientTest.cs @@ -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,40 @@ 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() + { + this.ClientTryProcessTest(this.Client.TryProcess); + } + + [Test] + public void ClientTryProcessAsync() + { + this.ClientTryProcessTest(() => { + Task task = this.Client.TryProcessAsync(); + task.Wait(); + return task.Result; + }); + } + + private void ClientTryProcessTest(Func implementation) + { + Assert.That(implementation(), Is.False); + + using var service = Node.CreateService( + SERVICE_NAME, + HandleRequest + ); + Task pendingTask = this.Client.CallAsync(this.CreateRequest(3, 4)); + while (!service.TryProcess()) + {} + while (!implementation()) + {} + + Assert.That(pendingTask.IsCompletedSuccessfully); } } } diff --git a/src/ros2cs/ros2cs_tests/src/ClockTest.cs b/src/ros2cs/ros2cs_tests/src/ClockTest.cs index db36813a..d0e2f8a3 100644 --- a/src/ros2cs/ros2cs_tests/src/ClockTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ClockTest.cs @@ -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..15e52f5c --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/ContextTest.cs @@ -0,0 +1,146 @@ +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/CreateNodeTest.cs b/src/ros2cs/ros2cs_tests/src/CreateNodeTest.cs deleted file mode 100644 index b112dc87..00000000 --- a/src/ros2cs/ros2cs_tests/src/CreateNodeTest.cs +++ /dev/null @@ -1,42 +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 NUnit.Framework; - -namespace ROS2.Test -{ - [TestFixture] - public class CreateNodeTest - { - [SetUp] - public void SetUp() - { - Ros2cs.Init(); - } - - [TearDown] - public void TearDown() - { - Ros2cs.Shutdown(); - } - - [Test] - public void CreateNode() - { - string nodeName = "create_node_test"; - Ros2cs.CreateNode(nodeName).Dispose(); - } - } -} 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..b31b311d 100644 --- a/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs +++ b/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs @@ -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/NativeMetodsTest.cs b/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs index 056d52cc..7a7afcb5 100644 --- a/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs +++ b/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs @@ -23,7 +23,7 @@ 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); + 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); - Assert.That(NativeRcl.rcl_context_is_valid(ref context), Is.True); - ret = (RCLReturnEnum)NativeRcl.rcl_shutdown(ref context); - Assert.That(ret, Is.EqualTo(RCLReturnEnum.RCL_RET_OK)); - - 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() + { + return NativeRclInterface.rclcs_node_create_default_options(); + } + + internal static void ShutdownOptions(IntPtr options) { - node = NativeRcl.rcl_get_zero_initialized_node(); + 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() + { + this.Context = RCLInitialize.InitRcl(); + this.NodeOptions = NodeInitialize.InitOptions(); + this.Node = NodeInitialize.InitNode(this.NodeOptions, this.Context); + } + + [TearDown] + public void TearDown() { - rcl_subscription_t subscription = NativeRcl.rcl_get_zero_initialized_subscription(); + 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] @@ -435,14 +456,14 @@ public void WaitSetAddSubscription() (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, - ref context, + this.Context, allocator )); TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(ref waitSet)); - 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(ref waitSet, this.Subscription, ref index)); Assert.That(index.ToUInt64(), Is.EqualTo(0)); long timeout_ns = 10*1000*1000; @@ -454,22 +475,18 @@ public void WaitSetAddSubscription() [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); + RCLInitialize.ShutdownRcl(this.Context); } [Test] @@ -494,7 +511,7 @@ public void WaitSetInit() (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, - ref context, + this.Context, allocator )); TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_fini(ref waitSet)); @@ -513,7 +530,7 @@ public void WaitSetClear() (UIntPtr)0, (UIntPtr)0, (UIntPtr)0, - ref context, + this.Context, allocator ); TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(ref waitSet)); @@ -524,65 +541,46 @@ public void WaitSetClear() [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 +595,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..986ce171 100644 --- a/src/ros2cs/ros2cs_tests/src/NodeTest.cs +++ b/src/ros2cs/ros2cs_tests/src/NodeTest.cs @@ -22,113 +22,196 @@ 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)); + } - using (publisher = node.CreatePublisher("test_topic")) - { - } + [Test] + public void DefaultExecutor() + { + Assert.That(Node.Executor, Is.Null); } [Test] - public void Publish() + public void IsDisposed() { - using (Publisher publisher = node.CreatePublisher("test_topic")) - { - publisher.Publish(new std_msgs.msg.Bool()); - } + Assert.That(Node.IsDisposed, Is.False); + + Node.Dispose(); + + Assert.That(Node.IsDisposed); } [Test] - public void PublishChangingSize() + public void DoubleDisposal() { - using (Publisher publisher = node.CreatePublisher("test_topic")) - { - string[] setStringSequence = new string[2] - { - "First", - "Second string to send, has to be a bit longer for the test" - }; + Node.Dispose(); + Node.Dispose(); - test_msgs.msg.UnboundedSequences msg3 = new test_msgs.msg.UnboundedSequences(); - msg3.String_values = setStringSequence; - publisher.Publish(msg3); + Assert.That(Node.IsDisposed); + } - 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"; + [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" + ); - // Publish reusing the message - publisher.Publish(msg3); + Node.Dispose(); - 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); + 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 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 DisposePublisher() + { + IPublisher publisher = Node.CreatePublisher("test_topic"); + + Assert.That(Node.Publishers, Contains.Item(publisher)); + + publisher.Dispose(); - msg3.String_values = new string[1] { "hello" }; - msg3.Int32_values = new int[1] { 1 }; - publisher.Publish(msg3); - } + Assert.That(Node.Publishers, Does.Not.Contain(publisher)); } [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 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 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 DisposeService() { - var service = node.CreateService( - "/test", - request => { throw new InvalidOperationException("service should not be called"); } + 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 CreateClient() + { + 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 RemoveClient() + public void DisposeClient() { - var client = node.CreateClient("/test"); + 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)); } } } diff --git a/src/ros2cs/ros2cs_tests/src/PublisherTest.cs b/src/ros2cs/ros2cs_tests/src/PublisherTest.cs new file mode 100644 index 00000000..ed50c197 --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/PublisherTest.cs @@ -0,0 +1,61 @@ +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..cc9ff442 100644 --- a/src/ros2cs/ros2cs_tests/src/ServiceTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ServiceTest.cs @@ -23,43 +23,68 @@ 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() + { + ServiceTryProcessTest(Service.TryProcess); + } + + [Test] + public void ServiceTryProcessAsync() + { + ServiceTryProcessTest(() => { + var task = Service.TryProcessAsync(); + task.Wait(); + return task.Result; + }); + } + + private void ServiceTryProcessTest(Func implementation) + { + Assert.That(implementation(), Is.False); } } } diff --git a/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs b/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs index 4cec7897..b7fa92cb 100644 --- a/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs +++ b/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs @@ -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,27 +136,43 @@ 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)); } + + [Test] + public void SubscriptionTryProcessAsync() + { + bool callbackTriggered = false; + using var subscription = Node.CreateSubscription( + TOPIC, + (msg) => { callbackTriggered = true; } + ); + Node.CreatePublisher(TOPIC).Publish(CreateMessage(0)); + + var task = subscription.TryProcessAsync(); + task.Wait(); + Assert.That(task.Result); + + Assert.That(callbackTriggered, Is.True); + } } } From 561ec070338cd9c183ec353253a5a37724bd96a9 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Tue, 10 Jan 2023 13:53:39 +0100 Subject: [PATCH 18/46] update CMakeLists.txt --- src/ros2cs/ros2cs_core/CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index f6a2f929..db5ddf5b 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -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,8 @@ set(CS_NATIVE set(CS_UTILS utils/Utils.cs + utils/LockedDictionary.cs + utils/MappedValueDictionary.cs ) set(CS_SOURCES @@ -103,9 +108,9 @@ set(CS_SOURCES Node.cs Publisher.cs QualityOfServiceProfile.cs - Ros2cs.cs Subscription.cs WaitSet.cs + Context.cs properties/AssemblyInfo.cs ) From 3a1682704065e863ea5d88b353bfe29efafa71b9 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 18 Jan 2023 15:48:36 +0100 Subject: [PATCH 19/46] add `IWaitable.Handle` This allows wrappers to be put in a wait set by delegating to the wrapped implementation. --- src/ros2cs/ros2cs_core/Client.cs | 2 +- src/ros2cs/ros2cs_core/Service.cs | 2 +- src/ros2cs/ros2cs_core/Subscription.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/IWaitable.cs | 6 ++++++ 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index cc37f0ef..b8fb9032 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -50,7 +50,7 @@ public bool IsDisposed } } - private IntPtr Handle = IntPtr.Zero; + public IntPtr Handle {get; private set; } = IntPtr.Zero; private IntPtr Options = IntPtr.Zero; diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 642b95eb..6f306e6a 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -39,7 +39,7 @@ public bool IsDisposed } } - private IntPtr Handle = IntPtr.Zero; + public IntPtr Handle { get; private set; } = IntPtr.Zero; private IntPtr Options = IntPtr.Zero; diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 13a4698f..02ed333f 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -37,7 +37,7 @@ public bool IsDisposed } } - private IntPtr Handle = IntPtr.Zero; + public IntPtr Handle { get; private set; } = IntPtr.Zero; private IntPtr Options = IntPtr.Zero; diff --git a/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs index dbc4f2c0..9cd8f4ec 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs @@ -1,3 +1,4 @@ +using System; using System.Threading.Tasks; namespace ROS2 @@ -7,6 +8,11 @@ namespace ROS2 /// public interface IWaitable { + /// + /// The handle used for adding to a wait set. + /// + IntPtr Handle { get; } + /// /// Try to process if this instance is ready. /// From 11c3696746c4f99830939e1c4bd7256cf6c2ab41 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 18 Jan 2023 15:51:13 +0100 Subject: [PATCH 20/46] refactor WaitSet - allow for checking which resources became ready - implement IExtendedDisposable and IReadOnlyCollection interfaces - abstract away wait set resizing and filling - hide struct behind IntPtr and C wrappers to handle layout changes and GC moves --- src/ros2cs/ros2cs_core/WaitSet.cs | 949 +++++++++++++++--- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 43 +- .../ros2cs_core/native/NativeRclInterface.cs | 78 ++ src/ros2cs/ros2cs_core/native/NativeTypes.cs | 17 - .../ros2cs_core/native/rcl_native_interface.c | 92 ++ src/ros2cs/ros2cs_tests/CMakeLists.txt | 1 + .../ros2cs_tests/src/NativeMetodsTest.cs | 45 +- src/ros2cs/ros2cs_tests/src/WaitSetTest.cs | 284 ++++++ 8 files changed, 1281 insertions(+), 228 deletions(-) create mode 100644 src/ros2cs/ros2cs_tests/src/WaitSetTest.cs diff --git a/src/ros2cs/ros2cs_core/WaitSet.cs b/src/ros2cs/ros2cs_core/WaitSet.cs index 18d21935..3905a3c8 100644 --- a/src/ros2cs/ros2cs_core/WaitSet.cs +++ b/src/ros2cs/ros2cs_core/WaitSet.cs @@ -13,181 +13,814 @@ // limitations under the License. using System; +using System.Collections; +using System.Collections.Generic; +using System.Linq; namespace ROS2 { - internal enum AddResult - { - SUCCESS, - FULL, - DISPOSED - } + /// + /// Collection used for waiting on resources to become ready. + /// + 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(); }} + /// + /// Context associated with this wait set. + /// + public IContext Context { get; private set; } - private rcl_wait_set_t Handle; + /// + public bool IsDisposed + { + get + { + bool ok = NativeRclInterface.rclcs_wait_set_is_valid(this.Handle); + GC.KeepAlive(this); + return !ok; + } + } - 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())); - } + /// + public int Count + { + get { return this.Subscriptions.Count + this.Clients.Count + this.Services.Count; } + } - ~WaitSet() - { - Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_fini(ref Handle)); - } + private IntPtr Handle = IntPtr.Zero; - internal void Clear() - { - Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_clear(ref Handle)); - } + /// + /// Modification version used to detect if the wait set was modified. + /// + private uint Version = 0; - 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)); - } + // are exposed as collections to prevent users from depending on the changing indexes + private readonly List CurrentSubscriptions = new List(); - 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; - } - } + private readonly List CurrentClients = new List(); - 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; - } - } + private readonly List CurrentServices = 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; - } - } + /// + /// Construct a new instance. + /// + /// Associated context + internal WaitSet(Context context) + { + this.Context = 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)), + UIntPtr.Zero, + 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); + } + context.OnShutdown += this.Dispose; + } - internal bool Wait() - { - return Wait(TimeSpan.FromTicks(-1)); + /// + /// Assert that the instance has not been disposed. + /// + /// If the instance was disposed + private void AssertOk() + { + if (this.IsDisposed) + { + throw new ObjectDisposedException("rcl wait set"); + } + } + /// + public IEnumerator GetEnumerator() + { + return this.Subscriptions.Concat(this.Clients).Concat(this.Services).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() + { + Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_resize( + this.Handle, + new UIntPtr(Convert.ToUInt32(this.CurrentSubscriptions.Count)), + UIntPtr.Zero, + UIntPtr.Zero, + new UIntPtr(Convert.ToUInt32(this.CurrentClients.Count)), + new UIntPtr(Convert.ToUInt32(this.CurrentServices.Count)), + UIntPtr.Zero + )); + } + + /// + /// 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 + ); + } + + /// 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) + { + this.AssertOk(); + // 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_WAIT_SET_EMPTY) + { + throw new WaitSetEmptyException("empty wait set can not be waited on"); + } + if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_TIMEOUT) + { + result = default(WaitResult); + return false; + } + Utils.CheckReturnEnum(ret); + + result = new WaitResult( + new ReadyDictionary( + this, + NativeRclInterface.rclcs_wait_set_get_subscription, + NativeRclInterface.rclcs_wait_set_set_subscription, + this.CurrentSubscriptions + ), + new ReadyDictionary( + this, + NativeRclInterface.rclcs_wait_set_get_client, + NativeRclInterface.rclcs_wait_set_set_client, + this.CurrentClients + ), + new ReadyDictionary( + this, + NativeRclInterface.rclcs_wait_set_get_service, + NativeRclInterface.rclcs_wait_set_set_service, + this.CurrentServices + ) + ); + 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; + } + + Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_fini(this.Handle)); + this.FreeHandles(); + + if (disposing) + { + this.Context.OnShutdown -= this.Dispose; + this.Subscriptions.Clear(); + this.Clients.Clear(); + this.Services.Clear(); + } + } + + private void FreeHandles() + { + NativeRclInterface.rclcs_free_wait_set(this.Handle); + this.Handle = IntPtr.Zero; + } + + ~WaitSet() + { + this.Dispose(false); + } + + /// + /// Dictionary representing the containts of a wait set. + /// + /// Type of resource being represented + private sealed class ReadyDictionary : IDictionary where T : IWaitable + { + /// + public T this[int key] + { + get + { + if (this.TryGetValue(key, out T value)) + { + return value; + } + throw new KeyNotFoundException($"key {key} was not found"); + } + set { this.Add(key, value); } + } + + /// + public int Count { get { return this.Keys.Count; } } + + /// + public bool IsReadOnly { get { return true; } } + + /// + public ICollection Keys { get; private set; } + + /// + public ICollection Values { get; private set; } + + private readonly WaitSet WaitSet; + + /// + /// Modification version of wait set when created. + /// + private readonly uint CreatedVersion; + + /// + /// Delegate used to read the wait set. + /// + private readonly NativeRclInterface.WaitSetGetType Getter; + + /// + /// Delegate used to change the wait set. + /// + private readonly NativeRclInterface.WaitSetSetType Setter; + + /// + /// Mapping between wait set index and wrapper. + /// + private readonly IList Wrappers; + + internal ReadyDictionary(WaitSet waitSet, NativeRclInterface.WaitSetGetType getter, NativeRclInterface.WaitSetSetType setter, IList wrappers) + { + this.WaitSet = waitSet; + this.CreatedVersion = waitSet.Version; + this.Getter = getter; + this.Setter = setter; + this.Wrappers = wrappers; + this.Keys = new KeysCollection(this); + this.Values = new ValuesCollection(this); + } + + /// + /// Check if the wait set is not disposed or was modified. + /// + /// The wait set has been disposed + /// The wait set has been modified + private void AssertOk() + { + if (this.WaitSet.IsDisposed) + { + throw new ObjectDisposedException("backing rcl wait set"); + } + if (this.CreatedVersion != this.WaitSet.Version) + { + throw new InvalidOperationException("wait set has been waited on"); + } + } + + /// + /// Try to convert a key to a native index. + /// + /// Key to convert + /// Native index + /// Whether the conversion was successful + private bool TryConvertKey(int key, out UIntPtr nativeIndex) + { + try + { + nativeIndex = new UIntPtr(Convert.ToUInt32(key)); + } + catch (OverflowException) + { + nativeIndex = UIntPtr.Zero; + return false; + } + return true; + } + + /// + /// Get the pointer stored at an index. + /// + /// Index of the pointer + /// Pointer stored at index + /// Whether the index existed + private bool TryGetPtr(UIntPtr index, out IntPtr ptr) + { + return this.Getter(this.WaitSet.Handle, index, out ptr); + } + + /// + /// Set the pointer stored at an index. + /// + /// Index of the pointer + /// Pointer to be stored at index + /// Whether the index existed + private bool TrySetPtr(UIntPtr index, IntPtr ptr) + { + return this.Setter(this.WaitSet.Handle, index, ptr); + } + + /// + public void Add(int key, T value) + { + throw new NotSupportedException("adding new elements can only be done on WaitSet"); + } + + /// + public void Add(KeyValuePair item) + { + this.Add(item.Key, item.Value); + } + + /// + public bool ContainsKey(int key) + { + this.AssertOk(); + return this.TryConvertKey(key, out UIntPtr index) && + this.TryGetPtr(index, out IntPtr ptr) && + ptr != IntPtr.Zero; + } + + /// + public bool Contains(KeyValuePair item) + { + return this.TryGetValue(item.Key, out T value) && + EqualityComparer.Default.Equals(item.Value, value); + } + + /// + public bool Remove(int key) + { + this.AssertOk(); + if (this.TryConvertKey(key, out UIntPtr index) && + this.TryGetPtr(index, out IntPtr ptr) && + ptr != IntPtr.Zero) + { + return this.TrySetPtr(index, IntPtr.Zero); + } + return false; + } + + /// + public bool Remove(KeyValuePair item) + { + return this.Contains(item) && this.Remove(item.Key); + } + + /// + public void Clear() + { + foreach (int key in this.Keys) + { + this.TrySetPtr(new UIntPtr(Convert.ToUInt32(key)), IntPtr.Zero); + } + } + + /// + public bool TryGetValue(int key, out T value) + { + this.AssertOk(); + if (this.TryConvertKey(key, out UIntPtr index) && this.TryGetPtr(index, out IntPtr ptr) && ptr != IntPtr.Zero) + { + value = this.Wrappers[key]; + return true; + } + value = default(T); + return false; + } + + /// + public void CopyTo(KeyValuePair[] array, int arrayIndex) + { + if (arrayIndex < 0) + { + throw new ArgumentOutOfRangeException("arrayIndex is less than 0"); + } + foreach (var item in this) + { + try + { + array[arrayIndex] = item; + } + catch (IndexOutOfRangeException e) + { + throw new ArgumentException("array is too small", e); + } + arrayIndex += 1; + } + } + + /// + public IEnumerator> GetEnumerator() + { + return this.Keys.Select(key => new KeyValuePair(key, this.Wrappers[key])).GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + + /// + /// Collection representing the indexes being ready. + /// + private sealed class KeysCollection : ICollection + { + /// + public int Count + { + get + { + // cant use Enumerable.Count() since it + // just returns .Count since we are a collection + IEnumerator e = this.GetEnumerator(); + int count = 0; + while (e.MoveNext()) + { + checked + { + count += 1; + } + } + return count; + } + } + + /// + public bool IsReadOnly + { + get { return this.ReadyDictionary.IsReadOnly; } + } + + private readonly ReadyDictionary ReadyDictionary; + + internal KeysCollection(ReadyDictionary readyDictionary) + { + this.ReadyDictionary = readyDictionary; + } + + /// + private void AssertOk() + { + this.ReadyDictionary.AssertOk(); + } + + /// + public void Add(int key) + { + throw new NotSupportedException("adding new elements can only be done on WaitSet"); + } + + /// + public void Clear() + { + this.ReadyDictionary.Clear(); + } + + /// + public bool Contains(int key) + { + return this.ReadyDictionary.ContainsKey(key); + } + + /// + public void CopyTo(int[] array, int arrayIndex) + { + if (arrayIndex < 0) + { + throw new ArgumentOutOfRangeException("arrayIndex is less than 0"); + } + foreach (int key in this) + { + try + { + array[arrayIndex] = key; + } + catch (IndexOutOfRangeException e) + { + throw new ArgumentException("array too small", e); + } + arrayIndex += 1; + } + } + + /// + public bool Remove(int key) + { + return this.ReadyDictionary.Remove(key); + } + + /// + public IEnumerator GetEnumerator() + { + this.AssertOk(); + for (int key = 0; this.ReadyDictionary.TryConvertKey(key, out UIntPtr index) && this.ReadyDictionary.TryGetPtr(index, out IntPtr ptr); key += 1) + { + if (ptr != IntPtr.Zero) + { + yield return key; + // can be invalidated before being resumed + this.AssertOk(); + } + } + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + } + + /// + /// Collection representing the wrappers which are ready. + /// + private sealed class ValuesCollection : ICollection + { + /// + public int Count + { + get { return this.ReadyDictionary.Count; } + } + + /// + public bool IsReadOnly + { + get { return this.ReadyDictionary.IsReadOnly; } + } + + private readonly ReadyDictionary ReadyDictionary; + + internal ValuesCollection(ReadyDictionary readDictionary) + { + this.ReadyDictionary = readDictionary; + } + + /// + public void Add(T value) + { + throw new NotSupportedException("adding new elements can only be done on WaitSet"); + } + + /// + public void Clear() + { + this.ReadyDictionary.Clear(); + } + + /// + public bool Contains(T value) + { + return this.Any(v => EqualityComparer.Default.Equals(v, value)); + } + + /// + public void CopyTo(T[] array, int arrayIndex) + { + if (arrayIndex < 0) + { + throw new ArgumentOutOfRangeException("arrayIndex is less than 0"); + } + foreach (T value in this) + { + try + { + array[arrayIndex] = value; + } + catch (IndexOutOfRangeException e) + { + throw new ArgumentException("array too small", e); + } + arrayIndex += 1; + } + } + + /// + public bool Remove(T value) + { + foreach (var item in this.ReadyDictionary) + { + if (EqualityComparer.Default.Equals(item.Value, value)) + { + return this.ReadyDictionary.Remove(item.Key); + } + } + return false; + } + + /// + public IEnumerator GetEnumerator() + { + return this.ReadyDictionary.Select(item => item.Value).GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + } + } } - internal bool Wait(TimeSpan timeout) + /// + /// Result of waiting on a wait set. + /// + /// + /// The dictionaries are invalidated when waiting on the wait set again. + /// + internal sealed class WaitResult : IEnumerable { - 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; - } + /// + /// Mapping from index to subscriptions which are ready. + /// + public IDictionary ReadySubscriptions { get; private set; } + + /// + /// Mapping from index to clients which are ready. + /// + public IDictionary ReadyClients { get; private set; } + + /// + /// Mapping from index to services which are ready. + /// + public IDictionary ReadyServices { get; private set; } + + internal WaitResult(IDictionary subscriptions, IDictionary clients, IDictionary services) + { + this.ReadySubscriptions = subscriptions; + this.ReadyClients = clients; + this.ReadyServices = services; + } + + /// + public IEnumerator GetEnumerator() + { + return this.ReadySubscriptions.Values + .Concat(this.ReadyClients.Values) + .Concat(this.ReadyServices.Values) + .GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + + /// + /// Deconstruct the result into the resources which are ready. + /// + public void Deconstruct(out IDictionary subscriptions, out IDictionary clients, out IDictionary services) + { + subscriptions = this.ReadySubscriptions; + clients = this.ReadyClients; + services = this.ReadyServices; + } } - } } diff --git a/src/ros2cs/ros2cs_core/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index cec90d8f..5b88124e 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -270,16 +270,7 @@ 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( - nativeRCL, - "rcl_get_zero_initialized_wait_set"), - typeof(GetZeroInitializedWaitSetType)); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetResizeType(ref rcl_wait_set_t wait_set, + internal delegate int WaitSetResizeType(IntPtr wait_set, UIntPtr number_of_subscriptions, UIntPtr number_of_guard_conditions, UIntPtr number_of_timers, @@ -293,7 +284,7 @@ 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, @@ -310,7 +301,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( @@ -319,7 +310,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( @@ -328,34 +319,30 @@ internal static WaitSetClearType typeof(WaitSetClearType)); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int WaitSetAddSubscriptionType(ref rcl_wait_set_t wait_set, IntPtr 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, IntPtr 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, IntPtr 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)); [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( diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index 42e1ab7b..9cd4d0b7 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -295,6 +295,84 @@ internal static ServiceIsValidType "rclcs_service_is_valid"), typeof(ServiceIsValidType)); + [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); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + [return: MarshalAs(UnmanagedType.U1)] + internal delegate bool WaitSetSetType(IntPtr waitSet, UIntPtr index, 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 WaitSetSetType + rclcs_wait_set_set_subscription = + (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_set_subscription"), + typeof(WaitSetSetType)); + + internal static WaitSetGetType + rclcs_wait_set_get_client = + (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_get_client"), + typeof(WaitSetGetType)); + + internal static WaitSetSetType + rclcs_wait_set_set_client = + (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_set_client"), + typeof(WaitSetSetType)); + + internal static WaitSetGetType + rclcs_wait_set_get_service = + (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_get_service"), + typeof(WaitSetGetType)); + + internal static WaitSetSetType + rclcs_wait_set_set_service = + (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_set_service"), + typeof(WaitSetSetType)); + [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 f16d6b28..8aa3348a 100644 --- a/src/ros2cs/ros2cs_core/native/NativeTypes.cs +++ b/src/ros2cs/ros2cs_core/native/NativeTypes.cs @@ -63,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 d10b183a..1b93cb2e 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -272,6 +273,97 @@ void rclcs_service_dispose_options(rcl_service_options_t * service_options_handl free(service_options_handle); } +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_set_subscription(rcl_wait_set_t * wait_set, size_t index, const rcl_subscription_t * subscription) +{ + if (index < wait_set->size_of_subscriptions) + { + wait_set->subscriptions[index] = subscription; + 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_set_client(rcl_wait_set_t * wait_set, size_t index, const rcl_client_t * client) +{ + if (index < wait_set->size_of_clients) + { + wait_set->clients[index] = client; + 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_set_service(rcl_wait_set_t * wait_set, size_t index, const rcl_service_t * service) +{ + if (index < wait_set->size_of_services) + { + wait_set->services[index] = service; + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT char * rclcs_get_error_string() { diff --git a/src/ros2cs/ros2cs_tests/CMakeLists.txt b/src/ros2cs/ros2cs_tests/CMakeLists.txt index 0515dc33..0d686021 100644 --- a/src/ros2cs/ros2cs_tests/CMakeLists.txt +++ b/src/ros2cs/ros2cs_tests/CMakeLists.txt @@ -39,6 +39,7 @@ if(BUILD_TESTING) src/ServiceTest.cs src/SubscriptionTest.cs src/TestUtils.cs + src/WaitSetTest.cs ) add_dotnet_test(ros2cs_tests diff --git a/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs b/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs index 7a7afcb5..92e344ee 100644 --- a/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs +++ b/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs @@ -13,10 +13,10 @@ // 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 { @@ -447,9 +447,9 @@ 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, @@ -459,16 +459,18 @@ public void WaitSetAddSubscription() this.Context, allocator )); - TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(ref waitSet)); + TestUtils.AssertRetOk(NativeRcl.rcl_wait_set_clear(handle)); 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, this.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); } } @@ -489,22 +491,13 @@ public void TearDown() RCLInitialize.ShutdownRcl(this.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(); - } - [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, @@ -514,16 +507,17 @@ public void WaitSetInit() 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, @@ -532,9 +526,10 @@ public void WaitSetClear() (UIntPtr)0, 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); } } diff --git a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs new file mode 100644 index 00000000..52654bf7 --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs @@ -0,0 +1,284 @@ +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 = new WaitSet(this.Context); + } + + [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 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 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(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 TestReadyDictionary() + { + this.Context.TryCreateNode("TestNode", out var node); + + using var publisher = node.CreatePublisher(SUBSCRIPTION_TOPIC); + using var subscription = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => { } + ); + using var subscriptionDummy = node.CreateSubscription( + SUBSCRIPTION_TOPIC, + msg => { throw new InvalidOperationException($"callback was triggered with {msg}"); } + ); + using var client = node.CreateClient(SERVICE_TOPIC); + using var clientDummy = node.CreateClient(SERVICE_TOPIC); + using var service = node.CreateService( + SERVICE_TOPIC, + request => new AddTwoInts_Response() + ); + using var serviceDummy = node.CreateService( + SERVICE_TOPIC, + request => { throw new InvalidOperationException($"received request ${request}"); } + ); + + 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); + this.TestReadyDictionary(result1.ReadySubscriptions, subscription, subscriptionDummy); + this.TestReadyDictionary(result1.ReadyServices, service, serviceDummy); + Assert.That(result1.ReadyClients.Count, Is.Zero); + + 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.Count; }); + Assert.Catch(() => { _ = result1.ReadyClients.Count; }); + Assert.Catch(() => { _ = result1.ReadyServices.Count; }); + Assert.That(result2.ReadySubscriptions.Count, Is.Zero); + Assert.That(result2.ReadyServices.Count, Is.Zero); + this.TestReadyDictionary(result2.ReadyClients, client, clientDummy); + + this.WaitSet.Dispose(); + Assert.Catch(() => { _ = result2.ReadySubscriptions.Count; }); + Assert.Catch(() => { _ = result2.ReadyClients.Count; }); + Assert.Catch(() => { _ = result2.ReadyServices.Count; }); + } + + private void TestReadyDictionary(IDictionary dictionary, T waitable, T dummy) where T : IWaitable + { + Assert.That(dictionary.Count, Is.EqualTo(1)); + Assert.That(dictionary.Count(), Is.EqualTo(1)); + + Assert.Throws(() => { dictionary[1] = waitable; }); + Assert.Throws(() => { dictionary.Add(1, waitable); }); + + var pair = dictionary.First(); + Assert.That(pair.Value, Is.EqualTo(waitable)); + + Assert.That(dictionary.Contains(new KeyValuePair(pair.Key, waitable)), Is.True); + Assert.That(dictionary.Contains(new KeyValuePair(pair.Key + 1, waitable)), Is.False); + Assert.That(dictionary.Contains(new KeyValuePair(pair.Key, dummy)), Is.False); + + Assert.That(dictionary.ContainsKey(pair.Key), Is.True); + Assert.That(dictionary.ContainsKey(pair.Key + 1), Is.False); + + Assert.That(dictionary.TryGetValue(pair.Key, out var result), Is.True); + Assert.That(result, Is.EqualTo(waitable)); + Assert.That(dictionary.TryGetValue(pair.Key + 1, out _), Is.False); + + this.TestReadyDictionaryKeys(dictionary.Keys, pair.Key); + this.TestReadyDictionaryValues(dictionary.Values, waitable, dummy); + + Assert.That(dictionary.Remove(pair.Key + 1), Is.False); + Assert.That(dictionary.Remove(pair), Is.True); + Assert.That(dictionary.Remove(pair), Is.False); + Assert.That(dictionary.Count, Is.EqualTo(0)); + } + + private void TestReadyDictionaryKeys(ICollection keys, int key) + { + Assert.That(keys.Count, Is.EqualTo(1)); + Assert.That(keys.Count(), Is.EqualTo(1)); + + Assert.Throws(() => { keys.Add(key + 1); }); + + Assert.That(keys.Contains(key), Is.True); + Assert.That(keys.Contains(key + 1), Is.False); + + Assert.That(keys.First(), Is.EqualTo(key)); + } + + private void TestReadyDictionaryValues(ICollection values, T value, T dummy) where T : IWaitable + { + Assert.That(values.Count, Is.EqualTo(1)); + Assert.That(values.Count(), Is.EqualTo(1)); + + Assert.Throws(() => { values.Add(value); }); + + Assert.That(values.Contains(value), Is.True); + Assert.That(values.Contains(dummy), Is.False); + + Assert.That(values.First(), Is.EqualTo(value)); + } + } +} \ No newline at end of file From 2a42d142d4911c42f7c267de363855a89a352ea5 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Tue, 7 Mar 2023 16:37:00 +0100 Subject: [PATCH 21/46] add GuardCondition It is currently only internal and used for the executor. --- src/ros2cs/ros2cs_core/CMakeLists.txt | 1 + src/ros2cs/ros2cs_core/GuardCondition.cs | 152 ++++++++++++++++++ src/ros2cs/ros2cs_core/WaitSet.cs | 56 ++++++- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 25 +++ .../ros2cs_core/native/NativeRclInterface.cs | 42 +++++ .../ros2cs_core/native/rcl_native_interface.c | 53 ++++++ src/ros2cs/ros2cs_tests/CMakeLists.txt | 1 + .../ros2cs_tests/src/GuardConditionTest.cs | 77 +++++++++ src/ros2cs/ros2cs_tests/src/WaitSetTest.cs | 20 +++ 9 files changed, 421 insertions(+), 6 deletions(-) create mode 100644 src/ros2cs/ros2cs_core/GuardCondition.cs create mode 100644 src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index db5ddf5b..aa154453 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -111,6 +111,7 @@ set(CS_SOURCES Subscription.cs WaitSet.cs Context.cs + GuardCondition.cs properties/AssemblyInfo.cs ) diff --git a/src/ros2cs/ros2cs_core/GuardCondition.cs b/src/ros2cs/ros2cs_core/GuardCondition.cs new file mode 100644 index 00000000..7956e11d --- /dev/null +++ b/src/ros2cs/ros2cs_core/GuardCondition.cs @@ -0,0 +1,152 @@ +// 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; + +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; + context.OnShutdown += this.Dispose; + } + + /// + /// 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; + } + + Utils.CheckReturnEnum(NativeRcl.rcl_guard_condition_fini(this.Handle)); + this.FreeHandles(); + + if (disposing) + { + this.Context.OnShutdown -= this.Dispose; + } + } + + /// + /// 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/WaitSet.cs b/src/ros2cs/ros2cs_core/WaitSet.cs index 3905a3c8..f3db3aaf 100644 --- a/src/ros2cs/ros2cs_core/WaitSet.cs +++ b/src/ros2cs/ros2cs_core/WaitSet.cs @@ -39,6 +39,11 @@ internal sealed class WaitSet : IReadOnlyCollection, IExtendedDisposa /// public ICollection Services { get { return this.CurrentServices; } } + /// + /// The instances currently in the wait set. + /// + public ICollection GuardConditions { get { return this.CurrentGuardConditions; } } + /// /// Context associated with this wait set. /// @@ -58,7 +63,13 @@ public bool IsDisposed /// public int Count { - get { return this.Subscriptions.Count + this.Clients.Count + this.Services.Count; } + get + { + return this.Subscriptions.Count + + this.Clients.Count + + this.Services.Count + + this.GuardConditions.Count; + } } private IntPtr Handle = IntPtr.Zero; @@ -75,6 +86,8 @@ public int Count private readonly List CurrentServices = new List(); + private readonly List CurrentGuardConditions = new List(); + /// /// Construct a new instance. /// @@ -86,7 +99,7 @@ internal WaitSet(Context context) int ret = NativeRcl.rcl_wait_set_init( this.Handle, new UIntPtr(Convert.ToUInt32(this.CurrentSubscriptions.Capacity)), - UIntPtr.Zero, + new UIntPtr(Convert.ToUInt32(this.CurrentGuardConditions.Capacity)), UIntPtr.Zero, new UIntPtr(Convert.ToUInt32(this.CurrentClients.Capacity)), new UIntPtr(Convert.ToUInt32(this.CurrentServices.Capacity)), @@ -116,7 +129,11 @@ private void AssertOk() /// public IEnumerator GetEnumerator() { - return this.Subscriptions.Concat(this.Clients).Concat(this.Services).GetEnumerator(); + return this.Subscriptions + .Concat(this.Clients) + .Concat(this.Services) + .Concat(this.GuardConditions) + .GetEnumerator(); } /// @@ -156,7 +173,7 @@ private void PrepareWaitSet() Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_resize( this.Handle, new UIntPtr(Convert.ToUInt32(this.CurrentSubscriptions.Count)), - UIntPtr.Zero, + new UIntPtr(Convert.ToUInt32(this.CurrentGuardConditions.Count)), UIntPtr.Zero, new UIntPtr(Convert.ToUInt32(this.CurrentClients.Count)), new UIntPtr(Convert.ToUInt32(this.CurrentServices.Count)), @@ -251,6 +268,11 @@ private void FillWaitSet() 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 @@ -296,6 +318,12 @@ public bool TryWait(TimeSpan timeout, out WaitResult result) NativeRclInterface.rclcs_wait_set_get_service, NativeRclInterface.rclcs_wait_set_set_service, this.CurrentServices + ), + new ReadyDictionary( + this, + NativeRclInterface.rclcs_wait_set_get_guard_condition, + NativeRclInterface.rclcs_wait_set_set_guard_condition, + this.CurrentGuardConditions ) ); return true; @@ -791,11 +819,21 @@ internal sealed class WaitResult : IEnumerable /// public IDictionary ReadyServices { get; private set; } - internal WaitResult(IDictionary subscriptions, IDictionary clients, IDictionary services) + /// + /// Mapping from index to guard conditions which are ready. + /// + public IDictionary ReadyGuardConditions { get; private set; } + + internal WaitResult( + IDictionary subscriptions, + IDictionary clients, + IDictionary services, + IDictionary guard_conditions) { this.ReadySubscriptions = subscriptions; this.ReadyClients = clients; this.ReadyServices = services; + this.ReadyGuardConditions = guard_conditions; } /// @@ -804,6 +842,7 @@ public IEnumerator GetEnumerator() return this.ReadySubscriptions.Values .Concat(this.ReadyClients.Values) .Concat(this.ReadyServices.Values) + .Concat(this.ReadyGuardConditions.Values) .GetEnumerator(); } @@ -816,11 +855,16 @@ IEnumerator IEnumerable.GetEnumerator() /// /// Deconstruct the result into the resources which are ready. /// - public void Deconstruct(out IDictionary subscriptions, out IDictionary clients, out IDictionary services) + public void Deconstruct( + out IDictionary subscriptions, + out IDictionary clients, + out IDictionary services, + out IDictionary guard_conditions) { subscriptions = this.ReadySubscriptions; clients = this.ReadyClients; services = this.ReadyServices; + guard_conditions = this.ReadyGuardConditions; } } } diff --git a/src/ros2cs/ros2cs_core/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index 5b88124e..78bf7854 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRcl.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRcl.cs @@ -269,6 +269,24 @@ internal static TakeType "rcl_take"), typeof(TakeType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int GuardConditionTriggerType(IntPtr guard_condition); + internal static GuardConditionTriggerType + rcl_trigger_guard_condition = + (GuardConditionTriggerType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeRCL, + "rcl_trigger_guard_condition"), + typeof(GuardConditionTriggerType)); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + 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, @@ -341,6 +359,13 @@ internal static WaitSetAddType "rcl_wait_set_add_service"), 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(IntPtr wait_set, long timeout); internal static WaitType diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index 9cd4d0b7..eabc7df7 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -295,6 +295,34 @@ internal static ServiceIsValidType "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 @@ -373,6 +401,20 @@ internal static WaitSetSetType "rclcs_wait_set_set_service"), typeof(WaitSetSetType)); + internal static WaitSetGetType + rclcs_wait_set_get_guard_condition = + (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_get_guard_condition"), + typeof(WaitSetGetType)); + + internal static WaitSetSetType + rclcs_wait_set_set_guard_condition = + (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( + nativeROS2CS, + "rclcs_wait_set_set_guard_condition"), + typeof(WaitSetSetType)); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr ServiceCreateOptionsType(IntPtr qos); internal static ServiceCreateOptionsType diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index 1b93cb2e..b1baa98e 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -273,6 +274,36 @@ 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() { @@ -364,6 +395,28 @@ uint8_t rclcs_wait_set_set_service(rcl_wait_set_t * wait_set, size_t index, cons 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 +uint8_t rclcs_wait_set_set_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) + { + wait_set->guard_conditions[index] = guard_condition; + return 1; + } + return 0; +} + ROSIDL_GENERATOR_C_EXPORT char * rclcs_get_error_string() { diff --git a/src/ros2cs/ros2cs_tests/CMakeLists.txt b/src/ros2cs/ros2cs_tests/CMakeLists.txt index 0d686021..a8c12d35 100644 --- a/src/ros2cs/ros2cs_tests/CMakeLists.txt +++ b/src/ros2cs/ros2cs_tests/CMakeLists.txt @@ -40,6 +40,7 @@ if(BUILD_TESTING) src/SubscriptionTest.cs src/TestUtils.cs src/WaitSetTest.cs + src/GuardConditionTest.cs ) add_dotnet_test(ros2cs_tests diff --git a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs new file mode 100644 index 00000000..1b3b48e7 --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs @@ -0,0 +1,77 @@ +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 = new GuardCondition( + this.Context, + () => { 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 DisposedContextHandling() + { + this.Context.Dispose(); + + Assert.Throws(() => new GuardCondition(this.Context, () => {})); + } + + [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.Values.Contains(this.GuardCondition), Is.True); + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs index 52654bf7..e600cdd9 100644 --- a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs +++ b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs @@ -114,6 +114,26 @@ public void TestServiceCollection() 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() { From bc73ca76a3288ac13c99dbfb7e528e3e26990a64 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 15 Mar 2023 14:09:07 +0100 Subject: [PATCH 22/46] add ManualExecutor - implementation of IExecutor which has to be spun manually - roughly the same as the old spin logic in Ros2cs - does not rescan every spin create mode 100644 src/ros2cs/ros2cs_core/executors/ManualExecutor.cs create mode 100644 src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs --- src/ros2cs/ros2cs_core/CMakeLists.txt | 1 + .../ros2cs_core/executors/ManualExecutor.cs | 312 ++++++++++++++++++ src/ros2cs/ros2cs_tests/CMakeLists.txt | 1 + .../ros2cs_tests/src/ManualExecutorTest.cs | 302 +++++++++++++++++ 4 files changed, 616 insertions(+) create mode 100644 src/ros2cs/ros2cs_core/executors/ManualExecutor.cs create mode 100644 src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index aa154453..36d0a0b4 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -112,6 +112,7 @@ set(CS_SOURCES WaitSet.cs Context.cs GuardCondition.cs + executors/ManualExecutor.cs properties/AssemblyInfo.cs ) diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs new file mode 100644 index 00000000..7e3d8320 --- /dev/null +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -0,0 +1,312 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Threading; + +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; } + } + + public bool IsSpinning + { + get { return !this.IsIdle.IsSet; } + } + + public bool RescanScheduled + { + get { return this._RescanScheduled; } + } + + /// + public bool IsDisposed + { + get { return this.WaitSet.IsDisposed || this.InterruptCondition.IsDisposed; } + } + + /// + public int Count + { + get { return this.Nodes.Count; } + } + + /// + public bool IsReadOnly + { + get { return false; } + } + + private readonly WaitSet WaitSet; + + private readonly GuardCondition InterruptCondition; + + private readonly HashSet Nodes = new HashSet(); + + private readonly ManualResetEventSlim IsIdle = new ManualResetEventSlim(true); + + private volatile bool _RescanScheduled = false; + + public ManualExecutor(Context context) : this( + new WaitSet(context), + new GuardCondition(context, () => { }) + ) + { } + + internal ManualExecutor(WaitSet waitSet, GuardCondition interruptCondition) + { + this.WaitSet = waitSet; + this.InterruptCondition = interruptCondition; + this.WaitSet.GuardConditions.Add(this.InterruptCondition); + } + + /// + public void Add(INode node) + { + if (!(node.Executor is null)) + { + throw new InvalidOperationException("node already has an executor"); + } + this.Nodes.Add(node); + node.Executor = this; + this.ScheduleRescan(); + } + + /// + public bool Remove(INode node) + { + if (this.Nodes.Remove(node)) + { + Debug.Assert( + Object.ReferenceEquals(node.Executor, this), + "node has different executor" + ); + node.Executor = null; + this.ScheduleRescan(); + this.Wait(); + return true; + } + return false; + } + + /// + public void Clear() + { + if (this.Nodes.Count == 0) + { + return; + } + foreach (INode node in this.Nodes.ToArray()) + { + this.Nodes.Remove(node); + node.Executor = null; + } + this.ScheduleRescan(); + this.Wait(); + } + + /// + public bool Contains(INode node) + { + return this.Nodes.Contains(node); + } + + /// + 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"); + } + foreach (var item in this) + { + try + { + array[index] = item; + } + catch (IndexOutOfRangeException e) + { + throw new ArgumentException("array is too small", e); + } + index += 1; + } + } + + /// + public IEnumerator GetEnumerator() + { + return this.Nodes.GetEnumerator(); + } + + /// + IEnumerator IEnumerable.GetEnumerator() + { + return this.GetEnumerator(); + } + + /// + public void ScheduleRescan() + { + this._RescanScheduled = true; + } + + /// + public bool TryScheduleRescan(INode node) + { + this.ScheduleRescan(); + return true; + } + + /// + public void Wait() + { + bool success = this.TryWait(TimeSpan.FromMilliseconds(-1)); + Debug.Assert(success, "infinite wait timed out"); + } + + /// + public bool TryWait(TimeSpan timeout) + { + if (this._RescanScheduled) + { + try + { + this.Interrupt(); + } + catch (ObjectDisposedException) + { + // if the context is shut down then the + // guard condition might be disposed but + // nodes still have to be removed + } + return this.IsIdle.Wait(timeout); + } + return true; + } + + /// + /// Interrupt if waiting for + /// work to become available. + /// + /// + /// If is not currently waiting then + /// this method does not have any effect. + /// Furthermore no work is skipped. + /// + public void Interrupt() + { + this.InterruptCondition.Trigger(); + } + + /// + /// Try to process work if no rescan is scheduled. + /// + /// Maximum time to wait for work to become available. + /// Whether work could be processed since no rescan was scheduled. + public bool TrySpin(TimeSpan timeout) + { + this.IsIdle.Reset(); + try + { + if (this._RescanScheduled) + { + return false; + } + if (this.WaitSet.TryWait(timeout, out var result)) + { + foreach (IWaitable waitable in result) + { + waitable.TryProcess(); + } + } + } + finally + { + this.IsIdle.Set(); + } + return true; + } + + /// + /// Rescan the nodes of this executor for + /// new objects to wait for. + /// + /// This clears any scheduled rescans. + public void Rescan() + { + this.WaitSet.Subscriptions.Clear(); + this.WaitSet.Services.Clear(); + this.WaitSet.Clients.Clear(); + // clear it first to prevent clearing rescans + // scheduled just after we finished rescaning + this._RescanScheduled = false; + foreach (INode node in this.Nodes) + { + 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); + } + } + } + + /// + /// Utility which handles automatic rescaning. + /// + /// Maximum time to wait for work to become available. + /// + /// trying to spin in each iteration + /// and yielding if a rescan had to be performed. + /// + public IEnumerator Spin(TimeSpan timeout) + { + while (true) + { + if (this.TrySpin(timeout)) + { + yield return false; + } + else + { + this.Rescan(); + yield return true; + } + } + } + + /// + public void Dispose() + { + this.Clear(); + this.WaitSet.Dispose(); + this.InterruptCondition.Dispose(); + this.IsIdle.Dispose(); + } + } +} \ No newline at end of file diff --git a/src/ros2cs/ros2cs_tests/CMakeLists.txt b/src/ros2cs/ros2cs_tests/CMakeLists.txt index a8c12d35..cd03b3a2 100644 --- a/src/ros2cs/ros2cs_tests/CMakeLists.txt +++ b/src/ros2cs/ros2cs_tests/CMakeLists.txt @@ -41,6 +41,7 @@ if(BUILD_TESTING) src/TestUtils.cs src/WaitSetTest.cs src/GuardConditionTest.cs + src/ManualExecutorTest.cs ) add_dotnet_test(ros2cs_tests diff --git a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs new file mode 100644 index 00000000..855689f9 --- /dev/null +++ b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs @@ -0,0 +1,302 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Threading; +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; + + [SetUp] + public void SetUp() + { + this.Context = new Context(); + this.Executor = new ManualExecutor(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.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); + Assert.That(this.Executor.RescanScheduled, Is.True); + } + + [Test] + public void TryWaitUnscheduled() + { + Assert.That(this.Executor.TryWait(TimeSpan.Zero), Is.True); + 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) + { } + // possible race condition since the wait may not have started + Thread.Sleep(TimeSpan.FromSeconds(0.1)); + 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 InterruptWaiting() + { + Thread spinThread = new Thread(() => { this.Executor.TrySpin(TimeSpan.FromMinutes(1)); }); + spinThread.Start(); + // wait for spin to start + while (!this.Executor.IsSpinning) + { } + // possible race condition since the wait may not have started + Thread.Sleep(TimeSpan.FromSeconds(0.1)); + + 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); + } + + 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 From c635d56572f570af07391147d90b294e975b9a0d Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 15 Mar 2023 16:10:10 +0100 Subject: [PATCH 23/46] doc and tests updates for guard condition behavior Guard conditions seem to stay triggered until waited on. --- .../ros2cs_core/executors/ManualExecutor.cs | 9 ++--- .../ros2cs_tests/src/GuardConditionTest.cs | 12 +++++++ .../ros2cs_tests/src/ManualExecutorTest.cs | 35 ++++++++++++++++--- 3 files changed, 46 insertions(+), 10 deletions(-) diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index 7e3d8320..62fd3e49 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -186,7 +186,7 @@ public void Wait() /// public bool TryWait(TimeSpan timeout) { - if (this._RescanScheduled) + if (this._RescanScheduled && this.IsSpinning) { try { @@ -204,13 +204,10 @@ public bool TryWait(TimeSpan timeout) } /// - /// Interrupt if waiting for - /// work to become available. + /// Interrupt the next or current . /// /// - /// If is not currently waiting then - /// this method does not have any effect. - /// Furthermore no work is skipped. + /// This method only causes the wait to be skipped, work which is ready will be executed. /// public void Interrupt() { diff --git a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs index 1b3b48e7..daefedae 100644 --- a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs +++ b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs @@ -73,5 +73,17 @@ public void TriggerGuardCondition() Assert.That(waitSet.TryWait(TimeSpan.FromSeconds(0.5), out var result), Is.True); Assert.That(result.ReadyGuardConditions.Values.Contains(this.GuardCondition), Is.True); } + + [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/ManualExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs index 855689f9..c20f1263 100644 --- a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs @@ -122,14 +122,43 @@ public void TryWaitInterrupt() // wait for spin to start while (!this.Executor.IsSpinning) { } - // possible race condition since the wait may not have started - Thread.Sleep(TimeSpan.FromSeconds(0.1)); 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() { @@ -138,8 +167,6 @@ public void InterruptWaiting() // wait for spin to start while (!this.Executor.IsSpinning) { } - // possible race condition since the wait may not have started - Thread.Sleep(TimeSpan.FromSeconds(0.1)); this.Executor.Interrupt(); From b1594c15307be31b9f063f93379337c6c58631f5 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 16 Mar 2023 15:52:43 +0100 Subject: [PATCH 24/46] remove redundant `IsDisposed` checks The checks are already done by rcl. --- src/ros2cs/ros2cs_core/Client.cs | 29 ++++--------- src/ros2cs/ros2cs_core/Context.cs | 12 ------ src/ros2cs/ros2cs_core/Node.cs | 29 +++++-------- src/ros2cs/ros2cs_core/Publisher.cs | 12 ------ src/ros2cs/ros2cs_core/Service.cs | 21 +++++----- src/ros2cs/ros2cs_core/Subscription.cs | 22 +++++----- src/ros2cs/ros2cs_core/WaitSet.cs | 25 ++++------- src/ros2cs/ros2cs_core/utils/Utils.cs | 8 ++++ src/ros2cs/ros2cs_tests/src/NodeTest.cs | 56 ++++++++++++++++++++++++- 9 files changed, 111 insertions(+), 103 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index b8fb9032..15b03201 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -106,21 +106,9 @@ public Client(string topic, Node node, QualityOfServiceProfile qos = null) } } - /// - /// Assert that the client has not been disposed. - /// - private void AssertOk() - { - if (this.IsDisposed) - { - throw new ObjectDisposedException($"client for topic '{this.Topic}'"); - } - } - /// public bool IsServiceAvailable() { - this.AssertOk(); bool available = false; Utils.CheckReturnEnum(NativeRcl.rcl_service_server_is_available( this.Node.Handle, @@ -134,11 +122,6 @@ out available /// public bool TryProcess() { - if (this.IsDisposed) - { - return false; - } - rcl_rmw_request_id_t header = default(rcl_rmw_request_id_t); O message = new O(); (TaskCompletionSource, Task) source; @@ -154,11 +137,16 @@ public bool TryProcess() ); GC.KeepAlive(this); - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_CLIENT_TAKE_FAILED) + switch ((RCLReturnEnum)ret) { - return false; + case RCLReturnEnum.RCL_RET_CLIENT_TAKE_FAILED: + case RCLReturnEnum.RCL_RET_CLIENT_INVALID: + return false; + default: + Utils.CheckReturnEnum(ret); + break; } - Utils.CheckReturnEnum(ret); + if (this.Requests.TryGetValue(header.sequence_number, out source)) { exists = true; @@ -200,7 +188,6 @@ public Task CallAsync(I msg) /// public Task CallAsync(I msg, TaskCreationOptions options) { - this.AssertOk(); var source = new TaskCompletionSource(options); lock (this.Requests) { diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs index 993e5938..94e18b9f 100644 --- a/src/ros2cs/ros2cs_core/Context.cs +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -60,17 +60,6 @@ public Context() } } - /// - /// Assert that the context has not been disposed. - /// - private void AssertOk() - { - if (this.IsDisposed) - { - throw new ObjectDisposedException("current ROS context"); - } - } - /// public bool Ok() { @@ -80,7 +69,6 @@ public bool Ok() /// public bool TryCreateNode(string name, out INode node) { - this.AssertOk(); if (this.ROSNodes.ContainsKey(name)) { node = default(INode); diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 263c214c..28d7c8be 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -76,28 +76,24 @@ internal Node(string name, Context context) this.Options ); - if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) + switch ((RCLReturnEnum)ret) { - this.FreeHandles(); - Utils.CheckReturnEnum(ret); - } - } - - /// - /// Assert that the instance has not been disposed. - /// - private void AssertOk() - { - if (this.IsDisposed) - { - throw new ObjectDisposedException($"ROS2 node '{this.Name}'"); + 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; } } /// public IPublisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new() { - this.AssertOk(); Publisher publisher = new Publisher(topic, this, qos); bool success = this.CurrentPublishers.Add(publisher); Debug.Assert(success, "publisher already exists"); @@ -120,7 +116,6 @@ internal bool RemovePublisher(IRawPublisher publisher) /// public ISubscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new() { - this.AssertOk(); Subscription subscription = new Subscription(topic, this, callback, qos); bool success = this.CurrentSubscriptions.Add(subscription); Debug.Assert(success, "subscription already exists"); @@ -149,7 +144,6 @@ internal bool RemoveSubscription(IRawSubscription subscription) /// public IClient CreateClient(string topic, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() { - this.AssertOk(); Client client = new Client(topic, this, qos); bool success = this.CurrentClients.Add(client); Debug.Assert(success, "client already exists"); @@ -178,7 +172,6 @@ internal bool RemoveClient(IRawClient client) /// public IService CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new() { - this.AssertOk(); Service service = new Service(topic, this, callback, qos); bool success = this.CurrentServices.Add(service); Debug.Assert(success, "service already exists"); diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index a63867b8..b98e822f 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -68,21 +68,9 @@ internal Publisher(string topic, Node node, QualityOfServiceProfile qos = null) } } - /// - /// Assert that the publisher has not been disposed. - /// - private void AssertOk() - { - if (this.IsDisposed) - { - throw new ObjectDisposedException($"publisher for topic '{this.Topic}'"); - } - } - /// public void Publish(T msg) { - this.AssertOk(); MessageInternals msgInternals = msg as MessageInternals; msgInternals.WriteNativeMessage(); Utils.CheckReturnEnum(NativeRcl.rcl_publish(this.Handle, msgInternals.Handle, IntPtr.Zero)); diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 6f306e6a..66b54915 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -81,11 +81,6 @@ internal Service(string topic, Node node, Func callback, QualityOfServiceP /// public bool TryProcess() { - if (this.IsDisposed) - { - return false; - } - rcl_rmw_request_id_t header = default(rcl_rmw_request_id_t); I message = new I(); int ret = NativeRcl.rcl_take_request( @@ -95,13 +90,19 @@ public bool TryProcess() ); GC.KeepAlive(this); - if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_SERIVCE_TAKE_FAILD) + switch ((RCLReturnEnum)ret) { - Utils.CheckReturnEnum(ret); - this.ProcessRequest(header, message); - return true; + case RCLReturnEnum.RCL_RET_SERIVCE_TAKE_FAILD: + case RCLReturnEnum.RCL_RET_SERVICE_INVALID: + return false; + default: + Utils.CheckReturnEnum(ret); + break; } - return false; + + Utils.CheckReturnEnum(ret); + this.ProcessRequest(header, message); + return true; } /// diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 02ed333f..c09728dc 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -75,11 +75,6 @@ internal Subscription(string topic, Node node, Action callback, QualityOfServ /// public bool TryProcess() { - if (this.IsDisposed) - { - return false; - } - T message = new T(); int ret = NativeRcl.rcl_take( this.Handle, @@ -89,14 +84,19 @@ public bool TryProcess() ); GC.KeepAlive(this); - if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_SUBSCRIPTION_TAKE_FAILED) + switch ((RCLReturnEnum)ret) { - Utils.CheckReturnEnum(ret); - (message as MessageInternals).ReadNativeMessage(); - this.Callback(message); - return true; + case RCLReturnEnum.RCL_RET_SUBSCRIPTION_TAKE_FAILED: + case RCLReturnEnum.RCL_RET_SUBSCRIPTION_INVALID: + return false; + default: + Utils.CheckReturnEnum(ret); + break; } - return false; + + (message as MessageInternals).ReadNativeMessage(); + this.Callback(message); + return true; } /// diff --git a/src/ros2cs/ros2cs_core/WaitSet.cs b/src/ros2cs/ros2cs_core/WaitSet.cs index f3db3aaf..f36eb581 100644 --- a/src/ros2cs/ros2cs_core/WaitSet.cs +++ b/src/ros2cs/ros2cs_core/WaitSet.cs @@ -115,17 +115,6 @@ internal WaitSet(Context context) context.OnShutdown += this.Dispose; } - /// - /// Assert that the instance has not been disposed. - /// - /// If the instance was disposed - private void AssertOk() - { - if (this.IsDisposed) - { - throw new ObjectDisposedException("rcl wait set"); - } - } /// public IEnumerator GetEnumerator() { @@ -170,7 +159,7 @@ public WaitResult Wait() /// private void PrepareWaitSet() { - Utils.CheckReturnEnum(NativeRcl.rcl_wait_set_resize( + int ret = NativeRcl.rcl_wait_set_resize( this.Handle, new UIntPtr(Convert.ToUInt32(this.CurrentSubscriptions.Count)), new UIntPtr(Convert.ToUInt32(this.CurrentGuardConditions.Count)), @@ -178,7 +167,12 @@ private void PrepareWaitSet() 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); } /// @@ -281,7 +275,6 @@ private void FillWaitSet() /// public bool TryWait(TimeSpan timeout, out WaitResult result) { - this.AssertOk(); // invalidate last wait result this.Version += 1; @@ -289,10 +282,6 @@ public bool TryWait(TimeSpan timeout, out WaitResult result) long nanoSeconds = timeout.Ticks * (1_000_000_000 / TimeSpan.TicksPerSecond); int ret = NativeRcl.rcl_wait(this.Handle, nanoSeconds); - if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_WAIT_SET_EMPTY) - { - throw new WaitSetEmptyException("empty wait set can not be waited on"); - } if ((RCLReturnEnum)ret == RCLReturnEnum.RCL_RET_TIMEOUT) { result = default(WaitResult); diff --git a/src/ros2cs/ros2cs_core/utils/Utils.cs b/src/ros2cs/ros2cs_core/utils/Utils.cs index 7d747e41..2c38b5d7 100644 --- a/src/ros2cs/ros2cs_core/utils/Utils.cs +++ b/src/ros2cs/ros2cs_core/utils/Utils.cs @@ -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_tests/src/NodeTest.cs b/src/ros2cs/ros2cs_tests/src/NodeTest.cs index 986ce171..2bcc397a 100644 --- a/src/ros2cs/ros2cs_tests/src/NodeTest.cs +++ b/src/ros2cs/ros2cs_tests/src/NodeTest.cs @@ -14,8 +14,8 @@ // limitations under the License. using System; -using NUnit.Framework; using example_interfaces.srv; +using NUnit.Framework; namespace ROS2.Test { @@ -69,6 +69,60 @@ public void IsDisposed() Assert.That(Node.IsDisposed); } + [Test] + public void DisposedPublisherCreation() + { + Node.Dispose(); + + Assert.Throws(() => + { + Node.CreatePublisher("test_publisher"); + }); + Assert.That(Node.Publishers.Count, Is.Zero); + } + + [Test] + public void DisposedSubscriptionCreation() + { + 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(() => + { + Node.CreateService( + "service_test", + request => { throw new InvalidOperationException($"received request {request}"); } + ); + }); + Assert.That(Node.Services.Count, Is.Zero); + } + + [Test] + public void DisposedClientCreation() + { + Node.Dispose(); + + Assert.Throws(() => + { + Node.CreateClient("client_test"); + }); + Assert.That(Node.Clients.Count, Is.Zero); + } + [Test] public void DoubleDisposal() { From 8adf7e5f099373fd57ce369a228ac680e34373b3 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 16 Mar 2023 17:15:26 +0100 Subject: [PATCH 25/46] improve Context documentation on thread safety Creating nodes is now thread safe. --- src/ros2cs/ros2cs_core/Context.cs | 77 ++++++++++++++----- src/ros2cs/ros2cs_core/interfaces/IContext.cs | 2 +- 2 files changed, 60 insertions(+), 19 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs index 94e18b9f..013c6f96 100644 --- a/src/ros2cs/ros2cs_core/Context.cs +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -20,10 +20,17 @@ namespace ROS2 /// /// ROS Context encapsulating the non-global state of an init/shutdown cycle. /// - /// Can be disposed by the garbage collector + /// + /// 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; } /// @@ -32,12 +39,19 @@ public sealed class Context : IContext /// 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(); - private rcl_allocator_t ROSAllocator; - /// /// Get the current RMW implementation. /// @@ -47,12 +61,17 @@ 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(this.ROSNodes, node => node); - this.ROSAllocator = NativeRcl.rcutils_get_default_allocator(); + 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, this.ROSAllocator); + int ret = NativeRclInterface.rclcs_init(this.Handle, NativeRcl.rcutils_get_default_allocator()); if ((RCLReturnEnum)ret != RCLReturnEnum.RCL_RET_OK) { this.FreeHandles(); @@ -61,25 +80,34 @@ public Context() } /// + /// + /// 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) { - if (this.ROSNodes.ContainsKey(name)) - { - node = default(INode); - return false; - } - else + lock (this.ROSNodes) { - Node ROSNode = new Node(name, this); - this.ROSNodes.Add(name, ROSNode); - node = ROSNode; - return true; + 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; + } } } @@ -88,14 +116,23 @@ public bool TryCreateNode(string name, out INode 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) { - return this.ROSNodes.Remove(name); + lock (this.ROSNodes) + { + return this.ROSNodes.Remove(name); + } } + /// + /// This method is not thread safe. + /// Do not call while the context or any entities + /// associated with it are in use. + /// /// public void Dispose() { @@ -132,9 +169,13 @@ private void Dispose(bool disposing) } } + /// + /// Free the handles of this instance. + /// private void FreeHandles() { NativeRclInterface.rclcs_free_context(this.Handle); + // to allow .IsDisposed to work this.Handle = IntPtr.Zero; } diff --git a/src/ros2cs/ros2cs_core/interfaces/IContext.cs b/src/ros2cs/ros2cs_core/interfaces/IContext.cs index c649d21d..69c73759 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IContext.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IContext.cs @@ -21,7 +21,7 @@ namespace ROS2 /// ROS Context encapsulating the non-global state of an init/shutdown cycle. /// /// - /// Instances should be disposed with which is NOT automatically performed completely on shutdown. + /// Instances should be disposed with which may NOT automatically performed completely on garbage collection. /// public interface IContext : IExtendedDisposable { From 802dc40e79d98167324d2659de57a5d3aab66e05 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 22 Mar 2023 17:30:51 +0100 Subject: [PATCH 26/46] schedule rescan when rescanning fails --- .../ros2cs_core/executors/ManualExecutor.cs | 45 ++++++++++++------- .../ros2cs_tests/src/ManualExecutorTest.cs | 8 +++- 2 files changed, 36 insertions(+), 17 deletions(-) diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index 62fd3e49..3cdedba2 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -32,6 +32,7 @@ public bool IsSpinning public bool RescanScheduled { get { return this._RescanScheduled; } + private set { this._RescanScheduled = value; } } /// @@ -166,7 +167,7 @@ IEnumerator IEnumerable.GetEnumerator() /// public void ScheduleRescan() { - this._RescanScheduled = true; + this.RescanScheduled = true; } /// @@ -186,7 +187,7 @@ public void Wait() /// public bool TryWait(TimeSpan timeout) { - if (this._RescanScheduled && this.IsSpinning) + if (this.RescanScheduled && this.IsSpinning) { try { @@ -224,7 +225,7 @@ public bool TrySpin(TimeSpan timeout) this.IsIdle.Reset(); try { - if (this._RescanScheduled) + if (this.RescanScheduled) { return false; } @@ -250,27 +251,39 @@ public bool TrySpin(TimeSpan timeout) /// This clears any scheduled rescans. 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 it first to prevent clearing rescans + // clear the flag to prevent clearing rescans // scheduled just after we finished rescaning - this._RescanScheduled = false; - foreach (INode node in this.Nodes) + this.RescanScheduled = false; + try { - foreach (ISubscriptionBase subscription in node.Subscriptions) - { - this.WaitSet.Subscriptions.Add(subscription); - } - foreach (IServiceBase service in node.Services) + foreach (INode node in this.Nodes) { - this.WaitSet.Services.Add(service); - } - foreach (IClientBase client in node.Clients) - { - this.WaitSet.Clients.Add(client); + 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; + } } /// diff --git a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs index c20f1263..61f9c2c5 100644 --- a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs @@ -16,11 +16,17 @@ public class ManualExecutorTest private ManualExecutor Executor; + private WaitSet WaitSet; + [SetUp] public void SetUp() { this.Context = new Context(); - this.Executor = new ManualExecutor(this.Context); + this.WaitSet = new WaitSet(this.Context); + this.Executor = new ManualExecutor( + this.WaitSet, + new GuardCondition(this.Context, () => { }) + ); } [TearDown] From acb813fc8936b7d74e945cecefe096cf923065cf Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 22 Mar 2023 19:04:38 +0100 Subject: [PATCH 27/46] improve Publisher documentation The class is now public to allow users to read the implementation documentation like thread safety. To prevent access to internal methods the class remains sealed and uses explicit interface implementations. --- src/ros2cs/ros2cs_core/Publisher.cs | 56 ++++++++++++++++++++++++++--- 1 file changed, 52 insertions(+), 4 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index b98e822f..153c63ad 100644 --- a/src/ros2cs/ros2cs_core/Publisher.cs +++ b/src/ros2cs/ros2cs_core/Publisher.cs @@ -18,9 +18,16 @@ namespace ROS2 { - /// Publisher of a topic with a given type. + /// + /// 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. + /// + /// /// - internal sealed class Publisher : IPublisher, IRawPublisher where T : Message, new() + public sealed class Publisher : IPublisher, IRawPublisher where T : Message, new() { /// public string Topic { get; private set; } @@ -36,12 +43,32 @@ public bool IsDisposed } } + /// + /// Handle to the rcl publisher + /// private IntPtr Handle = IntPtr.Zero; + /// + /// Handle to the rcl publisher options + /// private IntPtr Options = IntPtr.Zero; + /// + /// Node associated with this instance. + /// private readonly Node Node; + /// + /// 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; @@ -68,15 +95,30 @@ internal Publisher(string topic, Node node, QualityOfServiceProfile qos = null) } } + + /// + /// 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() { @@ -101,11 +143,11 @@ private void Dispose(bool disposing) Debug.Assert(success, "failed to remove publisher"); } - this.DisposeFromNode(); + (this as IRawPublisher).DisposeFromNode(); } /// - public void DisposeFromNode() + void IRawPublisher.DisposeFromNode() { if (this.Handle == IntPtr.Zero) { @@ -116,6 +158,12 @@ public void DisposeFromNode() 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); From 2f72a515d977c33b12e7ba7e6c61b84f55710fc8 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 22 Mar 2023 19:31:06 +0100 Subject: [PATCH 28/46] improve Subscription documentation The class is now public to allow users to read the implementation documentation like thread safety. To prevent access to internal methods the class remains sealed and uses explicit interface implementations. --- src/ros2cs/ros2cs_core/Subscription.cs | 58 ++++++++++++++++++++++++-- 1 file changed, 54 insertions(+), 4 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index c09728dc..f92a874e 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -19,9 +19,16 @@ namespace ROS2 { - /// Subscription to a topic with a given type. + /// + /// 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. + /// + /// /// - internal sealed class Subscription : ISubscription, IRawSubscription where T : Message, new() + public sealed class Subscription : ISubscription, IRawSubscription where T : Message, new() { /// public string Topic { get; private set; } @@ -37,14 +44,38 @@ public bool IsDisposed } } + /// + /// 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; @@ -72,6 +103,10 @@ internal Subscription(string topic, Node node, Action callback, QualityOfServ } } + /// + /// Both variants of this method are equivalent + /// and not thread safe. + /// /// public bool TryProcess() { @@ -99,12 +134,21 @@ public bool TryProcess() return true; } + /// + /// Both variants of this method are equivalent + /// and not thread safe. + /// /// public Task TryProcessAsync() { return Task.FromResult(this.TryProcess()); } + /// + /// 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() { @@ -131,11 +175,11 @@ private void Dispose(bool disposing) this.Node.Executor?.Wait(); } - this.DisposeFromNode(); + (this as IRawSubscription).DisposeFromNode(); } /// - public void DisposeFromNode() + void IRawSubscription.DisposeFromNode() { if (this.Handle == IntPtr.Zero) { @@ -146,6 +190,12 @@ public void DisposeFromNode() 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_subscription(this.Handle); From 4535da30310fae98c02388f7b73cfbec87526dac Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Tue, 28 Mar 2023 14:31:08 +0200 Subject: [PATCH 29/46] improve Service documentation The class is now public to allow users to read the implementation documentation like thread safety. To prevent access to internal methods the class remains sealed and uses explicit interface implementations. --- src/ros2cs/ros2cs_core/Service.cs | 57 ++++++++++++++++++++++++++++--- 1 file changed, 52 insertions(+), 5 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 66b54915..3d788f17 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -19,9 +19,16 @@ namespace ROS2 { - /// Service with a topic and Types for Messages. + /// + /// Service with a topic and types for Messages wrapping a rcl service. + /// + /// + /// This is the implementation produced by , + /// use this method to create new instances. + /// + /// /// - internal sealed class Service : IService, IRawService + public sealed class Service : IService, IRawService where I : Message, new() where O : Message, new() { @@ -39,17 +46,38 @@ public bool IsDisposed } } + /// + /// 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 + /// 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; @@ -78,6 +106,10 @@ internal Service(string topic, Node node, Func callback, QualityOfServiceP } } + /// + /// Both variants of this method are equivalent + /// and not thread safe. + /// /// public bool TryProcess() { @@ -105,6 +137,10 @@ public bool TryProcess() return true; } + /// + /// Both variants of this method are equivalent + /// and not thread safe. + /// /// public Task TryProcessAsync() { @@ -140,6 +176,11 @@ private void SendResp(rcl_rmw_request_id_t header, O msg) GC.KeepAlive(this); } + /// + /// 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() { @@ -166,11 +207,11 @@ private void Dispose(bool disposing) this.Node.Executor?.Wait(); } - this.DisposeFromNode(); + (this as IRawService).DisposeFromNode(); } /// - public void DisposeFromNode() + void IRawService.DisposeFromNode() { if (this.Handle == IntPtr.Zero) { @@ -181,6 +222,12 @@ public void DisposeFromNode() 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_service(this.Handle); From 1d6cc1da05e353fe39e5a27277a574b9a93a4ef8 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Tue, 28 Mar 2023 15:03:34 +0200 Subject: [PATCH 30/46] improve Client documentation The class is now public to allow users to read the implementation documentation like thread safety. To prevent access to internal methods the class remains sealed and uses explicit interface implementations. --- src/ros2cs/ros2cs_core/Client.cs | 92 +++++++++++++++++++++++++++++--- 1 file changed, 85 insertions(+), 7 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index 15b03201..9e178f6e 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -22,21 +22,37 @@ namespace ROS2 { - /// Client with a topic and Types for Messages. + /// + /// Client with a topic and types for messages wrapping a rcl client. + /// + /// + /// This is the implementation produced by , + /// use this method to create new instances. + /// + /// /// - internal sealed class Client : IClient, IRawClient + public sealed class Client : IClient, IRawClient where I : Message, new() where O : Message, new() { /// 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; /// @@ -50,10 +66,19 @@ public bool IsDisposed } } - public IntPtr Handle {get; private set; } = IntPtr.Zero; + /// + /// 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; /// @@ -62,14 +87,22 @@ public bool IsDisposed /// /// 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)>(); /// - /// Internal constructor for Client + /// Create a new instance. /// - /// Use to construct new Instances - public Client(string topic, Node node, QualityOfServiceProfile qos = null) + /// + /// 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) { this.Topic = topic; this.Node = node; @@ -106,6 +139,10 @@ public Client(string topic, Node node, QualityOfServiceProfile qos = null) } } + /// + /// This method is not thread safe. + /// + /// If the instance was disposed. /// public bool IsServiceAvailable() { @@ -119,6 +156,10 @@ out available return available; } + /// + /// Both variants of this method are equivalent + /// and thread safe. + /// /// public bool TryProcess() { @@ -165,12 +206,21 @@ public bool TryProcess() return true; } + /// + /// Both variants of this method are equivalent + /// and thread safe. + /// /// public Task TryProcessAsync() { return Task.FromResult(this.TryProcess()); } + /// + /// 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) { @@ -179,12 +229,20 @@ public O Call(I msg) return task.Result; } + /// + /// This method is thread safe. + /// + /// If the instance was disposed. /// public Task CallAsync(I msg) { return CallAsync(msg, TaskCreationOptions.None); } + /// + /// This method is thread safe. + /// + /// If the instance was disposed. /// public Task CallAsync(I msg, TaskCreationOptions options) { @@ -232,6 +290,10 @@ private Task RegisterSource(TaskCompletionSource source, long sequence_num return task; } + /// + /// 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) { @@ -254,6 +316,13 @@ public bool Cancel(Task task) return true; } + /// + /// 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() { @@ -286,7 +355,7 @@ private void Dispose(bool disposing) } /// - public void DisposeFromNode() + void IRawClient.DisposeFromNode() { if (this.Handle == IntPtr.Zero) { @@ -298,6 +367,9 @@ public void DisposeFromNode() this.FreeHandles(); } + /// + /// Dispose all tasks currently pending. + /// private void DisposeAllTasks() { lock (this.Requests) @@ -310,6 +382,12 @@ private void DisposeAllTasks() } } + /// + /// Free the rcl handles and replace them with null pointers. + /// + /// + /// The handles are not finalised by this method. + /// private void FreeHandles() { NativeRclInterface.rclcs_free_client(this.Handle); From 9bcafbddc433ffb9daa689d41bc15e52677f54d6 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Tue, 28 Mar 2023 16:04:27 +0200 Subject: [PATCH 31/46] improve Node documentation The class is now public to allow users to read the implementation documentation like thread safety. Furthermore, the primitive collections (and by extension their methods) are now thread safe. --- src/ros2cs/ros2cs_core/CMakeLists.txt | 1 + src/ros2cs/ros2cs_core/Node.cs | 177 +++++++++++++++--- .../ros2cs_core/utils/LockedCollection.cs | 52 +++++ 3 files changed, 200 insertions(+), 30 deletions(-) create mode 100644 src/ros2cs/ros2cs_core/utils/LockedCollection.cs diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index 36d0a0b4..0289a250 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -95,6 +95,7 @@ set(CS_NATIVE set(CS_UTILS utils/Utils.cs utils/LockedDictionary.cs + utils/LockedCollection.cs utils/MappedValueDictionary.cs ) diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 28d7c8be..0fb51606 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -18,8 +18,16 @@ namespace ROS2 { + /// + /// Node wrapping a rcl node. + /// + /// + /// This is the implementation produced by , + /// use this method to create instances. + /// + /// /// - internal sealed class Node : INode + public sealed class Node : INode { /// public string Name { get; private set; } @@ -36,36 +44,77 @@ public bool IsDisposed get { return !NativeRclInterface.rclcs_node_is_valid(this.Handle); } } - internal IntPtr Handle = IntPtr.Zero; + /// + /// Handle to the rcl node + /// + internal IntPtr Handle { get; private set; } = IntPtr.Zero; - private IntPtr Options; + /// + /// Handle to the rcl node options + /// + private IntPtr Options = IntPtr.Zero; + /// + /// Context associated with this instance. + /// private readonly Context ROSContext; + /// + /// 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 { return this.CurrentPublishers; } } + public IReadOnlyCollection Publishers { get; private set; } private readonly HashSet CurrentPublishers = new HashSet(); + /// + /// This collection is thread safe. + /// /// - public IReadOnlyCollection Subscriptions { get { return this.CurrentSubscriptions; } } + public IReadOnlyCollection Subscriptions { get; private set; } private readonly HashSet CurrentSubscriptions = new HashSet(); + /// + /// This collection is thread safe. + /// /// - public IReadOnlyCollection Services { get { return this.CurrentServices; } } + public IReadOnlyCollection Services { get; private set; } private readonly HashSet CurrentServices = new HashSet(); + /// + /// This collection is thread safe. + /// /// - public IReadOnlyCollection Clients { get { return this.CurrentClients; } } + 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) { 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( @@ -91,13 +140,21 @@ internal Node(string name, Context context) } } + /// + /// This method is thread safe. + /// + /// If the instance was disposed. + /// /// public IPublisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new() { - Publisher publisher = new Publisher(topic, this, qos); - bool success = this.CurrentPublishers.Add(publisher); - Debug.Assert(success, "publisher already exists"); - return publisher; + lock (this.Lock) + { + Publisher publisher = new Publisher(topic, this, qos); + bool success = this.CurrentPublishers.Add(publisher); + Debug.Assert(success, "publisher already exists"); + return publisher; + } } /// @@ -105,20 +162,34 @@ internal Node(string name, Context context) /// /// /// 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) { - return this.CurrentPublishers.Remove(publisher); + lock (this.Lock) + { + return this.CurrentPublishers.Remove(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 ISubscription CreateSubscription(string topic, Action callback, QualityOfServiceProfile qos = null) where T : Message, new() { - Subscription subscription = new Subscription(topic, this, callback, qos); - bool success = this.CurrentSubscriptions.Add(subscription); - Debug.Assert(success, "subscription already exists"); + 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; } @@ -128,25 +199,40 @@ internal bool RemovePublisher(IRawPublisher publisher) /// /// /// This method is intended to be used by and does not dispose the subscription. + /// Furthermore, it is thread safe if of the current executor is thread safe. /// /// Subscription to be removed. /// If the subscription existed on this node and has been removed. internal bool RemoveSubscription(IRawSubscription subscription) { - if (this.CurrentSubscriptions.Remove(subscription)) + bool removed; + lock (this.Lock) + { + removed = this.CurrentSubscriptions.Remove(subscription); + } + if (removed) { this.Executor?.TryScheduleRescan(this); - return true; } - return false; + return removed; } + /// + /// 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() { - Client client = new Client(topic, this, qos); - bool success = this.CurrentClients.Add(client); - Debug.Assert(success, "client already exists"); + 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; } @@ -156,25 +242,40 @@ internal bool RemoveSubscription(IRawSubscription subscription) /// /// /// This method is intended to be used by and does not dispose the client. + /// Furthermore, it is thread safe if of the current executor is thread safe. /// /// Client to be removed. /// If the client existed on this node and has been removed. internal bool RemoveClient(IRawClient client) { - if (this.CurrentClients.Remove(client)) + bool removed; + lock (this.Lock) + { + removed = this.CurrentClients.Remove(client); + } + if (removed) { this.Executor?.TryScheduleRescan(this); - return true; } - return false; + return removed; } + /// + /// 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 = new Service(topic, this, callback, qos); - bool success = this.CurrentServices.Add(service); - Debug.Assert(success, "service already exists"); + 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; } @@ -184,19 +285,29 @@ internal bool RemoveClient(IRawClient client) /// /// /// This method is intended to be used by and does not dispose the service. + /// Furthermore, it is thread safe if of the current executor is thread safe. /// /// Service to be removed. /// If the service existed on this node and has been removed. internal bool RemoveService(IRawService service) { - if (this.CurrentServices.Remove(service)) + bool removed; + lock (this.Lock) + { + removed = this.CurrentServices.Remove(service); + } + if (removed) { this.Executor?.TryScheduleRescan(this); - return true; } - return false; + return removed; } + /// + /// 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() { @@ -256,6 +367,12 @@ internal void DisposeFromContext() 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_node(this.Handle); diff --git a/src/ros2cs/ros2cs_core/utils/LockedCollection.cs b/src/ros2cs/ros2cs_core/utils/LockedCollection.cs new file mode 100644 index 00000000..8cf37ab5 --- /dev/null +++ b/src/ros2cs/ros2cs_core/utils/LockedCollection.cs @@ -0,0 +1,52 @@ +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 From cd161f0d7b89e5507f1cb78e4035f3817ac02def Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Tue, 28 Mar 2023 17:05:04 +0200 Subject: [PATCH 32/46] remove `IWaitable.TryProcessAsync` Async operations are out of scope for this branch. --- src/ros2cs/ros2cs_core/Client.cs | 13 +------------ src/ros2cs/ros2cs_core/Service.cs | 14 +------------- src/ros2cs/ros2cs_core/Subscription.cs | 14 +------------- .../ros2cs_core/interfaces/IWaitable.cs | 11 +---------- src/ros2cs/ros2cs_tests/src/ClientTest.cs | 19 ++----------------- src/ros2cs/ros2cs_tests/src/ServiceTest.cs | 17 +---------------- .../ros2cs_tests/src/SubscriptionTest.cs | 17 ----------------- 7 files changed, 7 insertions(+), 98 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index 9e178f6e..7bfc8c73 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -157,8 +157,7 @@ out available } /// - /// Both variants of this method are equivalent - /// and thread safe. + /// This method is thread safe. /// /// public bool TryProcess() @@ -206,16 +205,6 @@ public bool TryProcess() return true; } - /// - /// Both variants of this method are equivalent - /// and thread safe. - /// - /// - public Task TryProcessAsync() - { - return Task.FromResult(this.TryProcess()); - } - /// /// The provided message can be modified or disposed after this call. /// Furthermore, this method is thread safe. diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index 3d788f17..e41a6de3 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -14,7 +14,6 @@ using System; using System.Diagnostics; -using System.Threading.Tasks; using ROS2.Internal; namespace ROS2 @@ -107,8 +106,7 @@ internal Service(string topic, Node node, Func callback, QualityOfServiceP } /// - /// Both variants of this method are equivalent - /// and not thread safe. + /// This method is not thread safe. /// /// public bool TryProcess() @@ -137,16 +135,6 @@ public bool TryProcess() return true; } - /// - /// Both variants of this method are equivalent - /// and not thread safe. - /// - /// - public Task TryProcessAsync() - { - return Task.FromResult(this.TryProcess()); - } - /// /// Populates managed fields with native values and calls the callback with the created message /// diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index f92a874e..7c779395 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -14,7 +14,6 @@ using System; using System.Diagnostics; -using System.Threading.Tasks; using ROS2.Internal; namespace ROS2 @@ -104,8 +103,7 @@ internal Subscription(string topic, Node node, Action callback, QualityOfServ } /// - /// Both variants of this method are equivalent - /// and not thread safe. + /// This method is not thread safe. /// /// public bool TryProcess() @@ -134,16 +132,6 @@ public bool TryProcess() return true; } - /// - /// Both variants of this method are equivalent - /// and not thread safe. - /// - /// - public Task TryProcessAsync() - { - return Task.FromResult(this.TryProcess()); - } - /// /// This method is not thread safe and may not be called from /// multiple threads simultaneously or while the subscription is in use. diff --git a/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs index 9cd8f4ec..dea99a24 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs @@ -1,5 +1,4 @@ using System; -using System.Threading.Tasks; namespace ROS2 { @@ -16,15 +15,7 @@ public interface IWaitable /// /// Try to process if this instance is ready. /// - /// Both variants of this method are equivalent. - /// If the instance was ready. + /// If the instance was ready. bool TryProcess(); - - /// - /// Try to process preferably asynchronously if this instance is ready. - /// - /// indicating if the instance was ready. - /// - Task TryProcessAsync(); } } diff --git a/src/ros2cs/ros2cs_tests/src/ClientTest.cs b/src/ros2cs/ros2cs_tests/src/ClientTest.cs index 664d6e7f..dc7c8c9a 100644 --- a/src/ros2cs/ros2cs_tests/src/ClientTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ClientTest.cs @@ -213,22 +213,7 @@ public void ClientCancel() [Test] public void ClientTryProcess() { - this.ClientTryProcessTest(this.Client.TryProcess); - } - - [Test] - public void ClientTryProcessAsync() - { - this.ClientTryProcessTest(() => { - Task task = this.Client.TryProcessAsync(); - task.Wait(); - return task.Result; - }); - } - - private void ClientTryProcessTest(Func implementation) - { - Assert.That(implementation(), Is.False); + Assert.That(this.Client.TryProcess(), Is.False); using var service = Node.CreateService( SERVICE_NAME, @@ -237,7 +222,7 @@ private void ClientTryProcessTest(Func implementation) Task pendingTask = this.Client.CallAsync(this.CreateRequest(3, 4)); while (!service.TryProcess()) {} - while (!implementation()) + while (!this.Client.TryProcess()) {} Assert.That(pendingTask.IsCompletedSuccessfully); diff --git a/src/ros2cs/ros2cs_tests/src/ServiceTest.cs b/src/ros2cs/ros2cs_tests/src/ServiceTest.cs index cc9ff442..835fc3bc 100644 --- a/src/ros2cs/ros2cs_tests/src/ServiceTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ServiceTest.cs @@ -69,22 +69,7 @@ public void DoubleDisposeService() [Test] public void ServiceTryProcess() { - ServiceTryProcessTest(Service.TryProcess); - } - - [Test] - public void ServiceTryProcessAsync() - { - ServiceTryProcessTest(() => { - var task = Service.TryProcessAsync(); - task.Wait(); - return task.Result; - }); - } - - private void ServiceTryProcessTest(Func implementation) - { - Assert.That(implementation(), Is.False); + 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 b7fa92cb..6c768155 100644 --- a/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs +++ b/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs @@ -157,22 +157,5 @@ public void SubscriptionQosSensorDataDepth() Assert.That(count, Is.EqualTo(5)); } - - [Test] - public void SubscriptionTryProcessAsync() - { - bool callbackTriggered = false; - using var subscription = Node.CreateSubscription( - TOPIC, - (msg) => { callbackTriggered = true; } - ); - Node.CreatePublisher(TOPIC).Publish(CreateMessage(0)); - - var task = subscription.TryProcessAsync(); - task.Wait(); - Assert.That(task.Result); - - Assert.That(callbackTriggered, Is.True); - } } } From 1e51b5228bc4b45e97377683a754e09fd4a58f71 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 29 Mar 2023 16:48:05 +0200 Subject: [PATCH 33/46] add tests for node scheduling rescans --- src/ros2cs/ros2cs_tests/src/NodeTest.cs | 136 ++++++++++++++++++++++++ 1 file changed, 136 insertions(+) diff --git a/src/ros2cs/ros2cs_tests/src/NodeTest.cs b/src/ros2cs/ros2cs_tests/src/NodeTest.cs index 2bcc397a..7df4007a 100644 --- a/src/ros2cs/ros2cs_tests/src/NodeTest.cs +++ b/src/ros2cs/ros2cs_tests/src/NodeTest.cs @@ -16,6 +16,7 @@ using System; using example_interfaces.srv; using NUnit.Framework; +using ROS2.Executors; namespace ROS2.Test { @@ -160,6 +161,19 @@ public void DisposeAllOnDispose() 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() { @@ -171,6 +185,20 @@ public void CreatePublisher() 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] public void DisposePublisher() { @@ -183,6 +211,20 @@ public void DisposePublisher() Assert.That(Node.Publishers, Does.Not.Contain(publisher)); } + [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(); + + // publisher not in executor + Assert.That(executor.RescanScheduled, Is.False); + } + [Test] public void CreateSubscription() { @@ -197,6 +239,22 @@ public void CreateSubscription() 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() { @@ -212,6 +270,22 @@ public void DisposeSubscription() 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() { @@ -226,6 +300,22 @@ public void CreateService() Assert.That(service.Topic, Is.EqualTo(topic)); } + [Test] + public void CreateServiceWithExecutor() + { + 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() { @@ -241,6 +331,22 @@ public void DisposeService() 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 CreateClient() { @@ -254,6 +360,21 @@ public void CreateClient() 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() { @@ -267,5 +388,20 @@ public void DisposeClient() 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); + } } } From 4a4f19b081cd0e365bc530259795fe758aaa99c1 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 30 Mar 2023 14:39:42 +0200 Subject: [PATCH 34/46] make `Node.Executor` thread safe This commit prevents node primitives from waiting on the wrong executor while being disposed. --- src/ros2cs/ros2cs_core/Client.cs | 3 +- src/ros2cs/ros2cs_core/Node.cs | 101 +++++++++++++------------ src/ros2cs/ros2cs_core/Service.cs | 3 +- src/ros2cs/ros2cs_core/Subscription.cs | 3 +- 4 files changed, 54 insertions(+), 56 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index 7bfc8c73..d5e4a4f8 100644 --- a/src/ros2cs/ros2cs_core/Client.cs +++ b/src/ros2cs/ros2cs_core/Client.cs @@ -330,12 +330,11 @@ private void Dispose(bool disposing) } // 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"); - // not required if we are being finalized since the executor cant be running - this.Node.Executor?.Wait(); this.DisposeAllTasks(); } diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 0fb51606..a87a5f60 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -35,8 +35,28 @@ public sealed class Node : INode /// 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 { get; set; } + public IExecutor Executor + { + get { return this._Executor; } + set + { + // prevent a executor switch while + // a primitive is being removed + lock (this.Lock) + { + this._Executor = value; + } + } + } + + private IExecutor _Executor = null; /// public bool IsDisposed @@ -194,27 +214,10 @@ internal bool RemovePublisher(IRawPublisher publisher) return subscription; } - /// - /// Remove a subscription. - /// - /// - /// This method is intended to be used by and does not dispose the subscription. - /// Furthermore, it is thread safe if of the current executor is thread safe. - /// - /// Subscription to be removed. - /// If the subscription existed on this node and has been removed. - internal bool RemoveSubscription(IRawSubscription subscription) + /// + internal bool RemoveSubscription(IRawSubscription primitive) { - bool removed; - lock (this.Lock) - { - removed = this.CurrentSubscriptions.Remove(subscription); - } - if (removed) - { - this.Executor?.TryScheduleRescan(this); - } - return removed; + return this.RemovePrimitive(primitive, this.CurrentSubscriptions); } /// @@ -237,27 +240,10 @@ internal bool RemoveSubscription(IRawSubscription subscription) return client; } - /// - /// Remove a client. - /// - /// - /// This method is intended to be used by and does not dispose the client. - /// Furthermore, it is thread safe if of the current executor is thread safe. - /// - /// Client to be removed. - /// If the client existed on this node and has been removed. - internal bool RemoveClient(IRawClient client) + /// + internal bool RemoveClient(IRawClient primitive) { - bool removed; - lock (this.Lock) - { - removed = this.CurrentClients.Remove(client); - } - if (removed) - { - this.Executor?.TryScheduleRescan(this); - } - return removed; + return this.RemovePrimitive(primitive, this.CurrentClients); } /// @@ -280,25 +266,40 @@ internal bool RemoveClient(IRawClient client) return service; } + /// + internal bool RemoveService(IRawService primitive) + { + return this.RemovePrimitive(primitive, this.CurrentServices); + } + /// - /// Remove a service. + /// Remove a primitive and wait for it to be disposable. /// /// - /// This method is intended to be used by and does not dispose the service. - /// Furthermore, it is thread safe if of the current executor is thread safe. + /// 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. /// - /// Service to be removed. - /// If the service existed on this node and has been removed. - internal bool RemoveService(IRawService service) + /// 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 = this.CurrentServices.Remove(service); + 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) + if (removed && !(currentExecutor is null)) { - this.Executor?.TryScheduleRescan(this); + currentExecutor.TryScheduleRescan(this); + currentExecutor.Wait(); } return removed; } diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index e41a6de3..d0b7449b 100644 --- a/src/ros2cs/ros2cs_core/Service.cs +++ b/src/ros2cs/ros2cs_core/Service.cs @@ -187,12 +187,11 @@ private void Dispose(bool disposing) } // 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"); - // not required if we are being finalized since the executor cant be running - this.Node.Executor?.Wait(); } (this as IRawService).DisposeFromNode(); diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 7c779395..8ca0295a 100644 --- a/src/ros2cs/ros2cs_core/Subscription.cs +++ b/src/ros2cs/ros2cs_core/Subscription.cs @@ -155,12 +155,11 @@ private void Dispose(bool disposing) } // 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"); - // not required if we are being finalized since the executor cant be running - this.Node.Executor?.Wait(); } (this as IRawSubscription).DisposeFromNode(); From 7556027a079dbdd80ae606e9017f9e785310ad37 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 30 Mar 2023 15:59:29 +0200 Subject: [PATCH 35/46] improve documentation of ManualExecutor --- .../ros2cs_core/executors/ManualExecutor.cs | 199 ++++++++++++++---- 1 file changed, 161 insertions(+), 38 deletions(-) diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index 3cdedba2..76a7ffaf 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -24,51 +24,88 @@ public IContext Context get { return this.WaitSet.Context; } } + /// + /// Whether the executor is currently spinning. + /// public bool IsSpinning { get { return !this.IsIdle.IsSet; } } + /// + /// Whether a rescan is scheduled. + /// public bool RescanScheduled { get { return this._RescanScheduled; } private set { this._RescanScheduled = value; } } + // volatile since it may be changed by multiple threads + private volatile bool _RescanScheduled = false; + /// public bool IsDisposed { get { return this.WaitSet.IsDisposed || this.InterruptCondition.IsDisposed; } } + /// + /// This property is thread safe. + /// /// public int Count { - get { return this.Nodes.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. + /// private readonly WaitSet WaitSet; + /// + /// Guard condition used for interrupting waits. + /// private readonly GuardCondition InterruptCondition; + /// + /// Nodes in the executor. + /// private readonly HashSet Nodes = new HashSet(); + /// + /// Event signaling whether the executor is not spinning. + /// private readonly ManualResetEventSlim IsIdle = new ManualResetEventSlim(true); - private volatile bool _RescanScheduled = false; - + /// + /// Create a new instance. + /// + /// Context to associate with. + /// If is disposed. public ManualExecutor(Context context) : this( new WaitSet(context), new GuardCondition(context, () => { }) ) { } + /// internal ManualExecutor(WaitSet waitSet, GuardCondition interruptCondition) { this.WaitSet = waitSet; @@ -76,6 +113,12 @@ internal ManualExecutor(WaitSet waitSet, GuardCondition 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) { @@ -83,50 +126,80 @@ public void Add(INode node) { throw new InvalidOperationException("node already has an executor"); } - this.Nodes.Add(node); + // 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) { - if (this.Nodes.Remove(node)) + bool removed; + lock (this.Nodes) { - Debug.Assert( - Object.ReferenceEquals(node.Executor, this), - "node has different executor" - ); - node.Executor = null; - this.ScheduleRescan(); - this.Wait(); - return true; + 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 false; + return removed; } + /// + /// This method is thread safe when setting + /// is thread safe. + /// + /// If the executor was disposed. /// public void Clear() { - if (this.Nodes.Count == 0) - { - return; - } - foreach (INode node in this.Nodes.ToArray()) + // use thread safe enumerator + foreach (INode node in this) { - this.Nodes.Remove(node); - node.Executor = null; + this.Remove(node); } - this.ScheduleRescan(); - this.Wait(); } + /// + /// This method is thread safe. + /// /// public bool Contains(INode node) { - return this.Nodes.Contains(node); + lock (this.Nodes) + { + return this.Nodes.Contains(node); + } } + /// + /// This method is thread safe. + /// /// public void CopyTo(INode[] array, int index) { @@ -138,24 +211,33 @@ public void CopyTo(INode[] array, int index) { throw new ArgumentOutOfRangeException("index is less than 0"); } - foreach (var item in this) + lock (this.Nodes) { - try + foreach (INode item in this.Nodes) { - array[index] = item; - } - catch (IndexOutOfRangeException e) - { - throw new ArgumentException("array is too small", e); + try + { + array[index] = item; + } + catch (IndexOutOfRangeException e) + { + throw new ArgumentException("array is too small", e); + } + index += 1; } - index += 1; } } + /// + /// The enumerator is thread safe. + /// /// public IEnumerator GetEnumerator() { - return this.Nodes.GetEnumerator(); + lock (this.Nodes) + { + return this.Nodes.ToArray().AsEnumerable().GetEnumerator(); + } } /// @@ -164,12 +246,18 @@ 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) { @@ -177,6 +265,9 @@ public bool TryScheduleRescan(INode node) return true; } + /// + /// This method is thread safe and uses . + /// /// public void Wait() { @@ -184,6 +275,10 @@ public void Wait() Debug.Assert(success, "infinite wait timed out"); } + /// + /// This method is thread safe. + /// + /// If the executor was disposed. /// public bool TryWait(TimeSpan timeout) { @@ -209,7 +304,9 @@ public bool TryWait(TimeSpan timeout) /// /// /// 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(); @@ -218,13 +315,19 @@ public void Interrupt() /// /// 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.IsIdle.Reset(); try { + // check after resetting IsIdle to + // prevent race condition if (this.RescanScheduled) { return false; @@ -246,9 +349,12 @@ public bool TrySpin(TimeSpan timeout) /// /// Rescan the nodes of this executor for - /// new objects to wait for. + /// new objects to wait for and clear scheduled rescans. /// - /// This clears any 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 @@ -262,7 +368,8 @@ public void Rescan() this.RescanScheduled = false; try { - foreach (INode node in this.Nodes) + // use the thread safe GetEnumerator wrapper + foreach (INode node in this) { foreach (ISubscriptionBase subscription in node.Subscriptions) { @@ -292,7 +399,7 @@ public void Rescan() /// Maximum time to wait for work to become available. /// /// trying to spin in each iteration - /// and yielding if a rescan had to be performed. + /// and yielding if a rescan had to be performed instead. /// public IEnumerator Spin(TimeSpan timeout) { @@ -310,10 +417,26 @@ public IEnumerator Spin(TimeSpan timeout) } } + /// + /// 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() { - this.Clear(); + // 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(); this.IsIdle.Dispose(); From 2fbfeae4740234d4ec32d26beaa4d9ff4c10b40b Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 30 Mar 2023 17:17:55 +0200 Subject: [PATCH 36/46] update examples --- src/ros2cs/ros2cs_examples/ROS2Client.cs | 56 +++--- src/ros2cs/ros2cs_examples/ROS2Listener.cs | 30 +-- .../ROS2PerformanceListener.cs | 180 ++++++++++-------- .../ros2cs_examples/ROS2PerformanceTalker.cs | 147 +++++++------- src/ros2cs/ros2cs_examples/ROS2Service.cs | 49 ++--- src/ros2cs/ros2cs_examples/ROS2Talker.cs | 39 ++-- 6 files changed, 267 insertions(+), 234 deletions(-) diff --git a/src/ros2cs/ros2cs_examples/ROS2Client.cs b/src/ros2cs/ros2cs_examples/ROS2Client.cs index e9869e65..ebc628f6 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Client.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Client.cs @@ -14,38 +14,48 @@ 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); + for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) + { + if (rsp.IsCompleted) + { + break; + } + } + + 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..8d976a8a 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Listener.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Listener.cs @@ -13,23 +13,31 @@ // 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(); + for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) + {} + } } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs b/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs index 304eeb81..cfb43176 100644 --- a/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs +++ b/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs @@ -13,113 +13,125 @@ // 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) + ); + + for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) + { + if (exitScheduled) + { + break; + } + } + } } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs b/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs index 7acf5f9d..9b3bce08 100644 --- a/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs +++ b/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs @@ -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..892b2b37 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Service.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Service.cs @@ -14,34 +14,39 @@ 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; + } + ); + + for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) + {} + } } - } } diff --git a/src/ros2cs/ros2cs_examples/ROS2Talker.cs b/src/ros2cs/ros2cs_examples/ROS2Talker.cs index afaa93a3..a782a479 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Talker.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Talker.cs @@ -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); + } + } } - } } From 89aaf68a32b77b15efcf0974c72fd1bee41f6493 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 31 Mar 2023 14:33:56 +0200 Subject: [PATCH 37/46] simplify WaitSet The results are now only exposed as `IEnumerable`s. --- src/ros2cs/ros2cs_core/WaitSet.cs | 550 +++--------------- .../ros2cs_core/native/NativeRclInterface.cs | 32 - .../ros2cs_core/native/rcl_native_interface.c | 44 -- .../ros2cs_tests/src/GuardConditionTest.cs | 2 +- src/ros2cs/ros2cs_tests/src/WaitSetTest.cs | 114 ++-- 5 files changed, 124 insertions(+), 618 deletions(-) diff --git a/src/ros2cs/ros2cs_core/WaitSet.cs b/src/ros2cs/ros2cs_core/WaitSet.cs index f36eb581..11c5d78c 100644 --- a/src/ros2cs/ros2cs_core/WaitSet.cs +++ b/src/ros2cs/ros2cs_core/WaitSet.cs @@ -21,6 +21,7 @@ namespace ROS2 { /// /// Collection used for waiting on resources to become ready. + /// All methods and properties are NOT thread safe. /// internal sealed class WaitSet : IReadOnlyCollection, IExtendedDisposable { @@ -72,6 +73,9 @@ public int Count } } + /// + /// Handle to the rcl wait set. + /// private IntPtr Handle = IntPtr.Zero; /// @@ -289,32 +293,7 @@ public bool TryWait(TimeSpan timeout, out WaitResult result) } Utils.CheckReturnEnum(ret); - result = new WaitResult( - new ReadyDictionary( - this, - NativeRclInterface.rclcs_wait_set_get_subscription, - NativeRclInterface.rclcs_wait_set_set_subscription, - this.CurrentSubscriptions - ), - new ReadyDictionary( - this, - NativeRclInterface.rclcs_wait_set_get_client, - NativeRclInterface.rclcs_wait_set_set_client, - this.CurrentClients - ), - new ReadyDictionary( - this, - NativeRclInterface.rclcs_wait_set_get_service, - NativeRclInterface.rclcs_wait_set_set_service, - this.CurrentServices - ), - new ReadyDictionary( - this, - NativeRclInterface.rclcs_wait_set_get_guard_condition, - NativeRclInterface.rclcs_wait_set_set_guard_condition, - this.CurrentGuardConditions - ) - ); + result = new WaitResult(this); return true; } @@ -358,223 +337,119 @@ private void FreeHandles() } /// - /// Dictionary representing the containts of a wait set. + /// Result of waiting on a wait set. /// - /// Type of resource being represented - private sealed class ReadyDictionary : IDictionary where T : IWaitable + /// + /// 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 { - /// - public T this[int key] - { - get - { - if (this.TryGetValue(key, out T value)) - { - return value; - } - throw new KeyNotFoundException($"key {key} was not found"); - } - set { this.Add(key, value); } - } - - /// - public int Count { get { return this.Keys.Count; } } - - /// - public bool IsReadOnly { get { return true; } } - - /// - public ICollection Keys { get; private set; } - - /// - public ICollection Values { get; private set; } - - private readonly WaitSet WaitSet; - - /// - /// Modification version of wait set when created. - /// - private readonly uint CreatedVersion; - - /// - /// Delegate used to read the wait set. - /// - private readonly NativeRclInterface.WaitSetGetType Getter; - - /// - /// Delegate used to change the wait set. - /// - private readonly NativeRclInterface.WaitSetSetType Setter; - /// - /// Mapping between wait set index and wrapper. + /// Subscriptions which are ready. /// - private readonly IList Wrappers; - - internal ReadyDictionary(WaitSet waitSet, NativeRclInterface.WaitSetGetType getter, NativeRclInterface.WaitSetSetType setter, IList wrappers) + public IEnumerable ReadySubscriptions { - this.WaitSet = waitSet; - this.CreatedVersion = waitSet.Version; - this.Getter = getter; - this.Setter = setter; - this.Wrappers = wrappers; - this.Keys = new KeysCollection(this); - this.Values = new ValuesCollection(this); + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_subscription, + this.WaitSet.CurrentSubscriptions + ); } /// - /// Check if the wait set is not disposed or was modified. + /// Clients which are ready. /// - /// The wait set has been disposed - /// The wait set has been modified - private void AssertOk() + public IEnumerable ReadyClients { - if (this.WaitSet.IsDisposed) - { - throw new ObjectDisposedException("backing rcl wait set"); - } - if (this.CreatedVersion != this.WaitSet.Version) - { - throw new InvalidOperationException("wait set has been waited on"); - } + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_client, + this.WaitSet.CurrentClients + ); } /// - /// Try to convert a key to a native index. + /// Services which are ready. /// - /// Key to convert - /// Native index - /// Whether the conversion was successful - private bool TryConvertKey(int key, out UIntPtr nativeIndex) + public IEnumerable ReadyServices { - try - { - nativeIndex = new UIntPtr(Convert.ToUInt32(key)); - } - catch (OverflowException) - { - nativeIndex = UIntPtr.Zero; - return false; - } - return true; + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_service, + this.WaitSet.CurrentServices + ); } /// - /// Get the pointer stored at an index. + /// Guard conditions which are ready. /// - /// Index of the pointer - /// Pointer stored at index - /// Whether the index existed - private bool TryGetPtr(UIntPtr index, out IntPtr ptr) + public IEnumerable ReadyGuardConditions { - return this.Getter(this.WaitSet.Handle, index, out ptr); + get => this.CurrentPrimitives( + NativeRclInterface.rclcs_wait_set_get_guard_condition, + this.WaitSet.CurrentGuardConditions + ); } /// - /// Set the pointer stored at an index. + /// Wait set associated with this result. /// - /// Index of the pointer - /// Pointer to be stored at index - /// Whether the index existed - private bool TrySetPtr(UIntPtr index, IntPtr ptr) - { - return this.Setter(this.WaitSet.Handle, index, ptr); - } - - /// - public void Add(int key, T value) - { - throw new NotSupportedException("adding new elements can only be done on WaitSet"); - } - - /// - public void Add(KeyValuePair item) - { - this.Add(item.Key, item.Value); - } - - /// - public bool ContainsKey(int key) - { - this.AssertOk(); - return this.TryConvertKey(key, out UIntPtr index) && - this.TryGetPtr(index, out IntPtr ptr) && - ptr != IntPtr.Zero; - } - - /// - public bool Contains(KeyValuePair item) - { - return this.TryGetValue(item.Key, out T value) && - EqualityComparer.Default.Equals(item.Value, value); - } + private readonly WaitSet WaitSet; - /// - public bool Remove(int key) - { - this.AssertOk(); - if (this.TryConvertKey(key, out UIntPtr index) && - this.TryGetPtr(index, out IntPtr ptr) && - ptr != IntPtr.Zero) - { - return this.TrySetPtr(index, IntPtr.Zero); - } - return false; - } + /// + /// Version when this result was created. + /// + private readonly uint CreatedVersion; - /// - public bool Remove(KeyValuePair item) + internal WaitResult(WaitSet waitSet) { - return this.Contains(item) && this.Remove(item.Key); + this.WaitSet = waitSet; + this.CreatedVersion = waitSet.Version; } - /// - public void Clear() + /// + /// Assert that the wait set is valid and has not been waited on. + /// + /// If the wait set was disposed. + private void AssertOk() { - foreach (int key in this.Keys) + if (this.WaitSet.Version != this.CreatedVersion || this.WaitSet.IsDisposed) { - this.TrySetPtr(new UIntPtr(Convert.ToUInt32(key)), IntPtr.Zero); + throw new ObjectDisposedException("rcl wait set"); } } - /// - public bool TryGetValue(int key, out T value) + /// + /// 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(); - if (this.TryConvertKey(key, out UIntPtr index) && this.TryGetPtr(index, out IntPtr ptr) && ptr != IntPtr.Zero) + for (UIntPtr index = UIntPtr.Zero; getter(this.WaitSet.Handle, index, out IntPtr ptr); index += 1) { - value = this.Wrappers[key]; - return true; - } - value = default(T); - return false; - } - - /// - public void CopyTo(KeyValuePair[] array, int arrayIndex) - { - if (arrayIndex < 0) - { - throw new ArgumentOutOfRangeException("arrayIndex is less than 0"); - } - foreach (var item in this) - { - try + if (ptr != IntPtr.Zero) { - array[arrayIndex] = item; + yield return primitives[Convert.ToInt32(index.ToUInt32())]; + this.AssertOk(); } - catch (IndexOutOfRangeException e) - { - throw new ArgumentException("array is too small", e); - } - arrayIndex += 1; } } /// - public IEnumerator> GetEnumerator() + public IEnumerator GetEnumerator() { - return this.Keys.Select(key => new KeyValuePair(key, this.Wrappers[key])).GetEnumerator(); + return this.ReadySubscriptions + .Concat(this.ReadyClients) + .Concat(this.ReadyServices) + .Concat(this.ReadyGuardConditions) + .GetEnumerator(); } /// @@ -584,276 +459,19 @@ IEnumerator IEnumerable.GetEnumerator() } /// - /// Collection representing the indexes being ready. - /// - private sealed class KeysCollection : ICollection - { - /// - public int Count - { - get - { - // cant use Enumerable.Count() since it - // just returns .Count since we are a collection - IEnumerator e = this.GetEnumerator(); - int count = 0; - while (e.MoveNext()) - { - checked - { - count += 1; - } - } - return count; - } - } - - /// - public bool IsReadOnly - { - get { return this.ReadyDictionary.IsReadOnly; } - } - - private readonly ReadyDictionary ReadyDictionary; - - internal KeysCollection(ReadyDictionary readyDictionary) - { - this.ReadyDictionary = readyDictionary; - } - - /// - private void AssertOk() - { - this.ReadyDictionary.AssertOk(); - } - - /// - public void Add(int key) - { - throw new NotSupportedException("adding new elements can only be done on WaitSet"); - } - - /// - public void Clear() - { - this.ReadyDictionary.Clear(); - } - - /// - public bool Contains(int key) - { - return this.ReadyDictionary.ContainsKey(key); - } - - /// - public void CopyTo(int[] array, int arrayIndex) - { - if (arrayIndex < 0) - { - throw new ArgumentOutOfRangeException("arrayIndex is less than 0"); - } - foreach (int key in this) - { - try - { - array[arrayIndex] = key; - } - catch (IndexOutOfRangeException e) - { - throw new ArgumentException("array too small", e); - } - arrayIndex += 1; - } - } - - /// - public bool Remove(int key) - { - return this.ReadyDictionary.Remove(key); - } - - /// - public IEnumerator GetEnumerator() - { - this.AssertOk(); - for (int key = 0; this.ReadyDictionary.TryConvertKey(key, out UIntPtr index) && this.ReadyDictionary.TryGetPtr(index, out IntPtr ptr); key += 1) - { - if (ptr != IntPtr.Zero) - { - yield return key; - // can be invalidated before being resumed - this.AssertOk(); - } - } - } - - /// - IEnumerator IEnumerable.GetEnumerator() - { - return this.GetEnumerator(); - } - } - - /// - /// Collection representing the wrappers which are ready. + /// Deconstruct the result into the resources which are ready. /// - private sealed class ValuesCollection : ICollection + public void Deconstruct( + out IEnumerable subscriptions, + out IEnumerable clients, + out IEnumerable services, + out IEnumerable guard_conditions) { - /// - public int Count - { - get { return this.ReadyDictionary.Count; } - } - - /// - public bool IsReadOnly - { - get { return this.ReadyDictionary.IsReadOnly; } - } - - private readonly ReadyDictionary ReadyDictionary; - - internal ValuesCollection(ReadyDictionary readDictionary) - { - this.ReadyDictionary = readDictionary; - } - - /// - public void Add(T value) - { - throw new NotSupportedException("adding new elements can only be done on WaitSet"); - } - - /// - public void Clear() - { - this.ReadyDictionary.Clear(); - } - - /// - public bool Contains(T value) - { - return this.Any(v => EqualityComparer.Default.Equals(v, value)); - } - - /// - public void CopyTo(T[] array, int arrayIndex) - { - if (arrayIndex < 0) - { - throw new ArgumentOutOfRangeException("arrayIndex is less than 0"); - } - foreach (T value in this) - { - try - { - array[arrayIndex] = value; - } - catch (IndexOutOfRangeException e) - { - throw new ArgumentException("array too small", e); - } - arrayIndex += 1; - } - } - - /// - public bool Remove(T value) - { - foreach (var item in this.ReadyDictionary) - { - if (EqualityComparer.Default.Equals(item.Value, value)) - { - return this.ReadyDictionary.Remove(item.Key); - } - } - return false; - } - - /// - public IEnumerator GetEnumerator() - { - return this.ReadyDictionary.Select(item => item.Value).GetEnumerator(); - } - - /// - IEnumerator IEnumerable.GetEnumerator() - { - return this.GetEnumerator(); - } + subscriptions = this.ReadySubscriptions; + clients = this.ReadyClients; + services = this.ReadyServices; + guard_conditions = this.ReadyGuardConditions; } } } - - /// - /// Result of waiting on a wait set. - /// - /// - /// The dictionaries are invalidated when waiting on the wait set again. - /// - internal sealed class WaitResult : IEnumerable - { - /// - /// Mapping from index to subscriptions which are ready. - /// - public IDictionary ReadySubscriptions { get; private set; } - - /// - /// Mapping from index to clients which are ready. - /// - public IDictionary ReadyClients { get; private set; } - - /// - /// Mapping from index to services which are ready. - /// - public IDictionary ReadyServices { get; private set; } - - /// - /// Mapping from index to guard conditions which are ready. - /// - public IDictionary ReadyGuardConditions { get; private set; } - - internal WaitResult( - IDictionary subscriptions, - IDictionary clients, - IDictionary services, - IDictionary guard_conditions) - { - this.ReadySubscriptions = subscriptions; - this.ReadyClients = clients; - this.ReadyServices = services; - this.ReadyGuardConditions = guard_conditions; - } - - /// - public IEnumerator GetEnumerator() - { - return this.ReadySubscriptions.Values - .Concat(this.ReadyClients.Values) - .Concat(this.ReadyServices.Values) - .Concat(this.ReadyGuardConditions.Values) - .GetEnumerator(); - } - - /// - IEnumerator IEnumerable.GetEnumerator() - { - return this.GetEnumerator(); - } - - /// - /// Deconstruct the result into the resources which are ready. - /// - public void Deconstruct( - out IDictionary subscriptions, - out IDictionary clients, - out IDictionary services, - out IDictionary guard_conditions) - { - subscriptions = this.ReadySubscriptions; - clients = this.ReadyClients; - services = this.ReadyServices; - guard_conditions = this.ReadyGuardConditions; - } - } } diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index eabc7df7..8f0be695 100644 --- a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs +++ b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs @@ -355,10 +355,6 @@ internal static WaitSetIsValidType [return: MarshalAs(UnmanagedType.U1)] internal delegate bool WaitSetGetType(IntPtr waitSet, UIntPtr index, out IntPtr value); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - [return: MarshalAs(UnmanagedType.U1)] - internal delegate bool WaitSetSetType(IntPtr waitSet, UIntPtr index, IntPtr value); - internal static WaitSetGetType rclcs_wait_set_get_subscription = (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -366,13 +362,6 @@ internal static WaitSetGetType "rclcs_wait_set_get_subscription"), typeof(WaitSetGetType)); - internal static WaitSetSetType - rclcs_wait_set_set_subscription = - (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeROS2CS, - "rclcs_wait_set_set_subscription"), - typeof(WaitSetSetType)); - internal static WaitSetGetType rclcs_wait_set_get_client = (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -380,13 +369,6 @@ internal static WaitSetGetType "rclcs_wait_set_get_client"), typeof(WaitSetGetType)); - internal static WaitSetSetType - rclcs_wait_set_set_client = - (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeROS2CS, - "rclcs_wait_set_set_client"), - typeof(WaitSetSetType)); - internal static WaitSetGetType rclcs_wait_set_get_service = (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -394,13 +376,6 @@ internal static WaitSetGetType "rclcs_wait_set_get_service"), typeof(WaitSetGetType)); - internal static WaitSetSetType - rclcs_wait_set_set_service = - (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeROS2CS, - "rclcs_wait_set_set_service"), - typeof(WaitSetSetType)); - internal static WaitSetGetType rclcs_wait_set_get_guard_condition = (WaitSetGetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( @@ -408,13 +383,6 @@ internal static WaitSetGetType "rclcs_wait_set_get_guard_condition"), typeof(WaitSetGetType)); - internal static WaitSetSetType - rclcs_wait_set_set_guard_condition = - (WaitSetSetType)Marshal.GetDelegateForFunctionPointer(dllLoadUtils.GetProcAddress( - nativeROS2CS, - "rclcs_wait_set_set_guard_condition"), - typeof(WaitSetSetType)); - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr ServiceCreateOptionsType(IntPtr qos); internal static ServiceCreateOptionsType diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index b1baa98e..05948e38 100644 --- a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c @@ -340,17 +340,6 @@ uint8_t rclcs_wait_set_get_subscription(rcl_wait_set_t * wait_set, size_t index, return 0; } -ROSIDL_GENERATOR_C_EXPORT -uint8_t rclcs_wait_set_set_subscription(rcl_wait_set_t * wait_set, size_t index, const rcl_subscription_t * subscription) -{ - if (index < wait_set->size_of_subscriptions) - { - wait_set->subscriptions[index] = subscription; - 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) { @@ -362,17 +351,6 @@ uint8_t rclcs_wait_set_get_client(rcl_wait_set_t * wait_set, size_t index, const return 0; } -ROSIDL_GENERATOR_C_EXPORT -uint8_t rclcs_wait_set_set_client(rcl_wait_set_t * wait_set, size_t index, const rcl_client_t * client) -{ - if (index < wait_set->size_of_clients) - { - wait_set->clients[index] = client; - 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) { @@ -384,17 +362,6 @@ uint8_t rclcs_wait_set_get_service(rcl_wait_set_t * wait_set, size_t index, cons return 0; } -ROSIDL_GENERATOR_C_EXPORT -uint8_t rclcs_wait_set_set_service(rcl_wait_set_t * wait_set, size_t index, const rcl_service_t * service) -{ - if (index < wait_set->size_of_services) - { - wait_set->services[index] = service; - 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) { @@ -406,17 +373,6 @@ uint8_t rclcs_wait_set_get_guard_condition(rcl_wait_set_t * wait_set, size_t ind return 0; } -ROSIDL_GENERATOR_C_EXPORT -uint8_t rclcs_wait_set_set_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) - { - wait_set->guard_conditions[index] = guard_condition; - return 1; - } - return 0; -} - ROSIDL_GENERATOR_C_EXPORT char * rclcs_get_error_string() { diff --git a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs index daefedae..2963c53c 100644 --- a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs +++ b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs @@ -71,7 +71,7 @@ public void TriggerGuardCondition() ); Assert.That(waitSet.TryWait(TimeSpan.FromSeconds(0.5), out var result), Is.True); - Assert.That(result.ReadyGuardConditions.Values.Contains(this.GuardCondition), Is.True); + Assert.That(result.ReadyGuardConditions, Does.Contain(this.GuardCondition)); } [Test] diff --git a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs index e600cdd9..e8796d34 100644 --- a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs +++ b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs @@ -158,6 +158,7 @@ public void TestTryWait() 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); @@ -191,7 +192,7 @@ public void TestTryWaitEmpty() } [Test] - public void TestReadyDictionary() + public void TestResult() { this.Context.TryCreateNode("TestNode", out var node); @@ -200,21 +201,11 @@ public void TestReadyDictionary() SUBSCRIPTION_TOPIC, msg => { } ); - using var subscriptionDummy = node.CreateSubscription( - SUBSCRIPTION_TOPIC, - msg => { throw new InvalidOperationException($"callback was triggered with {msg}"); } - ); using var client = node.CreateClient(SERVICE_TOPIC); - using var clientDummy = node.CreateClient(SERVICE_TOPIC); using var service = node.CreateService( SERVICE_TOPIC, request => new AddTwoInts_Response() ); - using var serviceDummy = node.CreateService( - SERVICE_TOPIC, - request => { throw new InvalidOperationException($"received request ${request}"); } - ); - this.WaitSet.Subscriptions.Add(subscription); this.WaitSet.Clients.Add(client); this.WaitSet.Services.Add(service); @@ -223,82 +214,55 @@ public void TestReadyDictionary() client.CallAsync(new AddTwoInts_Request()); Assert.That(this.WaitSet.TryWait(TimeSpan.FromSeconds(0.1), out var result1), Is.True); - this.TestReadyDictionary(result1.ReadySubscriptions, subscription, subscriptionDummy); - this.TestReadyDictionary(result1.ReadyServices, service, serviceDummy); - Assert.That(result1.ReadyClients.Count, Is.Zero); + 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.Count; }); - Assert.Catch(() => { _ = result1.ReadyClients.Count; }); - Assert.Catch(() => { _ = result1.ReadyServices.Count; }); - Assert.That(result2.ReadySubscriptions.Count, Is.Zero); - Assert.That(result2.ReadyServices.Count, Is.Zero); - this.TestReadyDictionary(result2.ReadyClients, client, clientDummy); + 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.Count; }); - Assert.Catch(() => { _ = result2.ReadyClients.Count; }); - Assert.Catch(() => { _ = result2.ReadyServices.Count; }); - } - - private void TestReadyDictionary(IDictionary dictionary, T waitable, T dummy) where T : IWaitable - { - Assert.That(dictionary.Count, Is.EqualTo(1)); - Assert.That(dictionary.Count(), Is.EqualTo(1)); - - Assert.Throws(() => { dictionary[1] = waitable; }); - Assert.Throws(() => { dictionary.Add(1, waitable); }); - - var pair = dictionary.First(); - Assert.That(pair.Value, Is.EqualTo(waitable)); - - Assert.That(dictionary.Contains(new KeyValuePair(pair.Key, waitable)), Is.True); - Assert.That(dictionary.Contains(new KeyValuePair(pair.Key + 1, waitable)), Is.False); - Assert.That(dictionary.Contains(new KeyValuePair(pair.Key, dummy)), Is.False); - - Assert.That(dictionary.ContainsKey(pair.Key), Is.True); - Assert.That(dictionary.ContainsKey(pair.Key + 1), Is.False); - - Assert.That(dictionary.TryGetValue(pair.Key, out var result), Is.True); - Assert.That(result, Is.EqualTo(waitable)); - Assert.That(dictionary.TryGetValue(pair.Key + 1, out _), Is.False); - - this.TestReadyDictionaryKeys(dictionary.Keys, pair.Key); - this.TestReadyDictionaryValues(dictionary.Values, waitable, dummy); - - Assert.That(dictionary.Remove(pair.Key + 1), Is.False); - Assert.That(dictionary.Remove(pair), Is.True); - Assert.That(dictionary.Remove(pair), Is.False); - Assert.That(dictionary.Count, Is.EqualTo(0)); + Assert.Catch(() => { _ = result2.ReadySubscriptions.GetEnumerator().MoveNext(); }); + Assert.Catch(() => { _ = result2.ReadyClients.GetEnumerator().MoveNext(); }); + Assert.Catch(() => { _ = result2.ReadyServices.GetEnumerator().MoveNext(); }); } - private void TestReadyDictionaryKeys(ICollection keys, int key) - { - Assert.That(keys.Count, Is.EqualTo(1)); - Assert.That(keys.Count(), Is.EqualTo(1)); - - Assert.Throws(() => { keys.Add(key + 1); }); - - Assert.That(keys.Contains(key), Is.True); - Assert.That(keys.Contains(key + 1), Is.False); - - Assert.That(keys.First(), Is.EqualTo(key)); - } - - private void TestReadyDictionaryValues(ICollection values, T value, T dummy) where T : IWaitable + [Test] + public void TestResultMultiple() { - Assert.That(values.Count, Is.EqualTo(1)); - Assert.That(values.Count(), Is.EqualTo(1)); - - Assert.Throws(() => { values.Add(value); }); + 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); - Assert.That(values.Contains(value), Is.True); - Assert.That(values.Contains(dummy), Is.False); + publisher1.Publish(new std_msgs.msg.Int32()); + publisher3.Publish(new std_msgs.msg.Int32()); - Assert.That(values.First(), Is.EqualTo(value)); + 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 From c89ea268161cb46b8ec631dec7e8b6d939d4e04e Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 28 Apr 2023 14:08:00 +0200 Subject: [PATCH 38/46] add `ManualExecutor.SpinWhile` This should be more user friendly than `ManualExecutor.Spin`. --- .../ros2cs_core/executors/ManualExecutor.cs | 28 ++++++++++--------- src/ros2cs/ros2cs_examples/ROS2Client.cs | 10 ++----- src/ros2cs/ros2cs_examples/ROS2Listener.cs | 3 +- .../ROS2PerformanceListener.cs | 8 +----- src/ros2cs/ros2cs_examples/ROS2Service.cs | 3 +- 5 files changed, 20 insertions(+), 32 deletions(-) diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index 76a7ffaf..313cb27c 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -394,25 +394,27 @@ public void Rescan() } /// - /// Utility which handles automatic rescaning. + /// 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. - /// - /// trying to spin in each iteration - /// and yielding if a rescan had to be performed instead. - /// - public IEnumerator Spin(TimeSpan timeout) + public void SpinWhile(Func condition, TimeSpan timeout) { - while (true) + while (condition()) { - if (this.TrySpin(timeout)) - { - yield return false; - } - else + if (!this.TrySpin(timeout)) { this.Rescan(); - yield return true; } } } diff --git a/src/ros2cs/ros2cs_examples/ROS2Client.cs b/src/ros2cs/ros2cs_examples/ROS2Client.cs index ebc628f6..aee549cc 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Client.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Client.cs @@ -47,14 +47,8 @@ public static void Main(string[] args) } Task rsp = my_client.CallAsync(msg); - for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) - { - if (rsp.IsCompleted) - { - break; - } - } - + 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 8d976a8a..9fe1b017 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Listener.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Listener.cs @@ -36,8 +36,7 @@ public static void Main(string[] args) msg => Console.WriteLine($"I heard: [{msg.Data}]") ); - for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) - {} + executor.SpinWhile(() => true); } } } diff --git a/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs b/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs index cfb43176..ca4c7f8c 100644 --- a/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs +++ b/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs @@ -125,13 +125,7 @@ public static void Main(string[] args) new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA) ); - for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) - { - if (exitScheduled) - { - break; - } - } + executor.SpinWhile(() => !exitScheduled); } } } diff --git a/src/ros2cs/ros2cs_examples/ROS2Service.cs b/src/ros2cs/ros2cs_examples/ROS2Service.cs index 892b2b37..2a8c77c0 100644 --- a/src/ros2cs/ros2cs_examples/ROS2Service.cs +++ b/src/ros2cs/ros2cs_examples/ROS2Service.cs @@ -45,8 +45,7 @@ public static void Main(string[] args) } ); - for (IEnumerator spin = executor.Spin(TimeSpan.FromSeconds(0.1)); spin.MoveNext();) - {} + executor.SpinWhile(() => true); } } } From f4ce8aee04993b9d45a011b9edc4c20a954dff4a Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 17 May 2023 15:22:16 +0200 Subject: [PATCH 39/46] fix `IClient` summary mentioning subscriptions --- src/ros2cs/ros2cs_core/interfaces/IClient.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2cs/ros2cs_core/interfaces/IClient.cs b/src/ros2cs/ros2cs_core/interfaces/IClient.cs index 56a6a9cb..3f275b9e 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IClient.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IClient.cs @@ -56,7 +56,7 @@ internal interface IRawClient : IClientBase void DisposeFromNode(); } - /// Generic base interface for all subscriptions + /// Generic base interface for all clients. /// Message Type to be send. /// Message Type to be received. public interface IClient : IClientBase From c248106ca961592c439b482f50e262d871b01fb6 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 17 May 2023 15:54:07 +0200 Subject: [PATCH 40/46] add copyright notices to new files Files created by this PR are under the copyright of ADVITEC Informatik GmbH as suggested by review. The license is the same as existing files. --- src/ros2cs/ros2cs_core/Context.cs | 2 +- src/ros2cs/ros2cs_core/GuardCondition.cs | 2 +- src/ros2cs/ros2cs_core/executors/ManualExecutor.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_core/interfaces/IContext.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/IExecutor.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/IWaitable.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_core/utils/LockedCollection.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_core/utils/LockedDictionary.cs | 14 ++++++++++++++ .../ros2cs_core/utils/MappedValueDictionary.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_tests/src/ContextTest.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_tests/src/PublisherTest.cs | 14 ++++++++++++++ src/ros2cs/ros2cs_tests/src/WaitSetTest.cs | 14 ++++++++++++++ 14 files changed, 144 insertions(+), 4 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs index 013c6f96..a9f6aaf9 100644 --- a/src/ros2cs/ros2cs_core/Context.cs +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -1,4 +1,4 @@ -// 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. diff --git a/src/ros2cs/ros2cs_core/GuardCondition.cs b/src/ros2cs/ros2cs_core/GuardCondition.cs index 7956e11d..2041b153 100644 --- a/src/ros2cs/ros2cs_core/GuardCondition.cs +++ b/src/ros2cs/ros2cs_core/GuardCondition.cs @@ -1,4 +1,4 @@ -// 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. diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index 313cb27c..93f414c8 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_core/interfaces/IContext.cs b/src/ros2cs/ros2cs_core/interfaces/IContext.cs index 69c73759..44fba76a 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IContext.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IContext.cs @@ -1,4 +1,4 @@ -// 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. diff --git a/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs index 3091bc70..5c658a36 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IExecutor.cs @@ -1,4 +1,4 @@ -// 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. diff --git a/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs index dea99a24..898feb86 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IWaitable.cs @@ -1,3 +1,17 @@ +// 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; namespace ROS2 diff --git a/src/ros2cs/ros2cs_core/utils/LockedCollection.cs b/src/ros2cs/ros2cs_core/utils/LockedCollection.cs index 8cf37ab5..3f91c268 100644 --- a/src/ros2cs/ros2cs_core/utils/LockedCollection.cs +++ b/src/ros2cs/ros2cs_core/utils/LockedCollection.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs b/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs index fe784369..f99dfebe 100644 --- a/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs +++ b/src/ros2cs/ros2cs_core/utils/LockedDictionary.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs b/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs index 2ddb14cd..d1c42349 100644 --- a/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs +++ b/src/ros2cs/ros2cs_core/utils/MappedValueDictionary.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_tests/src/ContextTest.cs b/src/ros2cs/ros2cs_tests/src/ContextTest.cs index 15e52f5c..99ce4f0e 100644 --- a/src/ros2cs/ros2cs_tests/src/ContextTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ContextTest.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs index 2963c53c..f5171bd3 100644 --- a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs +++ b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs index 61f9c2c5..6b69b670 100644 --- a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_tests/src/PublisherTest.cs b/src/ros2cs/ros2cs_tests/src/PublisherTest.cs index ed50c197..a84c4e1d 100644 --- a/src/ros2cs/ros2cs_tests/src/PublisherTest.cs +++ b/src/ros2cs/ros2cs_tests/src/PublisherTest.cs @@ -1,3 +1,17 @@ +// 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; diff --git a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs index e8796d34..2c023ae6 100644 --- a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs +++ b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs @@ -1,3 +1,17 @@ +// 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; From 38e053436c38acd79e77ca0024e0ed32097bd402 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 17 May 2023 16:05:53 +0200 Subject: [PATCH 41/46] update copyright year of modified files --- src/ros2cs/ros2cs_core/CMakeLists.txt | 2 +- src/ros2cs/ros2cs_core/Client.cs | 2 +- src/ros2cs/ros2cs_core/Node.cs | 2 +- src/ros2cs/ros2cs_core/Publisher.cs | 2 +- src/ros2cs/ros2cs_core/Service.cs | 2 +- src/ros2cs/ros2cs_core/Subscription.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/IClient.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/INode.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/IPublisher.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/IService.cs | 2 +- src/ros2cs/ros2cs_core/interfaces/ISubscription.cs | 2 +- src/ros2cs/ros2cs_core/native/NativeRcl.cs | 2 +- .../ros2cs_core/native/NativeRclInterface.cs | 2 +- src/ros2cs/ros2cs_core/native/NativeTypes.cs | 2 +- .../ros2cs_core/native/rcl_native_interface.c | 2 +- src/ros2cs/ros2cs_core/utils/Utils.cs | 2 +- src/ros2cs/ros2cs_examples/ROS2Client.cs | 2 +- src/ros2cs/ros2cs_examples/ROS2Listener.cs | 2 +- .../ros2cs_examples/ROS2PerformanceListener.cs | 2 +- .../ros2cs_examples/ROS2PerformanceTalker.cs | 2 +- src/ros2cs/ros2cs_examples/ROS2Service.cs | 2 +- src/ros2cs/ros2cs_examples/ROS2Talker.cs | 2 +- src/ros2cs/ros2cs_tests/CMakeLists.txt | 14 ++++++++++++++ src/ros2cs/ros2cs_tests/src/ClientTest.cs | 2 +- src/ros2cs/ros2cs_tests/src/ClockTest.cs | 2 +- src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs | 4 ++-- src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs | 4 ++-- src/ros2cs/ros2cs_tests/src/NodeTest.cs | 4 ++-- src/ros2cs/ros2cs_tests/src/ServiceTest.cs | 2 +- src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs | 4 ++-- 30 files changed, 47 insertions(+), 33 deletions(-) diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index 0289a250..9a9b902c 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. diff --git a/src/ros2cs/ros2cs_core/Client.cs b/src/ros2cs/ros2cs_core/Client.cs index d5e4a4f8..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. diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index a87a5f60..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. diff --git a/src/ros2cs/ros2cs_core/Publisher.cs b/src/ros2cs/ros2cs_core/Publisher.cs index 153c63ad..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. diff --git a/src/ros2cs/ros2cs_core/Service.cs b/src/ros2cs/ros2cs_core/Service.cs index d0b7449b..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. diff --git a/src/ros2cs/ros2cs_core/Subscription.cs b/src/ros2cs/ros2cs_core/Subscription.cs index 8ca0295a..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. diff --git a/src/ros2cs/ros2cs_core/interfaces/IClient.cs b/src/ros2cs/ros2cs_core/interfaces/IClient.cs index 3f275b9e..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. diff --git a/src/ros2cs/ros2cs_core/interfaces/INode.cs b/src/ros2cs/ros2cs_core/interfaces/INode.cs index 656623c3..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. diff --git a/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs b/src/ros2cs/ros2cs_core/interfaces/IPublisher.cs index aa3d46ae..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. diff --git a/src/ros2cs/ros2cs_core/interfaces/IService.cs b/src/ros2cs/ros2cs_core/interfaces/IService.cs index a8bfad20..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. diff --git a/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs b/src/ros2cs/ros2cs_core/interfaces/ISubscription.cs index 319329b3..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. diff --git a/src/ros2cs/ros2cs_core/native/NativeRcl.cs b/src/ros2cs/ros2cs_core/native/NativeRcl.cs index 78bf7854..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. diff --git a/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs b/src/ros2cs/ros2cs_core/native/NativeRclInterface.cs index 8f0be695..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. diff --git a/src/ros2cs/ros2cs_core/native/NativeTypes.cs b/src/ros2cs/ros2cs_core/native/NativeTypes.cs index 8aa3348a..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. diff --git a/src/ros2cs/ros2cs_core/native/rcl_native_interface.c b/src/ros2cs/ros2cs_core/native/rcl_native_interface.c index 05948e38..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. diff --git a/src/ros2cs/ros2cs_core/utils/Utils.cs b/src/ros2cs/ros2cs_core/utils/Utils.cs index 2c38b5d7..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. diff --git a/src/ros2cs/ros2cs_examples/ROS2Client.cs b/src/ros2cs/ros2cs_examples/ROS2Client.cs index aee549cc..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. diff --git a/src/ros2cs/ros2cs_examples/ROS2Listener.cs b/src/ros2cs/ros2cs_examples/ROS2Listener.cs index 9fe1b017..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. diff --git a/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs b/src/ros2cs/ros2cs_examples/ROS2PerformanceListener.cs index ca4c7f8c..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. diff --git a/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs b/src/ros2cs/ros2cs_examples/ROS2PerformanceTalker.cs index 9b3bce08..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. diff --git a/src/ros2cs/ros2cs_examples/ROS2Service.cs b/src/ros2cs/ros2cs_examples/ROS2Service.cs index 2a8c77c0..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. diff --git a/src/ros2cs/ros2cs_examples/ROS2Talker.cs b/src/ros2cs/ros2cs_examples/ROS2Talker.cs index a782a479..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. diff --git a/src/ros2cs/ros2cs_tests/CMakeLists.txt b/src/ros2cs/ros2cs_tests/CMakeLists.txt index cd03b3a2..f473325c 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) diff --git a/src/ros2cs/ros2cs_tests/src/ClientTest.cs b/src/ros2cs/ros2cs_tests/src/ClientTest.cs index dc7c8c9a..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. diff --git a/src/ros2cs/ros2cs_tests/src/ClockTest.cs b/src/ros2cs/ros2cs_tests/src/ClockTest.cs index d0e2f8a3..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"); diff --git a/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs b/src/ros2cs/ros2cs_tests/src/LargeMessageTest.cs index b31b311d..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. diff --git a/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs b/src/ros2cs/ros2cs_tests/src/NativeMetodsTest.cs index 92e344ee..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. diff --git a/src/ros2cs/ros2cs_tests/src/NodeTest.cs b/src/ros2cs/ros2cs_tests/src/NodeTest.cs index 7df4007a..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. diff --git a/src/ros2cs/ros2cs_tests/src/ServiceTest.cs b/src/ros2cs/ros2cs_tests/src/ServiceTest.cs index 835fc3bc..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. diff --git a/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs b/src/ros2cs/ros2cs_tests/src/SubscriptionTest.cs index 6c768155..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. From f062a989468e31fad05ca6c749cabc78eff511d4 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Wed, 17 May 2023 16:06:39 +0200 Subject: [PATCH 42/46] update copyright notice for significantly changed files --- src/ros2cs/ros2cs_core/WaitSet.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/ros2cs/ros2cs_core/WaitSet.cs b/src/ros2cs/ros2cs_core/WaitSet.cs index 11c5d78c..9c9eb44e 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. From 52a331dbf87233274dcd7609246c92118cff8026 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 7 Jul 2023 14:23:30 +0200 Subject: [PATCH 43/46] dispose guard conditions after invoking `IContext.OnShutdown` This prevents violating the documentation. --- src/ros2cs/ros2cs_core/Context.cs | 50 ++++++++++++++++++- src/ros2cs/ros2cs_core/GuardCondition.cs | 23 +++++++-- .../ros2cs_core/executors/ManualExecutor.cs | 2 +- .../ros2cs_tests/src/GuardConditionTest.cs | 13 +++-- 4 files changed, 78 insertions(+), 10 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs index a9f6aaf9..44adf865 100644 --- a/src/ros2cs/ros2cs_core/Context.cs +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -52,6 +52,14 @@ public sealed class Context : IContext /// 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(); + /// /// Get the current RMW implementation. /// @@ -128,6 +136,41 @@ internal bool RemoveNode(string 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); + } + } + /// /// This method is not thread safe. /// Do not call while the context or any entities @@ -154,7 +197,7 @@ private void Dispose(bool disposing) { Utils.CheckReturnEnum(ret); } - // only continue if ROSNodes has not been finalized + // only continue if ROSNodes and GuardConditions has not been finalized if (disposing) { this.OnShutdown?.Invoke(); @@ -163,6 +206,11 @@ private void Dispose(bool disposing) node.DisposeFromContext(); } this.ROSNodes.Clear(); + foreach (var guardCondition in this.GuardConditions) + { + guardCondition.DisposeFromContext(); + } + this.GuardConditions.Clear(); // only safe when all nodes are gone, not calling Dispose() will leak the Handle Utils.CheckReturnEnum(NativeRcl.rcl_context_fini(this.Handle)); this.FreeHandles(); diff --git a/src/ros2cs/ros2cs_core/GuardCondition.cs b/src/ros2cs/ros2cs_core/GuardCondition.cs index 2041b153..556b92f2 100644 --- a/src/ros2cs/ros2cs_core/GuardCondition.cs +++ b/src/ros2cs/ros2cs_core/GuardCondition.cs @@ -13,6 +13,7 @@ // limitations under the License. using System; +using System.Diagnostics; namespace ROS2 { @@ -68,7 +69,6 @@ out IntPtr handle } Utils.CheckReturnEnum(ret); this.Handle = handle; - context.OnShutdown += this.Dispose; } /// @@ -123,13 +123,26 @@ private void Dispose(bool disposing) return; } - Utils.CheckReturnEnum(NativeRcl.rcl_guard_condition_fini(this.Handle)); - this.FreeHandles(); - + // only do if Context.GuardConditions has not been finalized if (disposing) { - this.Context.OnShutdown -= this.Dispose; + 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(); } /// diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index 93f414c8..d64b43a8 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -115,7 +115,7 @@ public bool IsReadOnly /// If is disposed. public ManualExecutor(Context context) : this( new WaitSet(context), - new GuardCondition(context, () => { }) + context.CreateGuardCondition(() => { }) ) { } diff --git a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs index f5171bd3..f37b0f3d 100644 --- a/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs +++ b/src/ros2cs/ros2cs_tests/src/GuardConditionTest.cs @@ -29,8 +29,7 @@ public class GuardConditionTest public void SetUp() { this.Context = new Context(); - this.GuardCondition = new GuardCondition( - this.Context, + this.GuardCondition = this.Context.CreateGuardCondition( () => { throw new InvalidOperationException("guard condition was called"); } ); } @@ -61,12 +60,20 @@ public void DoubleDisposeGuardCondition() 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(() => new GuardCondition(this.Context, () => {})); + Assert.Throws(() => this.Context.CreateGuardCondition(() => { })); } [Test] From 35306113c2ea0d4eea275fbe59fa1071e1bbacda Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 7 Jul 2023 16:27:14 +0200 Subject: [PATCH 44/46] dispose wait set after invoking `IContext.OnShutdown` This prevents violating the documentation. --- src/ros2cs/ros2cs_core/Context.cs | 51 ++++++++++++++++++- src/ros2cs/ros2cs_core/WaitSet.cs | 42 ++++++++++++--- .../ros2cs_core/executors/ManualExecutor.cs | 2 +- src/ros2cs/ros2cs_tests/src/WaitSetTest.cs | 10 +++- 4 files changed, 93 insertions(+), 12 deletions(-) diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs index 44adf865..07ba9a4f 100644 --- a/src/ros2cs/ros2cs_core/Context.cs +++ b/src/ros2cs/ros2cs_core/Context.cs @@ -60,6 +60,14 @@ public sealed class Context : IContext /// 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. /// @@ -171,6 +179,40 @@ internal bool RemoveGuardCondition(GuardCondition 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 @@ -197,7 +239,7 @@ private void Dispose(bool disposing) { Utils.CheckReturnEnum(ret); } - // only continue if ROSNodes and GuardConditions has not been finalized + // only continue if the collections of the active primitives have not been finalized if (disposing) { this.OnShutdown?.Invoke(); @@ -211,7 +253,12 @@ private void Dispose(bool disposing) guardCondition.DisposeFromContext(); } this.GuardConditions.Clear(); - // only safe when all nodes are gone, not calling Dispose() will leak the Handle + 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(); } diff --git a/src/ros2cs/ros2cs_core/WaitSet.cs b/src/ros2cs/ros2cs_core/WaitSet.cs index 9c9eb44e..3bfa9c44 100644 --- a/src/ros2cs/ros2cs_core/WaitSet.cs +++ b/src/ros2cs/ros2cs_core/WaitSet.cs @@ -16,6 +16,7 @@ using System; using System.Collections; using System.Collections.Generic; +using System.Diagnostics; using System.Linq; namespace ROS2 @@ -49,7 +50,12 @@ internal sealed class WaitSet : IReadOnlyCollection, IExtendedDisposa /// /// Context associated with this wait set. /// - public IContext Context { get; private set; } + public IContext Context + { + get => this.ROSContext; + } + + private Context ROSContext; /// public bool IsDisposed @@ -99,7 +105,7 @@ public int Count /// Associated context internal WaitSet(Context context) { - this.Context = context; + this.ROSContext = context; this.Handle = NativeRclInterface.rclcs_get_zero_initialized_wait_set(); int ret = NativeRcl.rcl_wait_set_init( this.Handle, @@ -117,7 +123,6 @@ internal WaitSet(Context context) this.FreeHandles(); Utils.CheckReturnEnum(ret); } - context.OnShutdown += this.Dispose; } /// @@ -314,16 +319,37 @@ private void Dispose(bool disposing) 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(); + } - if (disposing) + /// Dispose without modifying the context. + internal void DisposeFromContext() + { + if (this.Handle == IntPtr.Zero) { - this.Context.OnShutdown -= this.Dispose; - this.Subscriptions.Clear(); - this.Clients.Clear(); - this.Services.Clear(); + 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() diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index d64b43a8..3fc42d35 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -114,7 +114,7 @@ public bool IsReadOnly /// Context to associate with. /// If is disposed. public ManualExecutor(Context context) : this( - new WaitSet(context), + context.CreateWaitSet(), context.CreateGuardCondition(() => { }) ) { } diff --git a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs index 2c023ae6..a6fb0b25 100644 --- a/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs +++ b/src/ros2cs/ros2cs_tests/src/WaitSetTest.cs @@ -36,7 +36,7 @@ public class WaitSetTest public void SetUp() { this.Context = new Context(); - this.WaitSet = new WaitSet(this.Context); + this.WaitSet = this.Context.CreateWaitSet(); } [TearDown] @@ -65,6 +65,14 @@ public void DoubleDisposeWaitSet() 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() { From 2e5bdd94e975655d5fce6d0fba95b4c2a46b1862 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Fri, 14 Jul 2023 12:27:39 +0200 Subject: [PATCH 45/46] Add executor spinning in the background To prevent users from having to implement the feature themselves and provide an alternative to the old spinning behaviour. The new executor uses a long running `Task` instead of a `Thread` to provide better integration with the C# ecosystem. --- src/ros2cs/ros2cs_core/CMakeLists.txt | 1 + .../ros2cs_core/executors/ManualExecutor.cs | 22 ++ .../ros2cs_core/executors/TaskExecutor.cs | 191 ++++++++++++++++++ src/ros2cs/ros2cs_tests/CMakeLists.txt | 1 + .../ros2cs_tests/src/ManualExecutorTest.cs | 89 +++++++- .../ros2cs_tests/src/TaskExecutorTest.cs | 120 +++++++++++ 6 files changed, 423 insertions(+), 1 deletion(-) create mode 100644 src/ros2cs/ros2cs_core/executors/TaskExecutor.cs create mode 100644 src/ros2cs/ros2cs_tests/src/TaskExecutorTest.cs diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index 9a9b902c..f08ab886 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -114,6 +114,7 @@ set(CS_SOURCES Context.cs GuardCondition.cs executors/ManualExecutor.cs + executors/TaskExecutor.cs properties/AssemblyInfo.cs ) diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index 3fc42d35..c7493b1b 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -18,6 +18,7 @@ using System.Diagnostics; using System.Linq; using System.Threading; +using System.Threading.Tasks; namespace ROS2.Executors { @@ -433,6 +434,27 @@ public void SpinWhile(Func condition, TimeSpan timeout) } } + /// + /// 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. 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_tests/CMakeLists.txt b/src/ros2cs/ros2cs_tests/CMakeLists.txt index f473325c..9b84b2a0 100644 --- a/src/ros2cs/ros2cs_tests/CMakeLists.txt +++ b/src/ros2cs/ros2cs_tests/CMakeLists.txt @@ -56,6 +56,7 @@ if(BUILD_TESTING) 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/ManualExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs index 6b69b670..0fa968b3 100644 --- a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs @@ -16,6 +16,7 @@ using System.Collections.Generic; using System.Diagnostics; using System.Threading; +using System.Threading.Tasks; using NUnit.Framework; using ROS2.Executors; @@ -319,6 +320,92 @@ public void Clear() 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 @@ -343,7 +430,7 @@ public bool TryWait(TimeSpan timeout) } public void Dispose() - {} + { } } } } \ No newline at end of file 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 From 77bfffd15c5460e191719af82fe986a49dc4c527 Mon Sep 17 00:00:00 2001 From: Eric Wolf Date: Thu, 31 Aug 2023 14:46:04 +0200 Subject: [PATCH 46/46] fix thread starvation in `ManualExecutor.*Wait` Threads may fail to wake for some time since the method waits on a `ManualResetEventSlim` which is reset before the next spin. To prevent such cases the waiting condition has been modified to additionally check an integer which is incremented after every spin to detected if the spin which was waited upon has ended. --- .../ros2cs_core/executors/ManualExecutor.cs | 132 +++++++++++++++--- .../ros2cs_tests/src/ManualExecutorTest.cs | 3 + 2 files changed, 119 insertions(+), 16 deletions(-) diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs index c7493b1b..3889f75b 100644 --- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs +++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs @@ -44,21 +44,29 @@ public IContext Context /// public bool IsSpinning { - get { return !this.IsIdle.IsSet; } + get => this._IsSpinning; + private set => this._IsSpinning = value; } + private volatile bool _IsSpinning = false; + /// /// Whether a rescan is scheduled. /// public bool RescanScheduled { - get { return this._RescanScheduled; } - private set { this._RescanScheduled = value; } + 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 { @@ -92,6 +100,9 @@ public bool IsReadOnly /// /// Wait set used while spinning. /// + /// + /// Is also used to notify when the executor finished spinning. + /// private readonly WaitSet WaitSet; /// @@ -104,11 +115,6 @@ public bool IsReadOnly /// private readonly HashSet Nodes = new HashSet(); - /// - /// Event signaling whether the executor is not spinning. - /// - private readonly ManualResetEventSlim IsIdle = new ManualResetEventSlim(true); - /// /// Create a new instance. /// @@ -286,21 +292,92 @@ public bool TryScheduleRescan(INode node) /// public void Wait() { - bool success = this.TryWait(TimeSpan.FromMilliseconds(-1)); - Debug.Assert(success, "infinite wait timed out"); + 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 (this.RescanScheduled && this.IsSpinning) + 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) @@ -309,7 +386,22 @@ public bool TryWait(TimeSpan timeout) // guard condition might be disposed but // nodes still have to be removed } - return this.IsIdle.Wait(timeout); + 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; } @@ -338,10 +430,10 @@ public void Interrupt() /// Whether work could be processed since no rescan was scheduled. public bool TrySpin(TimeSpan timeout) { - this.IsIdle.Reset(); + this.IsSpinning = true; try { - // check after resetting IsIdle to + // check after setting IsSpinning to // prevent race condition if (this.RescanScheduled) { @@ -357,7 +449,16 @@ public bool TrySpin(TimeSpan timeout) } finally { - this.IsIdle.Set(); + // 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; } @@ -477,7 +578,6 @@ public void Dispose() } this.WaitSet.Dispose(); this.InterruptCondition.Dispose(); - this.IsIdle.Dispose(); } } } \ No newline at end of file diff --git a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs index 0fa968b3..72a828bc 100644 --- a/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs +++ b/src/ros2cs/ros2cs_tests/src/ManualExecutorTest.cs @@ -58,6 +58,7 @@ public void DisposedExecutorHandling() 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); @@ -125,6 +126,7 @@ 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); } @@ -132,6 +134,7 @@ public void TryWaitScheduled() public void TryWaitUnscheduled() { Assert.That(this.Executor.TryWait(TimeSpan.Zero), Is.True); + this.Executor.Wait(); Assert.That(this.Executor.RescanScheduled, Is.False); }