Merge pull request #136 from benmaidel/feature/expose_timer_functions

expose rcl timers cancel, reset, is_canceled and is_ready functions.
ros2-dotnet · Dec 3, 2024 · e37645f · e37645f
2 parents 90d7470 + 8523d4c
commit e37645f
Show file tree

Hide file tree

Showing 3 changed files with 101 additions and 2 deletions.
diff --git a/rcldotnet/Timer.cs b/rcldotnet/Timer.cs
@@ -25,16 +25,36 @@ namespace ROS2
     internal static class TimerDelegates
     {
         [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
-        internal delegate RCLRet NativeRCLTimerFunctionType(SafeTimerHandle timerHandle);
+        internal delegate RCLRet NativeRCLTimerCallType(SafeTimerHandle timerHandle);
 
-        internal static NativeRCLTimerFunctionType native_rcl_timer_call = null;
+        internal static NativeRCLTimerCallType native_rcl_timer_call = null;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate RCLRet NativeRCLTimerCancelType(SafeTimerHandle timerHandle);
+        internal static NativeRCLTimerCancelType native_rcl_timer_cancel = null;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate RCLRet NativeRCLTimerResetType(SafeTimerHandle timerHandle);
+        internal static NativeRCLTimerResetType native_rcl_timer_reset = null;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate RCLRet NativeRCLTimerIsCanceledType(SafeTimerHandle timerHandle, out int isCanceled);
+        internal static NativeRCLTimerIsCanceledType native_rcl_timer_is_canceled = null;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate RCLRet NativeRCLTimerIsReadyType(SafeTimerHandle timerHandle, out int isReady);
+        internal static NativeRCLTimerIsReadyType native_rcl_timer_is_ready = null;
 
         static TimerDelegates()
         {
             var dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils();
             IntPtr nativeLibrary = dllLoadUtils.LoadLibrary("rcldotnet");
 
             dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_timer_call), out native_rcl_timer_call);
+            dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_timer_cancel), out native_rcl_timer_cancel);
+            dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_timer_reset), out native_rcl_timer_reset);
+            dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_timer_is_canceled), out native_rcl_timer_is_canceled);
+            dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_timer_is_ready), out native_rcl_timer_is_ready);
         }
     }
 
@@ -62,11 +82,48 @@ internal Timer(Clock clock, TimeSpan period, Action<TimeSpan> callback)
             Handle = handle;
         }
 
+        public void Cancel()
+        {
+            RCLRet ret = TimerDelegates.native_rcl_timer_cancel(Handle);
+            RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(TimerDelegates.native_rcl_timer_cancel)}() failed.");
+        }
+
+        public void Reset()
+        {
+            RCLRet ret = TimerDelegates.native_rcl_timer_reset(Handle);
+            RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(TimerDelegates.native_rcl_timer_reset)}() failed.");
+        }
+
+        public bool IsCanceled
+        {
+            get
+            {
+                RCLRet ret = TimerDelegates.native_rcl_timer_is_canceled(Handle, out int isCanceled);
+                RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(TimerDelegates.native_rcl_timer_is_canceled)}() failed.");
+                return isCanceled != 0;
+            }
+        }
+
+        public bool IsReady
+        {
+            get
+            {
+                RCLRet ret = TimerDelegates.native_rcl_timer_is_ready(Handle, out int isReady);
+                RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(TimerDelegates.native_rcl_timer_is_ready)}() failed.");
+                return isReady != 0;
+            }
+        }
+
         internal SafeTimerHandle Handle { get; }
 
         internal void Call()
         {
             RCLRet ret = TimerDelegates.native_rcl_timer_call(Handle);
+            if (ret == ROS2.RCLRet.TimerCanceled)
+            {
+                // Timer was canceled, do nothing.
+                return;
+            }
             RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(TimerDelegates.native_rcl_timer_call)}() failed.");
         }
 

diff --git a/rcldotnet/rcldotnet_timer.c b/rcldotnet/rcldotnet_timer.c
@@ -24,3 +24,33 @@ int32_t native_rcl_timer_call(void *timer_handle) {
 
   return rcl_timer_call(timer);
 }
+
+int32_t native_rcl_timer_cancel(void *timer_handle) {
+  rcl_timer_t *timer = (rcl_timer_t *)timer_handle;
+
+  return rcl_timer_cancel(timer);
+}
+
+int32_t native_rcl_timer_reset(void *timer_handle) {
+  rcl_timer_t *timer = (rcl_timer_t *)timer_handle;
+
+  return rcl_timer_reset(timer);
+}
+
+int32_t native_rcl_timer_is_canceled(void *timer_handle, int32_t *is_canceled) {
+  rcl_timer_t *timer = (rcl_timer_t *)timer_handle;
+
+  bool is_canceled_as_bool;
+  rcl_ret_t ret = rcl_timer_is_canceled(timer, &is_canceled_as_bool);
+  *is_canceled = is_canceled_as_bool ? 1 : 0;
+  return ret;
+}
+
+int32_t native_rcl_timer_is_ready(void *timer_handle, int32_t *is_ready) {
+  rcl_timer_t *timer = (rcl_timer_t *)timer_handle;
+
+  bool is_ready_as_bool;
+  rcl_ret_t ret = rcl_timer_is_ready(timer, &is_ready_as_bool);
+  *is_ready = is_ready_as_bool ? 1 : 0;
+  return ret;
+}
diff --git a/rcldotnet/rcldotnet_timer.h b/rcldotnet/rcldotnet_timer.h
@@ -20,4 +20,16 @@
 RCLDOTNET_EXPORT
 int32_t RCLDOTNET_CDECL native_rcl_timer_call(void *timer_handle);
 
+RCLDOTNET_EXPORT
+int32_t RCLDOTNET_CDECL native_rcl_timer_cancel(void *timer_handle);
+
+RCLDOTNET_EXPORT
+int32_t RCLDOTNET_CDECL native_rcl_timer_reset(void *timer_handle);
+
+RCLDOTNET_EXPORT
+int32_t RCLDOTNET_CDECL native_rcl_timer_is_canceled(void *timer_handle, int32_t *is_canceled);
+
+RCLDOTNET_EXPORT
+int32_t RCLDOTNET_CDECL native_rcl_timer_is_ready(void *timer_handle, int32_t *is_ready);
+
 #endif // RCLDOTNET_TIMER_H