From d004d9e41df0edacd03d27b6d0b70fb371210713 Mon Sep 17 00:00:00 2001
From: Eric Wolf <ewolf@advitec.de>
Date: Fri, 7 Jul 2023 14:23:30 +0200
Subject: [PATCH] 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    |  5 +-
 4 files changed, 70 insertions(+), 10 deletions(-)

diff --git a/src/ros2cs/ros2cs_core/Context.cs b/src/ros2cs/ros2cs_core/Context.cs
index a9f6aaf..44adf86 100644
--- a/src/ros2cs/ros2cs_core/Context.cs
+++ b/src/ros2cs/ros2cs_core/Context.cs
@@ -52,6 +52,14 @@ public sealed class Context : IContext
         /// </remarks>
         private Dictionary<string, Node> ROSNodes = new Dictionary<string, Node>();
 
+        /// <summary>
+        /// Collection of guard conditions active in this context.
+        /// </summary>
+        /// <remarks>
+        /// Also used for synchronisation when creating / removing guard conditions.
+        /// </remarks>
+        private HashSet<GuardCondition> GuardConditions = new HashSet<GuardCondition>();
+
         /// <summary>
         /// Get the current RMW implementation.
         /// </summary>
@@ -128,6 +136,41 @@ internal bool RemoveNode(string name)
             }
         }
 
+        /// <summary>
+        /// Create a guard condition.
+        /// </summary>
+        /// <remarks>
+        /// This method is thread safe.
+        /// </remarks>
+        /// <param name="callback"> Callback executed by the executor when the guard condition is triggered. </param>
+        /// <returns> A new guard condition instance. </returns>
+        internal GuardCondition CreateGuardCondition(Action callback)
+        {
+            lock (this.GuardConditions)
+            {
+                GuardCondition guardCondition = new GuardCondition(this, callback);
+                this.GuardConditions.Add(guardCondition);
+                return guardCondition;
+            }
+        }
+
+        /// <summary>
+        /// Remove a guard condition.
+        /// </summary>
+        /// <remarks>
+        /// This method is intended to be used by <see cref="GuardCondition.Dispose"/> and does not dispose the guard condition.
+        /// Furthermore, it is thread safe.
+        /// </remarks>
+        /// <param name="guardCondition"> Guard condition to remove. </param>
+        /// <returns> If the guard condition existed in this context and has been removed. </returns>
+        internal bool RemoveGuardCondition(GuardCondition guardCondition)
+        {
+            lock (this.GuardConditions)
+            {
+                return this.GuardConditions.Remove(guardCondition);
+            }
+        }
+
         /// <remarks>
         /// 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 2041b15..556b92f 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;
         }
 
         /// <summary>
@@ -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();
+        }
+
+        /// <summary> Dispose without modifying the context. </summary>
+        internal void DisposeFromContext()
+        {
+            if (this.Handle == IntPtr.Zero)
+            {
+                return;
             }
+
+            Utils.CheckReturnEnum(NativeRcl.rcl_guard_condition_fini(this.Handle));
+            this.FreeHandles();
         }
 
         /// <summary>
diff --git a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs
index 93f414c..d64b43a 100644
--- a/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs
+++ b/src/ros2cs/ros2cs_core/executors/ManualExecutor.cs
@@ -115,7 +115,7 @@ public bool IsReadOnly
         /// <exception cref="ObjectDisposedException"> If <paramref name="context"/> is disposed. </exception>
         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 f5171bd..e361513 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"); }
             );
         }
@@ -66,7 +65,7 @@ public void DisposedContextHandling()
         {
             this.Context.Dispose();
 
-            Assert.Throws<ObjectDisposedException>(() => new GuardCondition(this.Context, () => {}));
+            Assert.Throws<ObjectDisposedException>(() => this.Context.CreateGuardCondition(() => { }));
         }
 
         [Test]