Merge pull request #122 from Chootin/tutina/global_argument_parsing

Implemented RCL global argument parsing.
ros2-dotnet · Oct 16, 2023 · 6ee149e · 6ee149e
2 parents 315e669 + c51b096
commit 6ee149e
Show file tree

Hide file tree

Showing 6 changed files with 86 additions and 8 deletions.
diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs
@@ -96,6 +96,12 @@ internal delegate RCLRet NativeRCLActionDestroyServerHandleType(
 
         internal static NativeRCLActionDestroyServerHandleType native_rcl_action_destroy_server_handle = null;
 
+        internal static RCLdotnetDelegates.NativeRCLGetStringType native_rcl_node_get_name_handle = null;
+
+        internal static RCLdotnetDelegates.NativeRCLGetStringType native_rcl_node_get_namespace_handle = null;
+
+        internal static RCLdotnetDelegates.NativeRCLGetStringType native_rcl_node_get_fully_qualified_name_handle = null;
+
         static NodeDelegates()
         {
             _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils();
@@ -182,6 +188,24 @@ static NodeDelegates()
             NodeDelegates.native_rcl_action_destroy_server_handle =
                 (NativeRCLActionDestroyServerHandleType)Marshal.GetDelegateForFunctionPointer(
                     native_rcl_action_destroy_server_handle_ptr, typeof(NativeRCLActionDestroyServerHandleType));
+
+            IntPtr native_rcl_node_get_name_handle_ptr = _dllLoadUtils.GetProcAddress(
+                nativeLibrary, "native_rcl_node_get_name_handle");
+            NodeDelegates.native_rcl_node_get_name_handle =
+                (RCLdotnetDelegates.NativeRCLGetStringType)Marshal.GetDelegateForFunctionPointer(
+                    native_rcl_node_get_name_handle_ptr, typeof(RCLdotnetDelegates.NativeRCLGetStringType));
+
+            IntPtr native_rcl_node_get_namespace_handle_ptr = _dllLoadUtils.GetProcAddress(
+                nativeLibrary, "native_rcl_node_get_namespace_handle");
+            NodeDelegates.native_rcl_node_get_namespace_handle =
+                (RCLdotnetDelegates.NativeRCLGetStringType)Marshal.GetDelegateForFunctionPointer(
+                    native_rcl_node_get_namespace_handle_ptr, typeof(RCLdotnetDelegates.NativeRCLGetStringType));
+
+            IntPtr native_rcl_node_get_fully_qualified_name_handle_ptr = _dllLoadUtils.GetProcAddress(
+                nativeLibrary, "native_rcl_node_get_fully_qualified_name_handle");
+            NodeDelegates.native_rcl_node_get_fully_qualified_name_handle =
+                (RCLdotnetDelegates.NativeRCLGetStringType)Marshal.GetDelegateForFunctionPointer(
+                    native_rcl_node_get_fully_qualified_name_handle_ptr, typeof(RCLdotnetDelegates.NativeRCLGetStringType));
         }
     }
 
@@ -210,6 +234,12 @@ internal Node(SafeNodeHandle handle)
             _actionServers = new List<ActionServer>();
         }
 
+        public string Name => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_name_handle, Handle);
+
+        public string Namespace => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_namespace_handle, Handle);
+
+        public string FullyQualifiedName => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_fully_qualified_name_handle, Handle);
+
         public IList<Subscription> Subscriptions => _subscriptions;
 
         // TODO: (sh) wrap in readonly collection

diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs
@@ -26,7 +26,8 @@ internal static class RCLdotnetDelegates
         internal static readonly DllLoadUtils _dllLoadUtils;
 
         [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
-        internal delegate RCLRet NativeRCLInitType();
+        internal delegate RCLRet NativeRCLInitType(
+            int argc, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.LPStr)] string[] argv);
 
         internal static NativeRCLInitType native_rcl_init = null;
 
