From 1195ab7295aa882e8c168723f7f39f4844da31a6 Mon Sep 17 00:00:00 2001
From: Benjamin Maidel <benjamin.maidel@mojin-robotics.de>
Date: Tue, 26 Nov 2024 18:56:10 +0100
Subject: [PATCH] expose rcl_shutdown function

---
 rcldotnet/RCLdotnet.cs | 20 +++++++++++++++++++-
 rcldotnet/rcldotnet.c  | 14 ++++++++++++++
 rcldotnet/rcldotnet.h  |  3 +++
 3 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs
index eeefea8a..9e66b7d7 100644
--- a/rcldotnet/RCLdotnet.cs
+++ b/rcldotnet/RCLdotnet.cs
@@ -47,6 +47,11 @@ internal delegate void NativeRCLGetErrorStringType(
 
         internal static NativeRCLOkType native_rcl_ok = null;
 
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate RCLRet NativeRCLShutdownType();
+
+        internal static NativeRCLShutdownType native_rcl_shutdown = null;
+
         [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
         internal delegate RCLRet NativeRCLCreateNodeHandleType(
             ref SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, [MarshalAs(UnmanagedType.LPStr)] string nodeNamespace);
@@ -409,6 +414,12 @@ static RCLdotnetDelegates()
                 (NativeRCLOkType)Marshal.GetDelegateForFunctionPointer(
                     native_rcl_ok_ptr, typeof(NativeRCLOkType));
 
+            IntPtr native_rcl_shutdown_ptr =
+                _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_shutdown");
+            RCLdotnetDelegates.native_rcl_shutdown =
+                (NativeRCLShutdownType)Marshal.GetDelegateForFunctionPointer(
+                    native_rcl_shutdown_ptr, typeof(NativeRCLShutdownType));
+
             IntPtr native_rcl_create_node_handle_ptr =
                 _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_node_handle");
             RCLdotnetDelegates.native_rcl_create_node_handle =
@@ -744,7 +755,8 @@ public static class RCLdotnet
 
         public static bool Ok()
         {
-            return RCLdotnetDelegates.native_rcl_ok() != 0;
+            bool ret = RCLdotnetDelegates.native_rcl_ok() != 0;
+            return ret;
         }
 
         public static Node CreateNode(string nodeName)
@@ -774,6 +786,12 @@ public static void Spin(Node node)
             }
         }
 
+        public static void Shutdown()
+        {
+            RCLRet ret = RCLdotnetDelegates.native_rcl_shutdown();
+            RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_shutdown)}() failed.");
+        }
+
         public static Clock CreateClock(ClockType type = ClockType.RosTime)
         {
             var clockHandle = new SafeClockHandle();
diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c
index 40be6421..a8bf753e 100644
--- a/rcldotnet/rcldotnet.c
+++ b/rcldotnet/rcldotnet.c
@@ -83,6 +83,20 @@ int32_t native_rcl_ok() {
   return result ? 1 : 0;
 }
 
+int32_t native_rcl_shutdown() {
+  rcl_ret_t ret = rcl_shutdown(&context);
+  if (ret != RCL_RET_OK) {
+    return ret;
+  }
+  
+  // TODO: Add a `SafeContextHandle` and manipulate it's reference count
+  // in the child handles created from the context. This should be similar to
+  // `SafeNodeHandle` being referenced by `SafePublisherHandle` for example.
+  // See the documentation in rcl how the lifetime of the context object should work:
+  // https://github.com/ros2/rcl/blob/2a72dbaaf09458a998fad39d898adc9b8a60565a/rcl/include/rcl/context.h#L48-L112
+  return rcl_context_fini(&context);
+}
+
 int32_t native_rcl_create_node_handle(void **node_handle, const char *name, const char *namespace) {
   rcl_node_t *node = (rcl_node_t *)malloc(sizeof(rcl_node_t));
   *node = rcl_get_zero_initialized_node();
diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h
index c1021a76..677e915c 100644
--- a/rcldotnet/rcldotnet.h
+++ b/rcldotnet/rcldotnet.h
@@ -38,6 +38,9 @@ void RCLDOTNET_CDECL native_rcl_reset_error(void);
 RCLDOTNET_EXPORT
 int32_t RCLDOTNET_CDECL native_rcl_ok();
 
+RCLDOTNET_EXPORT
+int32_t RCLDOTNET_CDECL native_rcl_shutdown();
+
 RCLDOTNET_EXPORT
 int32_t RCLDOTNET_CDECL native_rcl_create_node_handle(void **, const char *, const char *);