diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index 47165b05..56d032e7 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -97,7 +97,8 @@ internal TimeJump(ClockChange clockChange, Duration delta) internal delegate void JumpCallbackInternal(IntPtr timeJumpPtr, bool beforeJump); - public delegate void JumpCallback(TimeJump timeJump, bool beforeJump); + public delegate void PreJumpCallback(); + public delegate void PostJumpCallback(TimeJump timeJump); [StructLayout(LayoutKind.Sequential)] public readonly struct JumpThreshold @@ -198,7 +199,7 @@ static ClockDelegates() public sealed class Clock { - private readonly Dictionary _registeredJumpCallbacks = new Dictionary(); + private readonly HashSet _registeredJumpCallbacks = new HashSet(); private readonly object _lock = new object(); @@ -257,40 +258,120 @@ internal RCLRet SetRosTimeOverride(long timePointValue) return ret; } - public void AddJumpCallback(JumpThreshold threshold, JumpCallback callback) + /// + /// Add a callback to be called when a time jump exceeds a threshold. + /// + /// Callbacks will be triggered if the time jump is greater then the threshold. + /// Callback to be called before new time is set. + /// Callback to be called after new time is set. + /// A disposable object that can be used to remove the callbacks from the clock. + public IDisposable CreateJumpCallback(JumpThreshold threshold, PreJumpCallback preJumpCallback, PostJumpCallback postJumpCallback) { - if (_registeredJumpCallbacks.ContainsKey(callback)) + JumpHandler jumpHandler = new JumpHandler(this, preJumpCallback, postJumpCallback); + + lock (_lock) { - throw new CallbackAlreadyRegisteredException("Provided jump callback was already registered!"); + RCLRet ret = ClockDelegates.native_rcl_clock_add_jump_callback(Handle, threshold, jumpHandler.JumpCallback); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_add_jump_callback)}() failed."); + + // Save a reference to the JumpCallback that was passed down to + // the native side. The PInvoke interop does create a stub that + // get's passed down so that the delegate can be called via a + // native function pointer. This stub does not get reallocated + // by the CG, so no extra pinning is needed, but it will get + // deallocated once the delegate object gets collected. So we + // need to make shure to have a strong reference to the + // delegate, otherwise the native side would call a function + // pointer that points to the deallocated stub. + // rcl_clock_add_jump_callback only adds the provided callback + // to it's list if it returned OK, so we don't need to worry + // about references to the delegate from the native side in this + // case. + _registeredJumpCallbacks.Add(jumpHandler.JumpCallback); } - JumpCallbackInternal callbackInternal = (timeJumpPtr, beforeJump) => - callback(Marshal.PtrToStructure(timeJumpPtr), beforeJump); + return jumpHandler; + } - RCLRet ret; + internal void RemoveJumpCallback(JumpHandler jumpHandler, ref bool jumpHandlerDisposed) + { lock (_lock) { - ret = ClockDelegates.native_rcl_clock_add_jump_callback(Handle, threshold, callbackInternal); + if (jumpHandlerDisposed) + return; + + RCLRet ret = ClockDelegates.native_rcl_clock_remove_jump_callback(Handle, jumpHandler.JumpCallback); + + // Calling Dispose multiple times should not throw errors. + // rcl_clock_remove_jump_callback failues: + // - The null-checks can't fail as it is ensured that we always + // pass valid object references down. + // - We track if a JumpHandler is disposed via a flag, so the + // "jump callback was not found" error can't happen. + // - Only failues for allocation and internal errors in + // rcldotnet could cause exceptions. + // + // So we should be save to throw exceptions here, the user of + // the library should have no way to trigger it. + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_remove_jump_callback)}() failed."); + + jumpHandlerDisposed = true; + _registeredJumpCallbacks.Remove(jumpHandler.JumpCallback); } + } + } + + internal sealed class JumpHandler : IDisposable + { + private readonly Clock _clock; + private readonly PreJumpCallback _preJumpCallback; + private readonly PostJumpCallback _postJumpCallback; + + private bool _disposed; - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_add_jump_callback)}() failed."); + public JumpHandler(Clock clock, PreJumpCallback preJumpCallback, PostJumpCallback postJumpCallback) + { + _clock = clock; + _preJumpCallback = preJumpCallback; + _postJumpCallback = postJumpCallback; - _registeredJumpCallbacks.Add(callback, callbackInternal); + // Only create the delegate once and cache in instance + // so the same instance is avaliable to deregister. + JumpCallback = new JumpCallbackInternal(OnJump); } - public bool RemoveJumpCallback(JumpCallback callback) + internal JumpCallbackInternal JumpCallback { get; } + + public void Dispose() { - if (!_registeredJumpCallbacks.TryGetValue(callback, out JumpCallbackInternal callbackInternal)) return false; + // fast return without look + if (_disposed) + return; - RCLRet ret; - lock (_lock) + _clock.RemoveJumpCallback(this, ref _disposed); + } + + private void OnJump(IntPtr timeJumpPtr, bool beforeJump) + { + try { - ret = ClockDelegates.native_rcl_clock_remove_jump_callback(Handle, callbackInternal); + if (beforeJump) + { + _preJumpCallback?.Invoke(); + } + else + { + _postJumpCallback?.Invoke(Marshal.PtrToStructure(timeJumpPtr)); + } } + catch + { + // Catch all exceptions, as on non-windows plattforms exceptions + // that are propageted to native code can cause crashes. + // see https://learn.microsoft.com/en-us/dotnet/standard/native-interop/exceptions-interoperability - RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_remove_jump_callback)}() failed."); - - return _registeredJumpCallbacks.Remove(callback); + // TODO: (sh) Add error handling/logging. + } } } }