diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 1439d56b..80dfad9b 100644 --- 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(); } + 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 Subscriptions => _subscriptions; // TODO: (sh) wrap in readonly collection diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 3a1136a1..5b3d296a 100644 --- 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 index ffd268dd..fe85820b 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -28,9 +28,7 @@ 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(); @@ -38,8 +36,7 @@ int32_t native_rcl_init() { 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 index 6c4252ff..e35114ea 100644 --- 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 index 3dfa900a..0d70f4be 100644 --- 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); +} \ No newline at end of file diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index f28da07e..ae610d9f 100644 --- 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