@@ -351,6 +352,9 @@ internal delegate RCLRet NativeRCLWriteToQosProfileHandleType(
 
         internal static NativeRCLWriteToQosProfileHandleType native_rcl_write_to_qos_profile_handle = null;
 
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate IntPtr NativeRCLGetStringType(SafeHandle handle);
+
         static RCLdotnetDelegates()
         {
             _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils();
@@ -1396,7 +1400,8 @@ public static void Init()
             {
                 if (!initialized)
                 {
-                    RCLRet ret = RCLdotnetDelegates.native_rcl_init();
+                    string[] args = System.Environment.GetCommandLineArgs();
+                    RCLRet ret = RCLdotnetDelegates.native_rcl_init(args.Length, args);
                     RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_init)}() failed.");
                     initialized = true;
                 }
@@ -1457,5 +1462,24 @@ internal static void WriteToMessageHandle(IRosMessage message, SafeHandle messag
                 }
             }
         }
+
+        internal static string GetStringFromNativeDelegate(RCLdotnetDelegates.NativeRCLGetStringType nativeDelegate, SafeHandle safeHandle)
+        {
+            bool mustRelease = false;
+            try
+            {
+                // This avoids accessing a invalid/freed pointer if some other thread disposes the SafeNodeHandle.
+                safeHandle.DangerousAddRef(ref mustRelease);
+                IntPtr namePtr = nativeDelegate(safeHandle);
+                return Marshal.PtrToStringAnsi(namePtr);
+            }
+            finally
+            {
+                if (mustRelease)
+                {
+                    safeHandle.DangerousRelease();
+                }
+            }
+        }
     }
 }
diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c
@@ -28,18 +28,15 @@
 static rcl_context_t context;
 static rcl_clock_t clock;
 
-int32_t native_rcl_init() {
-  // TODO(esteve): parse args
-  int num_args = 0;
+int32_t native_rcl_init(int argc, const char *argv[]) {
   context = rcl_get_zero_initialized_context();
   rcl_allocator_t allocator = rcl_get_default_allocator();
   rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
   rcl_ret_t ret = rcl_init_options_init(&init_options, allocator);
   if (RCL_RET_OK != ret) {
     return ret;
   }
-  const char ** arg_values = NULL;
-  ret = rcl_init(num_args, arg_values, &init_options, &context);
+  ret = rcl_init(argc, argv, &init_options, &context);
   if (ret != RCL_RET_OK) {
     return ret;
   }

diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h
@@ -18,7 +18,7 @@
 #include "rcldotnet_macros.h"
 
 RCLDOTNET_EXPORT
-int32_t RCLDOTNET_CDECL native_rcl_init();
+int32_t RCLDOTNET_CDECL native_rcl_init(int argc, const char *argv[]);
 
 rcl_clock_t *native_rcl_get_default_clock();
 

diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c
@@ -240,3 +240,21 @@ int32_t native_rcl_action_destroy_server_handle(void *action_server_handle, void
 
   return ret;
 }
+
+const char * native_rcl_node_get_name_handle(void *node_handle) {
+  rcl_node_t *node = (rcl_node_t *)node_handle;
+
+  return rcl_node_get_name(node);
+}
+
+const char * native_rcl_node_get_namespace_handle(void *node_handle) {
+  rcl_node_t *node = (rcl_node_t *)node_handle;
+
+  return rcl_node_get_namespace(node);
+}
+
+const char * native_rcl_node_get_fully_qualified_name_handle(void *node_handle) {
+  rcl_node_t *node = (rcl_node_t *)node_handle;
+
+  return rcl_node_get_fully_qualified_name(node);
+}
diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h
@@ -71,4 +71,13 @@ int32_t RCLDOTNET_CDECL native_rcl_action_create_server_handle(void **action_ser
 RCLDOTNET_EXPORT
 int32_t RCLDOTNET_CDECL native_rcl_action_destroy_server_handle(void *action_server_handle, void *node_handle);
 
+RCLDOTNET_EXPORT
+const char * RCLDOTNET_CDECL native_rcl_node_get_name_handle(void *node_handle);
+
+RCLDOTNET_EXPORT
+const char * RCLDOTNET_CDECL native_rcl_node_get_namespace_handle(void *node_handle);
+
+RCLDOTNET_EXPORT
+const char * RCLDOTNET_CDECL native_rcl_node_get_fully_qualified_name_handle(void *node_handle);
+
 #endif // RCLDOTNET_NODE_H