From d3a2d21f62f0275bc30a699958dfaaf6c10c5bd4 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 25 Sep 2023 17:41:39 +1000 Subject: [PATCH 01/20] Started implementing clock and time in c#. --- rcldotnet/ActionClient.cs | 3 +- rcldotnet/ActionClientGoalHandle.cs | 7 +-- rcldotnet/CMakeLists.txt | 3 + rcldotnet/Clock.cs | 93 +++++++++++++++++++++++++++++ rcldotnet/RCLdotnet.cs | 36 +++++++++++ rcldotnet/SafeClockHandle.cs | 41 +++++++++++++ rcldotnet/rcldotnet.c | 19 ++++++ rcldotnet/rcldotnet.h | 6 ++ rcldotnet/rcldotnet_clock.c | 27 +++++++++ rcldotnet/rcldotnet_clock.h | 23 +++++++ 10 files changed, 252 insertions(+), 6 deletions(-) create mode 100644 rcldotnet/Clock.cs create mode 100644 rcldotnet/SafeClockHandle.cs create mode 100644 rcldotnet/rcldotnet_clock.c create mode 100644 rcldotnet/rcldotnet_clock.h diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index 9f9e362e..fd083a10 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -19,7 +19,6 @@ using System.Threading.Tasks; using action_msgs.msg; using action_msgs.srv; -using builtin_interfaces.msg; using ROS2.Utils; using unique_identifier_msgs.msg; @@ -375,7 +374,7 @@ internal override void HandleGoalResponse(long sequenceNumber, IRosMessage goalR var goalId = pendingGoalRequest.GoalId; var goalResponseCasted = (IRosActionSendGoalResponse)goalResponse; var accepted = goalResponseCasted.Accepted; - var stamp = (Time)goalResponseCasted.StampAsRosMessage; + var stamp = (builtin_interfaces.msg.Time)goalResponseCasted.StampAsRosMessage; var goalHandle = new ActionClientGoalHandle(this, goalId, accepted, stamp, pendingGoalRequest.FeedbackCallback); diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs index 34ca9baa..866b4678 100644 --- a/rcldotnet/ActionClientGoalHandle.cs +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -16,7 +16,6 @@ using System; using System.Threading; using System.Threading.Tasks; -using builtin_interfaces.msg; namespace ROS2 { @@ -31,7 +30,7 @@ internal ActionClientGoalHandle() public abstract bool Accepted { get; } - public abstract Time Stamp { get; } + public abstract builtin_interfaces.msg.Time Stamp { get; } public abstract ActionGoalStatus Status { get; internal set; } } @@ -51,7 +50,7 @@ internal ActionClientGoalHandle( ActionClient actionClient, Guid goalId, bool accepted, - Time stamp, + builtin_interfaces.msg.Time stamp, Action feedbackCallback) { GoalId = goalId; @@ -65,7 +64,7 @@ internal ActionClientGoalHandle( public override bool Accepted { get; } - public override Time Stamp { get; } + public override builtin_interfaces.msg.Time Stamp { get; } public override ActionGoalStatus Status { get; internal set; } diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 883bbe85..3d80568d 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -39,6 +39,7 @@ set(CS_SOURCES ActionServer.cs ActionServerGoalHandle.cs Client.cs + Clock.cs GuardCondition.cs GuidExtensions.cs MessageStaticMemberCache.cs @@ -52,6 +53,7 @@ set(CS_SOURCES SafeActionGoalHandle.cs SafeActionServerHandle.cs SafeClientHandle.cs + SafeClockHandle.cs SafeGuardConditionHandle.cs SafeNodeHandle.cs SafePublisherHandle.cs @@ -87,6 +89,7 @@ ament_export_assemblies_dll("lib/${PROJECT_NAME}/dotnet/${PROJECT_NAME}_assembli add_library(${PROJECT_NAME}_native SHARED rcldotnet_action_client.c rcldotnet_client.c + rcldotnet_clock.c rcldotnet_guard_condition.c rcldotnet_node.c rcldotnet_publisher.c diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs new file mode 100644 index 00000000..dfb31281 --- /dev/null +++ b/rcldotnet/Clock.cs @@ -0,0 +1,93 @@ +/* Copyright 2023 Queensland University of Technology. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +using System; +using System.Runtime.InteropServices; +using ROS2.Utils; + +namespace ROS2 +{ + /// + /// Time source type, used to indicate the source of a time measurement. + /// + /// ROSTime will report the latest value reported by a ROS time source, or + /// if a ROS time source is not active it reports the same as RCL_SYSTEM_TIME. + /// For more information about ROS time sources, refer to the design document: + /// http://design.ros2.org/articles/clock_and_time.html + /// + /// SystemTime reports the same value as the system clock. + /// + /// SteadyTime reports a value from a monotonically increasing clock. + /// + public enum ClockType + { + ClockUninitialized = 0, + ROSTime, + SystemTime, + SteadyTime + } + + public struct Time + { + public uint seconds; + public uint nanoseconds; + } + + internal static class ClockDelegates + { + private static readonly DllLoadUtils _dllLoadUtils; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLClockGetNowType( + SafeClockHandle clockHandle, out long time); + + internal static NativeRCLClockGetNowType native_rcl_clock_get_now = null; + + static ClockDelegates() + { + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); + + IntPtr native_rcl_clock_get_now_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_clock_get_now"); + ClockDelegates.native_rcl_clock_get_now = (NativeRCLClockGetNowType)Marshal.GetDelegateForFunctionPointer( + native_rcl_clock_get_now_ptr, typeof(NativeRCLClockGetNowType)); + } + } + + public sealed class Clock + { + private const long SECONDS_TO_NANOSECONDS = 1000L * 1000L * 1000L; + + internal Clock(SafeClockHandle handle) + { + Handle = handle; + } + + internal SafeClockHandle Handle { get; } + + public Time Now() + { + RCLRet ret = ClockDelegates.native_rcl_clock_get_now(Handle, out long timePoint); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_get_now)}() failed."); + + Time time = new Time(); + time.seconds = (uint)(timePoint / SECONDS_TO_NANOSECONDS); + time.nanoseconds = (uint)(timePoint - (time.seconds * SECONDS_TO_NANOSECONDS)); + + return time; + } + } +} \ No newline at end of file diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 5b3d296a..e0273fdd 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -355,6 +355,16 @@ internal delegate RCLRet NativeRCLWriteToQosProfileHandleType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr NativeRCLGetStringType(SafeHandle handle); + internal delegate RCLRet NativeRCLCreateClockHandleType( + ref SafeClockHandle clockHandles, int clockType); + + internal static NativeRCLCreateClockHandleType native_rcl_create_clock_handle = null; + + internal delegate RCLRet NativeRCLDestroyClockHandleType( + IntPtr clockHandle); + + internal static NativeRCLDestroyClockHandleType native_rcl_destroy_clock_handle = null; + static RCLdotnetDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -707,6 +717,18 @@ static RCLdotnetDelegates() RCLdotnetDelegates.native_rcl_write_to_qos_profile_handle = (NativeRCLWriteToQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_write_to_qos_profile_handle_ptr, typeof(NativeRCLWriteToQosProfileHandleType)); + + IntPtr native_rcl_create_clock_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_clock_handle"); + RCLdotnetDelegates.native_rcl_create_clock_handle = + (NativeRCLCreateClockHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_create_clock_handle_ptr, typeof(NativeRCLCreateClockHandleType)); + + IntPtr native_rcl_destroy_clock_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_destroy_clock_handle"); + RCLdotnetDelegates.native_rcl_destroy_clock_handle = + (NativeRCLDestroyClockHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_destroy_clock_handle_ptr, typeof(NativeRCLDestroyClockHandleType)); } } @@ -747,6 +769,20 @@ public static void Spin(Node node) } } + public static Clock CreateClock(ClockType type = ClockType.ROSTime) + { + var clockHandle = new SafeClockHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_clock_handle(ref clockHandle, (int)type); + if (ret != RCLRet.Ok) + { + clockHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_clock_handle)}() failed."); + } + + Clock clock = new Clock(clockHandle); + return clock; + } + private static SafeWaitSetHandle CreateWaitSet( int numberOfSubscriptions, int numberOfGuardConditions, diff --git a/rcldotnet/SafeClockHandle.cs b/rcldotnet/SafeClockHandle.cs new file mode 100644 index 00000000..70cf6b07 --- /dev/null +++ b/rcldotnet/SafeClockHandle.cs @@ -0,0 +1,41 @@ +/* Copyright 2023 Queensland University of Technology + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_clock_t + /// + internal sealed class SafeClockHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeClockHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_destroy_clock_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index fe85820b..d6050628 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -49,6 +49,25 @@ rcl_clock_t *native_rcl_get_default_clock() { return &clock; } +int32_t native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_clock_t *clock; + + rcl_ret_t ret = rcl_clock_init((rcl_clock_type_t)clock_type, clock, &allocator); + + *clock_handle = (void *)clock; + return ret; +} + +int32_t native_rcl_destroy_clock_handle(void *clock_handle) { + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + + rcl_ret_t ret = rcl_clock_fini(clock); + free(clock); + + return ret; +} + const char *native_rcl_get_rmw_identifier() { return rmw_get_implementation_identifier(); } diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index e35114ea..5fea811b 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -22,6 +22,12 @@ int32_t RCLDOTNET_CDECL native_rcl_init(int argc, const char *argv[]); rcl_clock_t *native_rcl_get_default_clock(); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_clock_handle(void *clock_handle); + RCLDOTNET_EXPORT const char * RCLDOTNET_CDECL native_rcl_get_rmw_identifier(); diff --git a/rcldotnet/rcldotnet_clock.c b/rcldotnet/rcldotnet_clock.c new file mode 100644 index 00000000..4e1f9422 --- /dev/null +++ b/rcldotnet/rcldotnet_clock.c @@ -0,0 +1,27 @@ +// Copyright 2023 Queensland University of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include "rcldotnet_clock.h" + +int32_t native_rcl_clock_get_now(void *clock_handle, int64_t *time_point) { + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + rcl_time_point_value_t *time = (rcl_time_point_value_t *)time_point; + + return rcl_clock_get_now(clock, time); +} diff --git a/rcldotnet/rcldotnet_clock.h b/rcldotnet/rcldotnet_clock.h new file mode 100644 index 00000000..2ac5f1b8 --- /dev/null +++ b/rcldotnet/rcldotnet_clock.h @@ -0,0 +1,23 @@ +// Copyright 2023 Queensland University of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLDOTNET_CLOCK_H +#define RCLDOTNET_CLOCK_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_clock_get_now(void *clock_handle, int64_t *time_point); + +#endif // RCLDOTNET_CLOCK_H From 9c9daa0375ed544b94cbeb0fb40fa9d9fce749d9 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Tue, 26 Sep 2023 10:37:46 +1000 Subject: [PATCH 02/20] Finished exposing clock to .NET --- rcldotnet/ActionClient.cs | 3 ++- rcldotnet/ActionClientGoalHandle.cs | 7 ++++--- rcldotnet/Clock.cs | 30 ++++++++++++++++++----------- rcldotnet/rcldotnet.c | 2 +- 4 files changed, 26 insertions(+), 16 deletions(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index fd083a10..9f9e362e 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -19,6 +19,7 @@ using System.Threading.Tasks; using action_msgs.msg; using action_msgs.srv; +using builtin_interfaces.msg; using ROS2.Utils; using unique_identifier_msgs.msg; @@ -374,7 +375,7 @@ internal override void HandleGoalResponse(long sequenceNumber, IRosMessage goalR var goalId = pendingGoalRequest.GoalId; var goalResponseCasted = (IRosActionSendGoalResponse)goalResponse; var accepted = goalResponseCasted.Accepted; - var stamp = (builtin_interfaces.msg.Time)goalResponseCasted.StampAsRosMessage; + var stamp = (Time)goalResponseCasted.StampAsRosMessage; var goalHandle = new ActionClientGoalHandle(this, goalId, accepted, stamp, pendingGoalRequest.FeedbackCallback); diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs index 866b4678..34ca9baa 100644 --- a/rcldotnet/ActionClientGoalHandle.cs +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -16,6 +16,7 @@ using System; using System.Threading; using System.Threading.Tasks; +using builtin_interfaces.msg; namespace ROS2 { @@ -30,7 +31,7 @@ internal ActionClientGoalHandle() public abstract bool Accepted { get; } - public abstract builtin_interfaces.msg.Time Stamp { get; } + public abstract Time Stamp { get; } public abstract ActionGoalStatus Status { get; internal set; } } @@ -50,7 +51,7 @@ internal ActionClientGoalHandle( ActionClient actionClient, Guid goalId, bool accepted, - builtin_interfaces.msg.Time stamp, + Time stamp, Action feedbackCallback) { GoalId = goalId; @@ -64,7 +65,7 @@ internal ActionClientGoalHandle( public override bool Accepted { get; } - public override builtin_interfaces.msg.Time Stamp { get; } + public override Time Stamp { get; } public override ActionGoalStatus Status { get; internal set; } diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index dfb31281..957f992c 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -15,6 +15,7 @@ using System; using System.Runtime.InteropServices; +using builtin_interfaces.msg; using ROS2.Utils; namespace ROS2 @@ -39,10 +40,22 @@ public enum ClockType SteadyTime } - public struct Time + public struct TimePoint { - public uint seconds; - public uint nanoseconds; + private const long SECONDS_TO_NANOSECONDS = 1000L * 1000L * 1000L; + + public long nanoseconds; + + public Time ToMsg() + { + long sec = nanoseconds / SECONDS_TO_NANOSECONDS; + long nanosec = nanoseconds - (sec * SECONDS_TO_NANOSECONDS); + return new Time + { + Sec = (int)sec, + Nanosec = (uint)nanosec + }; + } } internal static class ClockDelegates @@ -51,7 +64,7 @@ internal static class ClockDelegates [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLClockGetNowType( - SafeClockHandle clockHandle, out long time); + SafeClockHandle clockHandle, out TimePoint time); internal static NativeRCLClockGetNowType native_rcl_clock_get_now = null; @@ -68,7 +81,6 @@ static ClockDelegates() public sealed class Clock { - private const long SECONDS_TO_NANOSECONDS = 1000L * 1000L * 1000L; internal Clock(SafeClockHandle handle) { @@ -79,15 +91,11 @@ internal Clock(SafeClockHandle handle) public Time Now() { - RCLRet ret = ClockDelegates.native_rcl_clock_get_now(Handle, out long timePoint); + RCLRet ret = ClockDelegates.native_rcl_clock_get_now(Handle, out TimePoint timePoint); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_get_now)}() failed."); - Time time = new Time(); - time.seconds = (uint)(timePoint / SECONDS_TO_NANOSECONDS); - time.nanoseconds = (uint)(timePoint - (time.seconds * SECONDS_TO_NANOSECONDS)); - - return time; + return timePoint.ToMsg(); } } } \ No newline at end of file diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index d6050628..a4c40007 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -51,7 +51,7 @@ rcl_clock_t *native_rcl_get_default_clock() { int32_t native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type) { rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_clock_t *clock; + rcl_clock_t *clock = malloc(sizeof(rcl_clock_t)); rcl_ret_t ret = rcl_clock_init((rcl_clock_type_t)clock_type, clock, &allocator); From 3bd4a55dd795c413e563c88cae9381ef1dc41f5d Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 28 Sep 2023 15:47:13 +1000 Subject: [PATCH 03/20] Adding convenience function to reduce boilerplate code. --- rcldotnet_common/DllLoadUtils.cs | 77 +++++++++++++++++--------------- 1 file changed, 40 insertions(+), 37 deletions(-) diff --git a/rcldotnet_common/DllLoadUtils.cs b/rcldotnet_common/DllLoadUtils.cs index 6ce14ee9..556d80a9 100644 --- a/rcldotnet_common/DllLoadUtils.cs +++ b/rcldotnet_common/DllLoadUtils.cs @@ -177,34 +177,46 @@ public interface DllLoadUtils IntPtr LoadLibrary(string fileName); void FreeLibrary(IntPtr handle); IntPtr GetProcAddress(IntPtr dllHandle, string name); + void RegisterNativeFunction(IntPtr dllHandle, string nativeFunctionName, + out FunctionType functionDelegate) where FunctionType : Delegate; } - public class DllLoadUtilsUWP : DllLoadUtils + public abstract class DllLoadUtilsAbs : DllLoadUtils + { + public abstract IntPtr LoadLibrary(string fileName); + + public abstract void FreeLibrary(IntPtr handle); + + public abstract IntPtr GetProcAddress(IntPtr dllHandle, string name); + + public void RegisterNativeFunction(IntPtr nativeLibrary, string nativeFunctionName, + out FunctionType functionDelegate) where FunctionType : Delegate + { + IntPtr nativeFunctionPointer = GetProcAddress(nativeLibrary, nativeFunctionName); + functionDelegate = (FunctionType)Marshal.GetDelegateForFunctionPointer(nativeFunctionPointer, typeof(FunctionType)); + } + } + + public class DllLoadUtilsUWP : DllLoadUtilsAbs { [DllImport("api-ms-win-core-libraryloader-l2-1-0.dll", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr LoadPackagedLibrary([MarshalAs(UnmanagedType.LPWStr)] string fileName, int reserved = 0); + private static extern IntPtr LoadPackagedLibraryUWP([MarshalAs(UnmanagedType.LPWStr)] string fileName, int reserved = 0); [DllImport("api-ms-win-core-libraryloader-l1-2-0.dll", SetLastError = true, ExactSpelling = true)] - private static extern int FreeLibrary(IntPtr handle); + private static extern int FreeLibraryUWP(IntPtr handle); [DllImport("api-ms-win-core-libraryloader-l1-2-0.dll", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr GetProcAddress(IntPtr handle, string procedureName); + private static extern IntPtr GetProcAddressUWP(IntPtr handle, string procedureName); - void DllLoadUtils.FreeLibrary(IntPtr handle) - { - FreeLibrary(handle); - } + public override void FreeLibrary(IntPtr handle) => FreeLibraryUWP(handle); - IntPtr DllLoadUtils.GetProcAddress(IntPtr dllHandle, string name) - { - return GetProcAddress(dllHandle, name); - } + public override IntPtr GetProcAddress(IntPtr dllHandle, string name) => GetProcAddressUWP(dllHandle, name); - IntPtr DllLoadUtils.LoadLibrary(string fileName) + public override IntPtr LoadLibrary(string fileName) { string libraryName = fileName + "_native.dll"; - IntPtr ptr = LoadPackagedLibrary(libraryName); + IntPtr ptr = LoadPackagedLibraryUWP(libraryName); if (ptr == IntPtr.Zero) { throw new UnsatisfiedLinkError(libraryName); @@ -213,29 +225,23 @@ IntPtr DllLoadUtils.LoadLibrary(string fileName) } } - public class DllLoadUtilsWindowsDesktop : DllLoadUtils + public class DllLoadUtilsWindowsDesktop : DllLoadUtilsAbs { [DllImport("kernel32.dll", EntryPoint = "LoadLibraryA", SetLastError = true, ExactSpelling = true)] private static extern IntPtr LoadLibraryA(string fileName, int reserved = 0); [DllImport("kernel32.dll", SetLastError = true, ExactSpelling = true)] - private static extern int FreeLibrary(IntPtr handle); + private static extern int FreeLibraryWin(IntPtr handle); [DllImport("kernel32.dll", SetLastError = true, ExactSpelling = true)] - private static extern IntPtr GetProcAddress(IntPtr handle, string procedureName); + private static extern IntPtr GetProcAddressWin(IntPtr handle, string procedureName); - void DllLoadUtils.FreeLibrary(IntPtr handle) - { - FreeLibrary(handle); - } + public override void FreeLibrary(IntPtr handle) => FreeLibraryWin(handle); - IntPtr DllLoadUtils.GetProcAddress(IntPtr dllHandle, string name) - { - return GetProcAddress(dllHandle, name); - } + public override IntPtr GetProcAddress(IntPtr dllHandle, string name) => GetProcAddressWin(dllHandle, name); - IntPtr DllLoadUtils.LoadLibrary(string fileName) + public override IntPtr LoadLibrary(string fileName) { string libraryName = fileName + "_native.dll"; IntPtr ptr = LoadLibraryA(libraryName); @@ -247,7 +253,7 @@ IntPtr DllLoadUtils.LoadLibrary(string fileName) } } - internal class DllLoadUtilsUnix : DllLoadUtils + internal class DllLoadUtilsUnix : DllLoadUtilsAbs { [DllImport("libdl.so.2", ExactSpelling = true)] @@ -264,12 +270,9 @@ internal class DllLoadUtilsUnix : DllLoadUtils private const int RTLD_NOW = 2; - public void FreeLibrary(IntPtr handle) - { - dlclose(handle); - } + public override void FreeLibrary(IntPtr handle) => dlclose(handle); - public IntPtr GetProcAddress(IntPtr dllHandle, string name) + public override IntPtr GetProcAddress(IntPtr dllHandle, string name) { // clear previous errors if any dlerror(); @@ -282,7 +285,7 @@ public IntPtr GetProcAddress(IntPtr dllHandle, string name) return res; } - public IntPtr LoadLibrary(string fileName) + public override IntPtr LoadLibrary(string fileName) { string libraryName = "lib" + fileName + "_native.so"; IntPtr ptr = dlopen(libraryName, RTLD_NOW); @@ -294,7 +297,7 @@ public IntPtr LoadLibrary(string fileName) } } - internal class DllLoadUtilsMacOSX : DllLoadUtils + internal class DllLoadUtilsMacOSX : DllLoadUtilsAbs { [DllImport("libdl.dylib", ExactSpelling = true)] @@ -311,12 +314,12 @@ internal class DllLoadUtilsMacOSX : DllLoadUtils private const int RTLD_NOW = 2; - public void FreeLibrary(IntPtr handle) + public override void FreeLibrary(IntPtr handle) { dlclose(handle); } - public IntPtr GetProcAddress(IntPtr dllHandle, string name) + public override IntPtr GetProcAddress(IntPtr dllHandle, string name) { // clear previous errors if any dlerror(); @@ -329,7 +332,7 @@ public IntPtr GetProcAddress(IntPtr dllHandle, string name) return res; } - public IntPtr LoadLibrary(string fileName) + public override IntPtr LoadLibrary(string fileName) { string libraryName = "lib" + fileName + "_native.dylib"; IntPtr ptr = dlopen(libraryName, RTLD_NOW); From e84d5371f649165267c0979c5fc5edccaf57dc8a Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 28 Sep 2023 17:56:11 +1000 Subject: [PATCH 04/20] Adding time jump callback features --- rcldotnet/Clock.cs | 141 +++++++++++++++++++++++++++++++++--- rcldotnet/rcldotnet_clock.c | 35 ++++++++- rcldotnet/rcldotnet_clock.h | 17 ++++- 3 files changed, 181 insertions(+), 12 deletions(-) diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index 957f992c..bbed5e22 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -20,6 +20,11 @@ namespace ROS2 { + public static class TimeConstants + { + public const long SECONDS_TO_NANOSECONDS = 1000L * 1000L * 1000L; + } + /// /// Time source type, used to indicate the source of a time measurement. /// @@ -40,22 +45,75 @@ public enum ClockType SteadyTime } + /// Enumeration to describe the type of time jump. + /// see definition here: https://github.com/ros2/rcl/blob/master/rcl/include/rcl/time.h + public enum ClockChange + { + /// The source before and after the jump is ROS_TIME. + RosTimeNoChange = 1, + /// The source switched to ROS_TIME from SYSTEM_TIME. + RosTimeActivated = 2, + /// The source switched to SYSTEM_TIME from ROS_TIME. + RosTimeDeactivated = 3, + /// The source before and after the jump is SYSTEM_TIME. + SystemTimeNoChange = 4 + } + + public struct TimeJump + { + public ClockChange clockChange; + public Duration delta; + } + + public delegate void JumpCallback(TimeJump timeJump, bool beforeJump); + + public struct JumpThreshold + { + public bool onClockChange; + public Duration minForward; + public Duration minBackward; + + public JumpThreshold(bool onClockChange, double minForwardSeconds, double minBackwardSeconds) + { + this.onClockChange = onClockChange; + minForward = new Duration(minForwardSeconds); + minBackward = new Duration(minBackwardSeconds); + } + } + public struct TimePoint { - private const long SECONDS_TO_NANOSECONDS = 1000L * 1000L * 1000L; public long nanoseconds; public Time ToMsg() { - long sec = nanoseconds / SECONDS_TO_NANOSECONDS; - long nanosec = nanoseconds - (sec * SECONDS_TO_NANOSECONDS); + long sec = nanoseconds / TimeConstants.SECONDS_TO_NANOSECONDS; + long nanosec = nanoseconds - (sec * TimeConstants.SECONDS_TO_NANOSECONDS); return new Time { Sec = (int)sec, Nanosec = (uint)nanosec }; } + + public static TimePoint FromMsg(Time message) + { + return new TimePoint + { + nanoseconds = message.Sec * TimeConstants.SECONDS_TO_NANOSECONDS + message.Nanosec + }; + } + } + + public struct Duration + { + public long nanoseconds; + + public Duration(double seconds) + { + nanoseconds = (long)(seconds * TimeConstants.SECONDS_TO_NANOSECONDS); + } } internal static class ClockDelegates @@ -63,19 +121,43 @@ internal static class ClockDelegates private static readonly DllLoadUtils _dllLoadUtils; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLClockGetNowType( - SafeClockHandle clockHandle, out TimePoint time); + internal delegate RCLRet NativeRCLClockFunctionType(SafeClockHandle clockHandle); + + internal static NativeRCLClockFunctionType native_rcl_enable_ros_time_override = null; + + internal static NativeRCLClockFunctionType native_rcl_disable_ros_time_override = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLClockGetNowType(SafeClockHandle clockHandle, out TimePoint time); internal static NativeRCLClockGetNowType native_rcl_clock_get_now = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLSetRosTimeOverrideType(SafeClockHandle clockHandle, long timePointValue); + + internal static NativeRCLSetRosTimeOverrideType native_rcl_set_ros_time_override = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLAddJumpCallbackType(SafeClockHandle clockHandle, JumpThreshold threshold, JumpCallback callback); + + internal static NativeRCLAddJumpCallbackType native_rcl_clock_add_jump_callback = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLRemoveJumpCallbackType(SafeClockHandle clockHandle, JumpCallback callback); + + internal static NativeRCLRemoveJumpCallbackType native_rcl_clock_remove_jump_callback = null; + static ClockDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); - IntPtr native_rcl_clock_get_now_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_clock_get_now"); - ClockDelegates.native_rcl_clock_get_now = (NativeRCLClockGetNowType)Marshal.GetDelegateForFunctionPointer( - native_rcl_clock_get_now_ptr, typeof(NativeRCLClockGetNowType)); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_enable_ros_time_override), out native_rcl_enable_ros_time_override); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_disable_ros_time_override), out native_rcl_disable_ros_time_override); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_clock_get_now), out native_rcl_clock_get_now); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_set_ros_time_override), out native_rcl_set_ros_time_override); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_clock_add_jump_callback), out native_rcl_clock_add_jump_callback); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_clock_remove_jump_callback), out native_rcl_clock_remove_jump_callback); } } @@ -97,5 +179,46 @@ public Time Now() return timePoint.ToMsg(); } + + internal RCLRet EnableRosTimeOverride() + { + RCLRet ret = ClockDelegates.native_rcl_enable_ros_time_override(Handle); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_enable_ros_time_override)}() failed."); + + return ret; + } + + internal RCLRet DisableRosTimeOverride() + { + RCLRet ret = ClockDelegates.native_rcl_disable_ros_time_override(Handle); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_disable_ros_time_override)}() failed."); + + return ret; + } + + internal RCLRet SetRosTimeOverride(long timePointValue) + { + RCLRet ret = ClockDelegates.native_rcl_set_ros_time_override(Handle, timePointValue); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_set_ros_time_override)}() failed."); + + return ret; + } + + public void AddJumpCallback(JumpThreshold threshold, JumpCallback callback) + { + RCLRet ret = ClockDelegates.native_rcl_clock_add_jump_callback(Handle, threshold, callback); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_add_jump_callback)}() failed."); + } + + public void RemoveJumpCallback(JumpCallback callback) + { + RCLRet ret = ClockDelegates.native_rcl_clock_remove_jump_callback(Handle, callback); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_remove_jump_callback)}() failed."); + } } -} \ No newline at end of file +} diff --git a/rcldotnet/rcldotnet_clock.c b/rcldotnet/rcldotnet_clock.c index 4e1f9422..c58d67ec 100644 --- a/rcldotnet/rcldotnet_clock.c +++ b/rcldotnet/rcldotnet_clock.c @@ -1,4 +1,4 @@ -// Copyright 2023 Queensland University of Technology +// Copyright 2023 Queensland University of Technology. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,6 +22,37 @@ int32_t native_rcl_clock_get_now(void *clock_handle, int64_t *time_point) { rcl_clock_t *clock = (rcl_clock_t *)clock_handle; rcl_time_point_value_t *time = (rcl_time_point_value_t *)time_point; - + return rcl_clock_get_now(clock, time); } + +int32_t native_rcl_enable_ros_time_override(void *clock_handle) { + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + + return rcl_enable_ros_time_override(clock); +} + +int32_t native_rcl_disable_ros_time_override(void *clock_handle) { + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + + return rcl_disable_ros_time_override(clock); +} + +int32_t native_rcl_set_ros_time_override(void *clock_handle, int64_t time_point_value) { + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + rcl_time_point_value_t time_point_value_native = (rcl_time_point_value_t)time_point_value; + + return rcl_set_ros_time_override(clock, time_point_value_native); +} + +int32_t native_rcl_clock_add_jump_callback(void *clock_handle, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback) { + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + + return rcl_clock_add_jump_callback(clock, threshold, callback, NULL); +} + +int32_t native_rcl_clock_remove_jump_callback(void *clock_handle, rcl_jump_callback_t callback) { + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + + return rcl_clock_remove_jump_callback(clock, callback, NULL); +} diff --git a/rcldotnet/rcldotnet_clock.h b/rcldotnet/rcldotnet_clock.h index 2ac5f1b8..a1457562 100644 --- a/rcldotnet/rcldotnet_clock.h +++ b/rcldotnet/rcldotnet_clock.h @@ -1,4 +1,4 @@ -// Copyright 2023 Queensland University of Technology +// Copyright 2023 Queensland University of Technology. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,4 +20,19 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_clock_get_now(void *clock_handle, int64_t *time_point); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_enable_ros_time_override(void *clock_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_disable_ros_time_override(void *clock_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_set_ros_time_override(void *clock_handle, int64_t time_point_value); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_clock_add_jump_callback(void *clock_handle, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_clock_remove_jump_callback(void *clock_handle, rcl_jump_callback_t callback); + #endif // RCLDOTNET_CLOCK_H From 8ec3824f00b48aaf69cfa3bb22177fe33475f1aa Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 23 Oct 2023 11:09:54 +1000 Subject: [PATCH 05/20] Adding clock to node --- rcldotnet/Node.cs | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 80dfad9b..731dbc33 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -211,6 +211,8 @@ static NodeDelegates() public sealed class Node { + private readonly Clock _clock; + private readonly IList _subscriptions; private readonly IList _services; @@ -226,6 +228,9 @@ public sealed class Node internal Node(SafeNodeHandle handle) { Handle = handle; + + _clock = RCLdotnet.CreateClock(); + _subscriptions = new List(); _services = new List(); _clients = new List(); @@ -240,6 +245,8 @@ internal Node(SafeNodeHandle handle) public string FullyQualifiedName => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_fully_qualified_name_handle, Handle); + public Clock Clock => _clock; + public IList Subscriptions => _subscriptions; // TODO: (sh) wrap in readonly collection From 2870c7648105d13b0c2ddedb5c18b91d0ddcca64 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 23 Oct 2023 11:10:17 +1000 Subject: [PATCH 06/20] Replacing static clock with node clock --- rcldotnet/Node.cs | 4 ++-- rcldotnet/rcldotnet.c | 6 ------ rcldotnet/rcldotnet.h | 2 -- rcldotnet/rcldotnet_node.c | 3 ++- rcldotnet/rcldotnet_node.h | 1 + 5 files changed, 5 insertions(+), 11 deletions(-) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 731dbc33..90c928d0 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -86,7 +86,7 @@ internal delegate RCLRet NativeRCLActionDestroyClientHandleType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionCreateServerHandleType( - ref SafeActionServerHandle actionServerHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); + ref SafeActionServerHandle actionServerHandle, SafeNodeHandle nodeHandle, SafeClockHandle clockHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); internal static NativeRCLActionCreateServerHandleType native_rcl_action_create_server_handle = null; @@ -449,7 +449,7 @@ public ActionServer CreateActionServer.GetTypeSupport(); var actionServerHandle = new SafeActionServerHandle(); - RCLRet ret = NodeDelegates.native_rcl_action_create_server_handle(ref actionServerHandle, Handle, actionName, typeSupport); + RCLRet ret = NodeDelegates.native_rcl_action_create_server_handle(ref actionServerHandle, Handle, _clock.Handle, actionName, typeSupport); actionServerHandle.SetParent(Handle); if (ret != RCLRet.Ok) { diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index a4c40007..1346b784 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -26,7 +26,6 @@ #include "rcldotnet.h" static rcl_context_t context; -static rcl_clock_t clock; int32_t native_rcl_init(int argc, const char *argv[]) { context = rcl_get_zero_initialized_context(); @@ -41,14 +40,9 @@ int32_t native_rcl_init(int argc, const char *argv[]) { return ret; } - ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); return ret; } -rcl_clock_t *native_rcl_get_default_clock() { - return &clock; -} - int32_t native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type) { rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_clock_t *clock = malloc(sizeof(rcl_clock_t)); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 5fea811b..4e8f7625 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -20,8 +20,6 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_init(int argc, const char *argv[]); -rcl_clock_t *native_rcl_get_default_clock(); - RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type); diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index 0d70f4be..989dc8b4 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -208,9 +208,11 @@ int32_t native_rcl_action_destroy_client_handle(void *action_client_handle, void int32_t native_rcl_action_create_server_handle(void **action_server_handle, void *node_handle, + void *clock_handle, const char *action_name, void *typesupport) { rcl_node_t *node = (rcl_node_t *)node_handle; + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; rosidl_action_type_support_t *ts = (rosidl_action_type_support_t *)typesupport; @@ -221,7 +223,6 @@ int32_t native_rcl_action_create_server_handle(void **action_server_handle, rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - rcl_clock_t *clock = native_rcl_get_default_clock(); rcl_ret_t ret = rcl_action_server_init(action_server, node, clock, ts, action_name, &action_server_ops); diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index ae610d9f..227e5325 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -65,6 +65,7 @@ int32_t RCLDOTNET_CDECL native_rcl_action_destroy_client_handle(void *action_cli RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_create_server_handle(void **action_server_handle, void *node_handle, + void *clock_handle, const char *action_name, void *typesupport); From b5c3330c08e21c2aa26f0b37ed64cfb2492800a1 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Fri, 29 Sep 2023 11:38:43 +1000 Subject: [PATCH 07/20] Completed integration of time jump callbacks. --- rcldotnet/Clock.cs | 39 ++++++++++++++++++++++++++++++++----- rcldotnet/rcldotnet_clock.c | 4 ++-- rcldotnet/rcldotnet_clock.h | 4 ++-- 3 files changed, 38 insertions(+), 9 deletions(-) diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index bbed5e22..a257eddf 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -14,6 +14,7 @@ */ using System; +using System.Collections.Generic; using System.Runtime.InteropServices; using builtin_interfaces.msg; using ROS2.Utils; @@ -59,12 +60,15 @@ public enum ClockChange SystemTimeNoChange = 4 } + [StructLayout(LayoutKind.Sequential)] public struct TimeJump { public ClockChange clockChange; public Duration delta; } + internal delegate void JumpCallbackInternal(IntPtr timeJumpPtr, bool beforeJump); + public delegate void JumpCallback(TimeJump timeJump, bool beforeJump); public struct JumpThreshold @@ -106,6 +110,7 @@ public static TimePoint FromMsg(Time message) } } + [StructLayout(LayoutKind.Sequential)] public struct Duration { public long nanoseconds; @@ -114,6 +119,15 @@ public Duration(double seconds) { nanoseconds = (long)(seconds * TimeConstants.SECONDS_TO_NANOSECONDS); } + + public double Seconds => (double)nanoseconds / TimeConstants.SECONDS_TO_NANOSECONDS; + } + + public class CallbackAlreadyRegisteredException : Exception + { + public CallbackAlreadyRegisteredException(string message) : base(message) + { + } } internal static class ClockDelegates @@ -138,12 +152,12 @@ internal static class ClockDelegates internal static NativeRCLSetRosTimeOverrideType native_rcl_set_ros_time_override = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLAddJumpCallbackType(SafeClockHandle clockHandle, JumpThreshold threshold, JumpCallback callback); + internal delegate RCLRet NativeRCLAddJumpCallbackType(SafeClockHandle clockHandle, JumpThreshold threshold, JumpCallbackInternal callback); internal static NativeRCLAddJumpCallbackType native_rcl_clock_add_jump_callback = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate RCLRet NativeRCLRemoveJumpCallbackType(SafeClockHandle clockHandle, JumpCallback callback); + internal delegate RCLRet NativeRCLRemoveJumpCallbackType(SafeClockHandle clockHandle, JumpCallbackInternal callback); internal static NativeRCLRemoveJumpCallbackType native_rcl_clock_remove_jump_callback = null; @@ -163,6 +177,7 @@ static ClockDelegates() public sealed class Clock { + private readonly Dictionary _registeredJumpCallbacks = new Dictionary(); internal Clock(SafeClockHandle handle) { @@ -209,16 +224,30 @@ internal RCLRet SetRosTimeOverride(long timePointValue) public void AddJumpCallback(JumpThreshold threshold, JumpCallback callback) { - RCLRet ret = ClockDelegates.native_rcl_clock_add_jump_callback(Handle, threshold, callback); + if (_registeredJumpCallbacks.ContainsKey(callback)) + { + throw new CallbackAlreadyRegisteredException("Provided jump callback was already registered!"); + } + + JumpCallbackInternal callbackInternal = (timeJumpPtr, beforeJump) => + callback(Marshal.PtrToStructure(timeJumpPtr), beforeJump); + + RCLRet ret = ClockDelegates.native_rcl_clock_add_jump_callback(Handle, threshold, callbackInternal); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_add_jump_callback)}() failed."); + + _registeredJumpCallbacks.Add(callback, callbackInternal); } - public void RemoveJumpCallback(JumpCallback callback) + public bool RemoveJumpCallback(JumpCallback callback) { - RCLRet ret = ClockDelegates.native_rcl_clock_remove_jump_callback(Handle, callback); + if (!_registeredJumpCallbacks.TryGetValue(callback, out JumpCallbackInternal callbackInternal)) return false; + + RCLRet ret = ClockDelegates.native_rcl_clock_remove_jump_callback(Handle, callbackInternal); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_remove_jump_callback)}() failed."); + + return _registeredJumpCallbacks.Remove(callback); } } } diff --git a/rcldotnet/rcldotnet_clock.c b/rcldotnet/rcldotnet_clock.c index c58d67ec..f415f6d6 100644 --- a/rcldotnet/rcldotnet_clock.c +++ b/rcldotnet/rcldotnet_clock.c @@ -19,7 +19,7 @@ #include "rcldotnet_clock.h" -int32_t native_rcl_clock_get_now(void *clock_handle, int64_t *time_point) { +int32_t native_rcl_clock_get_now(void *clock_handle, rcl_time_point_value_t *time_point) { rcl_clock_t *clock = (rcl_clock_t *)clock_handle; rcl_time_point_value_t *time = (rcl_time_point_value_t *)time_point; @@ -38,7 +38,7 @@ int32_t native_rcl_disable_ros_time_override(void *clock_handle) { return rcl_disable_ros_time_override(clock); } -int32_t native_rcl_set_ros_time_override(void *clock_handle, int64_t time_point_value) { +int32_t native_rcl_set_ros_time_override(void *clock_handle, rcl_time_point_value_t time_point_value) { rcl_clock_t *clock = (rcl_clock_t *)clock_handle; rcl_time_point_value_t time_point_value_native = (rcl_time_point_value_t)time_point_value; diff --git a/rcldotnet/rcldotnet_clock.h b/rcldotnet/rcldotnet_clock.h index a1457562..6275b7d0 100644 --- a/rcldotnet/rcldotnet_clock.h +++ b/rcldotnet/rcldotnet_clock.h @@ -18,7 +18,7 @@ #include "rcldotnet_macros.h" RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_clock_get_now(void *clock_handle, int64_t *time_point); +int32_t RCLDOTNET_CDECL native_rcl_clock_get_now(void *clock_handle, rcl_time_point_value_t *time_point); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_enable_ros_time_override(void *clock_handle); @@ -27,7 +27,7 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_disable_ros_time_override(void *clock_handle); RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_set_ros_time_override(void *clock_handle, int64_t time_point_value); +int32_t RCLDOTNET_CDECL native_rcl_set_ros_time_override(void *clock_handle, rcl_time_point_value_t time_point_value); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_clock_add_jump_callback(void *clock_handle, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback); From 47ad31a5305cbe039f78f72d00afe2ab197352f2 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Fri, 29 Sep 2023 15:10:20 +1000 Subject: [PATCH 08/20] Started work on integrating rcl_timer. --- rcldotnet/CMakeLists.txt | 2 + rcldotnet/Node.cs | 145 ++++++++------------------ rcldotnet/RCLdotnet.cs | 50 ++++++--- rcldotnet/SafeTimerHandle.cs | 41 ++++++++ rcldotnet/Timer.cs | 29 ++++++ rcldotnet/rcldotnet.c | 28 +++++ rcldotnet/rcldotnet.h | 9 ++ rcldotnet_examples/RCLDotnetTalker.cs | 39 ++++--- 8 files changed, 212 insertions(+), 131 deletions(-) create mode 100644 rcldotnet/SafeTimerHandle.cs create mode 100644 rcldotnet/Timer.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 3d80568d..8becb9c6 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -61,10 +61,12 @@ set(CS_SOURCES SafeRequestIdHandle.cs SafeServiceHandle.cs SafeSubscriptionHandle.cs + SafeTimerHandle.cs SafeWaitSetHandle.cs Service.cs ServiceDefinitionStaticMemberCache.cs Subscription.cs + Timer.cs ) find_package(rcldotnet_common REQUIRED) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 90c928d0..6bdde778 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -107,105 +107,21 @@ static NodeDelegates() _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); - IntPtr native_rcl_create_publisher_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_create_publisher_handle"); - - NodeDelegates.native_rcl_create_publisher_handle = - (NativeRCLCreatePublisherHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_create_publisher_handle_ptr, typeof(NativeRCLCreatePublisherHandleType)); - - IntPtr native_rcl_destroy_publisher_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_destroy_publisher_handle"); - - NodeDelegates.native_rcl_destroy_publisher_handle = - (NativeRCLDestroyPublisherHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_destroy_publisher_handle_ptr, typeof(NativeRCLDestroyPublisherHandleType)); - - IntPtr native_rcl_create_subscription_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_create_subscription_handle"); - - NodeDelegates.native_rcl_create_subscription_handle = - (NativeRCLCreateSubscriptionHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_create_subscription_handle_ptr, typeof(NativeRCLCreateSubscriptionHandleType)); - - IntPtr native_rcl_destroy_subscription_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_destroy_subscription_handle"); - - NodeDelegates.native_rcl_destroy_subscription_handle = - (NativeRCLDestroySubscriptionHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_destroy_subscription_handle_ptr, typeof(NativeRCLDestroySubscriptionHandleType)); - - IntPtr native_rcl_create_service_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_create_service_handle"); - - NodeDelegates.native_rcl_create_service_handle = - (NativeRCLCreateServiceHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_create_service_handle_ptr, typeof(NativeRCLCreateServiceHandleType)); - - IntPtr native_rcl_destroy_service_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_destroy_service_handle"); - - NodeDelegates.native_rcl_destroy_service_handle = - (NativeRCLDestroyServiceHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_destroy_service_handle_ptr, typeof(NativeRCLDestroyServiceHandleType)); - - IntPtr native_rcl_create_client_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_create_client_handle"); - - NodeDelegates.native_rcl_create_client_handle = - (NativeRCLCreateClientHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_create_client_handle_ptr, typeof(NativeRCLCreateClientHandleType)); - - IntPtr native_rcl_destroy_client_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_destroy_client_handle"); - - NodeDelegates.native_rcl_destroy_client_handle = - (NativeRCLDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_destroy_client_handle_ptr, typeof(NativeRCLDestroyClientHandleType)); - - IntPtr native_rcl_action_create_client_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_action_create_client_handle"); - - NodeDelegates.native_rcl_action_create_client_handle = - (NativeRCLActionCreateClientHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_action_create_client_handle_ptr, typeof(NativeRCLActionCreateClientHandleType)); - - IntPtr native_rcl_action_destroy_client_handle_ptr = _dllLoadUtils.GetProcAddress( - nativeLibrary, "native_rcl_action_destroy_client_handle"); - - NodeDelegates.native_rcl_action_destroy_client_handle = - (NativeRCLActionDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_action_destroy_client_handle_ptr, typeof(NativeRCLActionDestroyClientHandleType)); - - IntPtr native_rcl_action_create_server_handle_ptr = - _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_create_server_handle"); - NodeDelegates.native_rcl_action_create_server_handle = - (NativeRCLActionCreateServerHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_action_create_server_handle_ptr, typeof(NativeRCLActionCreateServerHandleType)); - - IntPtr native_rcl_action_destroy_server_handle_ptr = - _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_destroy_server_handle"); - 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)); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_publisher_handle), out native_rcl_create_publisher_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_publisher_handle), out native_rcl_destroy_publisher_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_subscription_handle), out native_rcl_create_subscription_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_subscription_handle), out native_rcl_destroy_subscription_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_service_handle), out native_rcl_create_service_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_service_handle), out native_rcl_destroy_service_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_client_handle), out native_rcl_create_client_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_client_handle), out native_rcl_destroy_client_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_action_create_client_handle), out native_rcl_action_create_client_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_action_destroy_client_handle), out native_rcl_action_destroy_client_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_action_create_server_handle), out native_rcl_action_create_server_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_action_destroy_server_handle), out native_rcl_action_destroy_server_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_node_get_name_handle), out native_rcl_node_get_name_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_node_get_namespace_handle), out native_rcl_node_get_namespace_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_node_get_fully_qualified_name_handle), out native_rcl_node_get_fully_qualified_name_handle); } } @@ -225,6 +141,8 @@ public sealed class Node private readonly IList _actionServers; + private readonly IList _timers; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -237,6 +155,7 @@ internal Node(SafeNodeHandle handle) _guardConditions = new List(); _actionClients = new List(); _actionServers = new List(); + _timers = new List(); } public string Name => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_name_handle, Handle); @@ -262,6 +181,8 @@ internal Node(SafeNodeHandle handle) public IList ActionServers => _actionServers; + public IList Timers => _timers; + // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its // internal handle is disposed. @@ -382,6 +303,32 @@ public Client CreateClient { callback?.Invoke(); }); + + if (ret != RCLRet.Ok) + { + timerHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_timer_handle)}() failed."); + } + + var timer = new Timer(timerHandle); + _timers.Add(timer); + return timer; + } + + public bool DestroyTimer(Timer timer) + { + if (!_timers.Contains(timer)) return false; + + _timers.Remove(timer); + timer.Handle.Dispose(); + return true; + } + public GuardCondition CreateGuardCondition(Action callback) { var guardConditionHandle = new SafeGuardConditionHandle(); diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index e0273fdd..bb3bf1fc 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -109,6 +109,11 @@ internal delegate RCLRet NativeRCLCreateWaitSetHandleType( internal static NativeRCLWaitSetAddClientType native_rcl_wait_set_add_client = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLWaitSetAddTimerType(SafeWaitSetHandle waitSetHandle, SafeTimerHandle timerHandle); + + internal static NativeRCLWaitSetAddTimerType native_rcl_wait_set_add_timer = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLWaitSetAddGuardConditionType(SafeWaitSetHandle waitSetHandle, SafeGuardConditionHandle guardConditionHandle); @@ -355,16 +360,23 @@ internal delegate RCLRet NativeRCLWriteToQosProfileHandleType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr NativeRCLGetStringType(SafeHandle handle); - internal delegate RCLRet NativeRCLCreateClockHandleType( - ref SafeClockHandle clockHandles, int clockType); + internal delegate RCLRet NativeRCLCreateClockHandleType(ref SafeClockHandle clockHandle, int clockType); internal static NativeRCLCreateClockHandleType native_rcl_create_clock_handle = null; - internal delegate RCLRet NativeRCLDestroyClockHandleType( - IntPtr clockHandle); + internal delegate RCLRet NativeRCLDestroyClockHandleType(IntPtr clockHandle); internal static NativeRCLDestroyClockHandleType native_rcl_destroy_clock_handle = null; + internal delegate RCLRet NativeRCLCreateTimerHandleType( + ref SafeTimerHandle timerHandle, SafeClockHandle clockHandle, Duration period, TimerCallback callback); + + internal static NativeRCLCreateTimerHandleType native_rcl_create_timer_handle = null; + + internal delegate RCLRet NativeRCLDestroyTimerHandleType(IntPtr timerHandle); + + internal static NativeRCLDestroyTimerHandleType native_rcl_destroy_timer_handle = null; + static RCLdotnetDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -460,6 +472,8 @@ static RCLdotnetDelegates() (NativeRCLWaitSetAddClientType)Marshal.GetDelegateForFunctionPointer( native_rcl_wait_set_add_client_ptr, typeof(NativeRCLWaitSetAddClientType)); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_wait_set_add_timer), out native_rcl_wait_set_add_timer); + IntPtr native_rcl_wait_set_add_guard_condition_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_add_guard_condition"); RCLdotnetDelegates.native_rcl_wait_set_add_guard_condition = @@ -718,17 +732,10 @@ static RCLdotnetDelegates() (NativeRCLWriteToQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_write_to_qos_profile_handle_ptr, typeof(NativeRCLWriteToQosProfileHandleType)); - IntPtr native_rcl_create_clock_handle_ptr = - _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_clock_handle"); - RCLdotnetDelegates.native_rcl_create_clock_handle = - (NativeRCLCreateClockHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_create_clock_handle_ptr, typeof(NativeRCLCreateClockHandleType)); - - IntPtr native_rcl_destroy_clock_handle_ptr = - _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_destroy_clock_handle"); - RCLdotnetDelegates.native_rcl_destroy_clock_handle = - (NativeRCLDestroyClockHandleType)Marshal.GetDelegateForFunctionPointer( - native_rcl_destroy_clock_handle_ptr, typeof(NativeRCLDestroyClockHandleType)); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_clock_handle), out native_rcl_create_clock_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_clock_handle), out native_rcl_destroy_clock_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_timer_handle), out native_rcl_create_timer_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_timer_handle), out native_rcl_destroy_timer_handle); } } @@ -828,6 +835,12 @@ private static void WaitSetAddClient(SafeWaitSetHandle waitSetHandle, SafeClient RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_client)}() failed."); } + private static void WaitSetAddTimer(SafeWaitSetHandle waitSetHandle, SafeTimerHandle timerHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_timer(waitSetHandle, timerHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_timer)}() failed."); + } + private static void WaitSetAddGuardCondition(SafeWaitSetHandle waitSetHandle, SafeGuardConditionHandle guardConditionHandle) { RCLRet ret = RCLdotnetDelegates.native_rcl_wait_set_add_guard_condition(waitSetHandle, guardConditionHandle); @@ -1122,7 +1135,7 @@ public static void SpinOnce(Node node, long timeout) { int numberOfSubscriptions = node.Subscriptions.Count; int numberOfGuardConditions = node.GuardConditions.Count; - int numberOfTimers = 0; + int numberOfTimers = node.Timers.Count; int numberOfClients = node.Clients.Count; int numberOfServices = node.Services.Count; int numberOfEvents = 0; @@ -1220,6 +1233,11 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddActionServer(waitSetHandle, actionServer.Handle); } + foreach (var timer in node.Timers) + { + WaitSetAddTimer(waitSetHandle, timer.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { diff --git a/rcldotnet/SafeTimerHandle.cs b/rcldotnet/SafeTimerHandle.cs new file mode 100644 index 00000000..981fdf1d --- /dev/null +++ b/rcldotnet/SafeTimerHandle.cs @@ -0,0 +1,41 @@ +/* Copyright 2023 Queensland University of Technology. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_timer_t + /// + internal sealed class SafeTimerHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeTimerHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_destroy_timer_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/Timer.cs b/rcldotnet/Timer.cs new file mode 100644 index 00000000..b2e28d49 --- /dev/null +++ b/rcldotnet/Timer.cs @@ -0,0 +1,29 @@ +/* Copyright 2023 Queensland University of Technology. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +namespace ROS2 +{ + internal delegate void TimerCallback(SafeTimerHandle timer, Duration elapsed); + + public class Timer + { + internal Timer(SafeTimerHandle handle) + { + Handle = handle; + } + + internal SafeTimerHandle Handle { get; } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 1346b784..dc2b2c10 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -187,6 +187,14 @@ int32_t native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handl return ret; } +int32_t native_rcl_wait_set_add_timer(void *wait_set_handle, void *timer_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_timer_t *timer = (rcl_timer_t *)timer_handle; + rcl_ret_t ret = rcl_wait_set_add_timer(wait_set, timer, NULL); + + return ret; +} + int32_t native_rcl_wait_set_add_guard_condition(void *wait_set_handle, void *guard_condition_handle) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_guard_condition_t *guard_condition = (rcl_guard_condition_t *)guard_condition_handle; @@ -741,3 +749,23 @@ int32_t native_rcl_write_to_qos_profile_handle( return RCL_RET_OK; } + +int32_t native_rcl_create_timer_handle(void **timer_handle, void *clock_handle, int64_t period, rcl_timer_callback_t callback) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_timer_t *timer = malloc(sizeof(rcl_timer_t)); + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; + + rcl_ret_t ret = rcl_timer_init(timer, clock, &context, period, callback, allocator); + + *timer_handle = (void *)timer; + return ret; +} + +int32_t native_rcl_destroy_timer_handle(void *timer_handle) { + rcl_timer_t *timer = (rcl_timer_t *)timer_handle; + + rcl_ret_t ret = rcl_timer_fini(timer); + free(timer); + + return ret; +} diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 4e8f7625..02335cd8 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -75,6 +75,9 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_service(void *wait_set_handle, v RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_client(void *wait_set_handle, void *client_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_timer(void *wait_set_handle, void *timer_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_guard_condition_handle(void *wait_set_handle, void *guard_condition_handle); @@ -242,4 +245,10 @@ int32_t RCLDOTNET_CDECL native_rcl_write_to_qos_profile_handle( uint64_t liveliness_lease_duration_nsec, int32_t /* bool */ avoid_ros_namespace_conventions); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_create_timer_handle(void **timer_handle, void *clock_handle, int64_t period, rcl_timer_callback_t callback); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_destroy_timer_handle(void *timer_handle); + #endif // RCLDOTNET_H diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index 89de127c..11cc59dc 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -1,34 +1,41 @@ using System; -using System.Threading; - using ROS2; namespace ConsoleApplication { public class RCLDotnetTalker { - public static void Main(string[] args) + private readonly Node node; + private readonly Publisher chatterPub; + + private int i = 0; + std_msgs.msg.String msg = new(); + + private RCLDotnetTalker() { RCLdotnet.Init(); + node = RCLdotnet.CreateNode("talker"); - Node node = RCLdotnet.CreateNode("talker"); + chatterPub = node.CreatePublisher("chatter"); - Publisher chatterPub = node.CreatePublisher("chatter"); + ROS2.Timer timer = node.CreateTimer(new Duration(1.0), PublishChatter); + } - std_msgs.msg.String msg = new std_msgs.msg.String(); + private void PublishChatter() + { + msg.Data = $"Hello World: {i}"; + Console.WriteLine($"Publishing: \"{msg.Data}\""); + chatterPub.Publish(msg); - int i = 1; + i++; + } - while (RCLdotnet.Ok()) - { - msg.Data = "Hello World: " + i; - i++; - Console.WriteLine("Publishing: \"" + msg.Data + "\""); - chatterPub.Publish(msg); + private void Spin() => RCLdotnet.Spin(node); - // Sleep a little bit between each message - Thread.Sleep(1000); - } + public static void Main(string[] args) + { + RCLDotnetTalker talker = new RCLDotnetTalker(); + talker.Spin(); } } } From eb3a2194d6a5653b8da1b6c24853f40d0c257242 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Fri, 29 Sep 2023 15:10:20 +1000 Subject: [PATCH 09/20] Updating RCLDotnetTalker to use a timer --- rcldotnet_examples/RCLDotnetTalker.cs | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index 11cc59dc..7cf6a683 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -5,32 +5,33 @@ namespace ConsoleApplication { public class RCLDotnetTalker { - private readonly Node node; - private readonly Publisher chatterPub; + private readonly Node _node; + private readonly Publisher _chatterPub; + private readonly Timer _timer; - private int i = 0; - std_msgs.msg.String msg = new(); + private int _i = 0; + std_msgs.msg.String _msg = new(); private RCLDotnetTalker() { RCLdotnet.Init(); - node = RCLdotnet.CreateNode("talker"); + _node = RCLdotnet.CreateNode("talker"); - chatterPub = node.CreatePublisher("chatter"); + _chatterPub = _node.CreatePublisher("chatter"); - ROS2.Timer timer = node.CreateTimer(new Duration(1.0), PublishChatter); + _timer = _node.CreateTimer(new Duration(1.0), PublishChatter); } private void PublishChatter() { - msg.Data = $"Hello World: {i}"; - Console.WriteLine($"Publishing: \"{msg.Data}\""); - chatterPub.Publish(msg); + _msg.Data = $"Hello World: {_i}"; + Console.WriteLine($"Publishing: \"{_msg.Data}\""); + _chatterPub.Publish(_msg); - i++; + _i++; } - private void Spin() => RCLdotnet.Spin(node); + private void Spin() => RCLdotnet.Spin(_node); public static void Main(string[] args) { From ee4a8fbc5ac4631cb28e80049d63a5033ab81b5f Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Tue, 3 Oct 2023 12:53:52 +1000 Subject: [PATCH 10/20] Finished integration of timers. --- rcldotnet/CMakeLists.txt | 1 + rcldotnet/Node.cs | 7 ++++- rcldotnet/RCLdotnet.cs | 58 +++++++++++++++++++++++-------------- rcldotnet/Timer.cs | 31 ++++++++++++++++++-- rcldotnet/rcldotnet.c | 12 ++++++++ rcldotnet/rcldotnet.h | 3 ++ rcldotnet/rcldotnet_timer.c | 26 +++++++++++++++++ rcldotnet/rcldotnet_timer.h | 23 +++++++++++++++ 8 files changed, 137 insertions(+), 24 deletions(-) create mode 100644 rcldotnet/rcldotnet_timer.c create mode 100644 rcldotnet/rcldotnet_timer.h diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 8becb9c6..766db758 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -95,6 +95,7 @@ add_library(${PROJECT_NAME}_native SHARED rcldotnet_guard_condition.c rcldotnet_node.c rcldotnet_publisher.c + rcldotnet_timer.c rcldotnet.c ) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 6bdde778..b7afac81 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -307,7 +307,12 @@ public Timer CreateTimer(Duration period, Action callback) { var timerHandle = new SafeTimerHandle(); - RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref timerHandle, Clock.Handle, period, (handle, elapsed) => { callback?.Invoke(); }); + TimerCallback callbackInternal = (handle, elapsed) => + { + callback?.Invoke(); + }; + + RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref timerHandle, Clock.Handle, period, callbackInternal); if (ret != RCLRet.Ok) { diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index bb3bf1fc..b44793b7 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -125,24 +125,17 @@ internal delegate RCLRet NativeRCLCreateWaitSetHandleType( internal static NativeRCLWaitType native_rcl_wait = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitSetSubscriptionReady(SafeWaitSetHandle waitSetHandle, int index); + internal delegate int NativeRCLWaitSetReady(SafeWaitSetHandle waitSetHandle, int index); - internal static NativeRCLWaitSetSubscriptionReady native_rcl_wait_set_subscription_ready = null; + internal static NativeRCLWaitSetReady native_rcl_wait_set_subscription_ready = null; - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitSetClientReady(SafeWaitSetHandle waitSetHandle, int index); - - internal static NativeRCLWaitSetClientReady native_rcl_wait_set_client_ready = null; + internal static NativeRCLWaitSetReady native_rcl_wait_set_client_ready = null; - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitSetServiceReady(SafeWaitSetHandle waitSetHandle, int index); + internal static NativeRCLWaitSetReady native_rcl_wait_set_timer_ready = null; - internal static NativeRCLWaitSetServiceReady native_rcl_wait_set_service_ready = null; - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate int NativeRCLWaitSetGuardConditionReady(SafeWaitSetHandle waitSetHandle, int index); + internal static NativeRCLWaitSetReady native_rcl_wait_set_service_ready = null; - internal static NativeRCLWaitSetGuardConditionReady native_rcl_wait_set_guard_condition_ready = null; + internal static NativeRCLWaitSetReady native_rcl_wait_set_guard_condition_ready = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLTakeType(SafeSubscriptionHandle subscriptionHandle, SafeHandle messageHandle); @@ -360,19 +353,23 @@ internal delegate RCLRet NativeRCLWriteToQosProfileHandleType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate IntPtr NativeRCLGetStringType(SafeHandle handle); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreateClockHandleType(ref SafeClockHandle clockHandle, int clockType); internal static NativeRCLCreateClockHandleType native_rcl_create_clock_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLDestroyClockHandleType(IntPtr clockHandle); internal static NativeRCLDestroyClockHandleType native_rcl_destroy_clock_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreateTimerHandleType( ref SafeTimerHandle timerHandle, SafeClockHandle clockHandle, Duration period, TimerCallback callback); internal static NativeRCLCreateTimerHandleType native_rcl_create_timer_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLDestroyTimerHandleType(IntPtr timerHandle); internal static NativeRCLDestroyTimerHandleType native_rcl_destroy_timer_handle = null; @@ -489,26 +486,26 @@ static RCLdotnetDelegates() IntPtr native_rcl_wait_set_subscription_ready_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_subscription_ready"); RCLdotnetDelegates.native_rcl_wait_set_subscription_ready = - (NativeRCLWaitSetSubscriptionReady)Marshal.GetDelegateForFunctionPointer( - native_rcl_wait_set_subscription_ready_ptr, typeof(NativeRCLWaitSetSubscriptionReady)); + (NativeRCLWaitSetReady)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_subscription_ready_ptr, typeof(NativeRCLWaitSetReady)); IntPtr native_rcl_wait_set_client_ready_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_client_ready"); RCLdotnetDelegates.native_rcl_wait_set_client_ready = - (NativeRCLWaitSetClientReady)Marshal.GetDelegateForFunctionPointer( - native_rcl_wait_set_client_ready_ptr, typeof(NativeRCLWaitSetClientReady)); + (NativeRCLWaitSetReady)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_client_ready_ptr, typeof(NativeRCLWaitSetReady)); IntPtr native_rcl_wait_set_service_ready_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_service_ready"); RCLdotnetDelegates.native_rcl_wait_set_service_ready = - (NativeRCLWaitSetServiceReady)Marshal.GetDelegateForFunctionPointer( - native_rcl_wait_set_service_ready_ptr, typeof(NativeRCLWaitSetServiceReady)); + (NativeRCLWaitSetReady)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_service_ready_ptr, typeof(NativeRCLWaitSetReady)); IntPtr native_rcl_wait_set_guard_condition_ready_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_wait_set_guard_condition_ready"); RCLdotnetDelegates.native_rcl_wait_set_guard_condition_ready = - (NativeRCLWaitSetGuardConditionReady)Marshal.GetDelegateForFunctionPointer( - native_rcl_wait_set_guard_condition_ready_ptr, typeof(NativeRCLWaitSetGuardConditionReady)); + (NativeRCLWaitSetReady)Marshal.GetDelegateForFunctionPointer( + native_rcl_wait_set_guard_condition_ready_ptr, typeof(NativeRCLWaitSetReady)); IntPtr native_rcl_take_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_take"); @@ -736,6 +733,7 @@ static RCLdotnetDelegates() _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_clock_handle), out native_rcl_destroy_clock_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_timer_handle), out native_rcl_create_timer_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_timer_handle), out native_rcl_destroy_timer_handle); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_wait_set_timer_ready), out native_rcl_wait_set_timer_ready); } } @@ -1301,6 +1299,24 @@ public static void SpinOnce(Node node, long timeout) clientIndex++; } + int timerIndex = 0; + foreach (var timer in node.Timers) + { + if (RCLdotnetDelegates.native_rcl_wait_set_timer_ready(waitSetHandle, timerIndex) != 0) + { + try + { + timer.Call(); + } + catch + { + // ignored + } + } + + timerIndex++; + } + foreach (var actionClient in node.ActionClients) { RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready( diff --git a/rcldotnet/Timer.cs b/rcldotnet/Timer.cs index b2e28d49..c3969e37 100644 --- a/rcldotnet/Timer.cs +++ b/rcldotnet/Timer.cs @@ -13,11 +13,32 @@ * limitations under the License. */ +using System; +using System.Runtime.InteropServices; +using ROS2.Utils; + namespace ROS2 { - internal delegate void TimerCallback(SafeTimerHandle timer, Duration elapsed); + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void TimerCallback(IntPtr timer, Duration elapsed); + + internal static class TimerDelegates + { + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLTimerFunctionType(SafeTimerHandle timerHandle); + + internal static NativeRCLTimerFunctionType native_rcl_timer_call = null; + + static TimerDelegates() + { + var dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = dllLoadUtils.LoadLibrary("rcldotnet"); - public class Timer + dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_timer_call), out native_rcl_timer_call); + } + } + + public sealed class Timer { internal Timer(SafeTimerHandle handle) { @@ -25,5 +46,11 @@ internal Timer(SafeTimerHandle handle) } internal SafeTimerHandle Handle { get; } + + internal void Call() + { + RCLRet ret = TimerDelegates.native_rcl_timer_call(Handle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(TimerDelegates.native_rcl_timer_call)}() failed."); + } } } diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index dc2b2c10..a10f2292 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -316,6 +316,18 @@ int32_t native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index) { return result ? 1 : 0; } +int32_t native_rcl_wait_set_timer_ready(void *wait_set_handle, int32_t index) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + + if (index >= wait_set->size_of_timers) + { + return false; + } + + bool result = wait_set->timers[index] != NULL; + return result ? 1 : 0; +} + int32_t native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 02335cd8..7c293704 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -114,6 +114,9 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_subscription_ready(void *wait_set_ha RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_client_ready(void *wait_set_handle, int32_t index); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_wait_set_timer_ready(void *wait_set_handle, int32_t index); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, int32_t index); diff --git a/rcldotnet/rcldotnet_timer.c b/rcldotnet/rcldotnet_timer.c new file mode 100644 index 00000000..a52c393f --- /dev/null +++ b/rcldotnet/rcldotnet_timer.c @@ -0,0 +1,26 @@ +// Copyright 2023 Queensland University of Technology. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include + +#include "rcldotnet_timer.h" + +int32_t native_rcl_timer_call(void *timer_handle) { + rcl_timer_t *timer = (rcl_timer_t *)timer_handle; + + return rcl_timer_call(timer); +} diff --git a/rcldotnet/rcldotnet_timer.h b/rcldotnet/rcldotnet_timer.h new file mode 100644 index 00000000..beaaeab2 --- /dev/null +++ b/rcldotnet/rcldotnet_timer.h @@ -0,0 +1,23 @@ +// Copyright 2023 Queensland University of Technology. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLDOTNET_TIMER_H +#define RCLDOTNET_TIMER_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_timer_call(void *timer_handle); + +#endif // RCLDOTNET_TIMER_H From fa205e6148c60c36655a13f76d2fb0197bbceec6 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 5 Oct 2023 12:01:20 +1000 Subject: [PATCH 11/20] Adding missing dependency to package.xml --- rcldotnet/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index 88be5f0f..df264e07 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -19,6 +19,7 @@ rmw_implementation_cmake rcl + rcl_interfaces rcl_action rmw action_msgs @@ -26,6 +27,7 @@ unique_identifier_msgs rcl + rcl_interfaces rcl_action action_msgs builtin_interfaces From 6218c854ff1480d46d6eb749c37158a581d9d304 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 5 Oct 2023 16:28:37 +1000 Subject: [PATCH 12/20] Zero initialising timer. --- rcldotnet/rcldotnet.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index a10f2292..40be6421 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -765,6 +765,8 @@ int32_t native_rcl_write_to_qos_profile_handle( int32_t native_rcl_create_timer_handle(void **timer_handle, void *clock_handle, int64_t period, rcl_timer_callback_t callback) { rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_timer_t *timer = malloc(sizeof(rcl_timer_t)); + *timer = rcl_get_zero_initialized_timer(); + rcl_clock_t *clock = (rcl_clock_t *)clock_handle; rcl_ret_t ret = rcl_timer_init(timer, clock, &context, period, callback, allocator); From 44871009616680992989925bae8ae402e10f61f4 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 5 Oct 2023 16:29:04 +1000 Subject: [PATCH 13/20] Adding elapsed duration to timer callback. --- rcldotnet/Node.cs | 4 ++-- rcldotnet_examples/RCLDotnetTalker.cs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index b7afac81..7ad8cda1 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -303,13 +303,13 @@ public Client CreateClient callback) { var timerHandle = new SafeTimerHandle(); TimerCallback callbackInternal = (handle, elapsed) => { - callback?.Invoke(); + callback?.Invoke(elapsed); }; RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref timerHandle, Clock.Handle, period, callbackInternal); diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index 7cf6a683..f7927b3b 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -22,7 +22,7 @@ private RCLDotnetTalker() _timer = _node.CreateTimer(new Duration(1.0), PublishChatter); } - private void PublishChatter() + private void PublishChatter(Duration elapsed) { _msg.Data = $"Hello World: {_i}"; Console.WriteLine($"Publishing: \"{_msg.Data}\""); From 99b3b340b79c4a8b9d96aa7152cd717d77a53f50 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Fri, 6 Oct 2023 13:40:19 +1000 Subject: [PATCH 14/20] Addressing GC of timer delegate. --- rcldotnet/Node.cs | 8 +++----- rcldotnet/Timer.cs | 7 ++++++- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 7ad8cda1..422748a3 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -307,10 +307,7 @@ public Timer CreateTimer(Duration period, Action callback) { var timerHandle = new SafeTimerHandle(); - TimerCallback callbackInternal = (handle, elapsed) => - { - callback?.Invoke(elapsed); - }; + TimerCallback callbackInternal = (_, elapsed) => callback?.Invoke(elapsed); RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref timerHandle, Clock.Handle, period, callbackInternal); @@ -320,7 +317,8 @@ public Timer CreateTimer(Duration period, Action callback) throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_timer_handle)}() failed."); } - var timer = new Timer(timerHandle); + var timer = new Timer(timerHandle, callbackInternal); + _timers.Add(timer); return timer; } diff --git a/rcldotnet/Timer.cs b/rcldotnet/Timer.cs index c3969e37..0e0755c1 100644 --- a/rcldotnet/Timer.cs +++ b/rcldotnet/Timer.cs @@ -40,13 +40,18 @@ static TimerDelegates() public sealed class Timer { - internal Timer(SafeTimerHandle handle) + // The garbage collector will eventually try to clean up the delegate if nothing on the .NET side is holding on to it. + private readonly TimerCallback _callback; + + internal Timer(SafeTimerHandle handle, TimerCallback callback) { Handle = handle; + _callback = callback; } internal SafeTimerHandle Handle { get; } + internal void Call() { RCLRet ret = TimerDelegates.native_rcl_timer_call(Handle); From 433936d9d061cfbb5da2031f58eb65ff23aa3546 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 9 Oct 2023 10:55:09 +1000 Subject: [PATCH 15/20] Consolidating timer instantiation. --- rcldotnet/Node.cs | 15 +-------------- rcldotnet/Timer.cs | 25 +++++++++++++++++++++---- 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 422748a3..a346913b 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -305,20 +305,7 @@ public Client CreateClient callback) { - var timerHandle = new SafeTimerHandle(); - - TimerCallback callbackInternal = (_, elapsed) => callback?.Invoke(elapsed); - - RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref timerHandle, Clock.Handle, period, callbackInternal); - - if (ret != RCLRet.Ok) - { - timerHandle.Dispose(); - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_timer_handle)}() failed."); - } - - var timer = new Timer(timerHandle, callbackInternal); - + Timer timer = new Timer(Clock, period, callback); _timers.Add(timer); return timer; } diff --git a/rcldotnet/Timer.cs b/rcldotnet/Timer.cs index 0e0755c1..7c6f2303 100644 --- a/rcldotnet/Timer.cs +++ b/rcldotnet/Timer.cs @@ -40,22 +40,39 @@ static TimerDelegates() public sealed class Timer { + private readonly Action _callback; + // The garbage collector will eventually try to clean up the delegate if nothing on the .NET side is holding on to it. - private readonly TimerCallback _callback; + private readonly TimerCallback _internalCallback; - internal Timer(SafeTimerHandle handle, TimerCallback callback) + internal Timer(Clock clock, Duration period, Action callback) { - Handle = handle; _callback = callback; + + SafeTimerHandle handle = new SafeTimerHandle(); + _internalCallback = OnTimer; + RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref handle, clock.Handle, period, _internalCallback); + + if (ret != RCLRet.Ok) + { + handle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_create_timer_handle)}() failed."); + } + + Handle = handle; } internal SafeTimerHandle Handle { get; } - internal void Call() { RCLRet ret = TimerDelegates.native_rcl_timer_call(Handle); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(TimerDelegates.native_rcl_timer_call)}() failed."); } + + private void OnTimer(IntPtr handle, Duration elapsed) + { + _callback?.Invoke(elapsed); + } } } From f1b4115f3177c4cc7ea5a5f1a302dc5ee57f73f2 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 9 Oct 2023 17:09:45 +1000 Subject: [PATCH 16/20] Using TimeSpan as user facing Duration --- rcldotnet/Clock.cs | 62 +++++++++++++++------------ rcldotnet/Node.cs | 2 +- rcldotnet/Timer.cs | 8 ++-- rcldotnet_examples/RCLDotnetTalker.cs | 7 ++- 4 files changed, 43 insertions(+), 36 deletions(-) diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index a257eddf..331eab02 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -23,7 +23,10 @@ namespace ROS2 { public static class TimeConstants { - public const long SECONDS_TO_NANOSECONDS = 1000L * 1000L * 1000L; + public const long NanosecondsPerSecond = 1000L * 1000L * 1000L; + public const long NanosecondsPerMillisecond = 1000000L; + + public const long NanosecondsPerTimespanTick = NanosecondsPerMillisecond / TimeSpan.TicksPerMillisecond; // ~= 100. } /// @@ -60,40 +63,58 @@ public enum ClockChange SystemTimeNoChange = 4 } + // Internal as TimeSpan should be user-facing. + [StructLayout(LayoutKind.Sequential)] + internal struct Duration + { + public long nanoseconds; + + public TimeSpan AsTimespan => new TimeSpan(nanoseconds / TimeConstants.NanosecondsPerTimespanTick); + + public Duration(TimeSpan timeSpan) + { + nanoseconds = (long)(timeSpan.TotalMilliseconds * TimeConstants.NanosecondsPerMillisecond); + } + } + [StructLayout(LayoutKind.Sequential)] public struct TimeJump { public ClockChange clockChange; - public Duration delta; + internal Duration delta; + + public TimeSpan Delta => delta.AsTimespan; } internal delegate void JumpCallbackInternal(IntPtr timeJumpPtr, bool beforeJump); public delegate void JumpCallback(TimeJump timeJump, bool beforeJump); + [StructLayout(LayoutKind.Sequential)] public struct JumpThreshold { public bool onClockChange; - public Duration minForward; - public Duration minBackward; + internal Duration minForward; + internal Duration minBackward; - public JumpThreshold(bool onClockChange, double minForwardSeconds, double minBackwardSeconds) + public JumpThreshold(bool onClockChange, TimeSpan minForward, TimeSpan minBackward) { this.onClockChange = onClockChange; - minForward = new Duration(minForwardSeconds); - minBackward = new Duration(minBackwardSeconds); + this.minForward = new Duration(minForward); + this.minBackward = new Duration(minBackward); } } - public struct TimePoint + [StructLayout(LayoutKind.Sequential)] + internal struct TimePoint { - public long nanoseconds; + internal long nanoseconds; - public Time ToMsg() + internal Time ToMsg() { - long sec = nanoseconds / TimeConstants.SECONDS_TO_NANOSECONDS; - long nanosec = nanoseconds - (sec * TimeConstants.SECONDS_TO_NANOSECONDS); + long sec = nanoseconds / TimeConstants.NanosecondsPerSecond; + long nanosec = nanoseconds - (sec * TimeConstants.NanosecondsPerSecond); return new Time { Sec = (int)sec, @@ -101,28 +122,15 @@ public Time ToMsg() }; } - public static TimePoint FromMsg(Time message) + internal static TimePoint FromMsg(Time message) { return new TimePoint { - nanoseconds = message.Sec * TimeConstants.SECONDS_TO_NANOSECONDS + message.Nanosec + nanoseconds = message.Sec * TimeConstants.NanosecondsPerSecond + message.Nanosec }; } } - [StructLayout(LayoutKind.Sequential)] - public struct Duration - { - public long nanoseconds; - - public Duration(double seconds) - { - nanoseconds = (long)(seconds * TimeConstants.SECONDS_TO_NANOSECONDS); - } - - public double Seconds => (double)nanoseconds / TimeConstants.SECONDS_TO_NANOSECONDS; - } - public class CallbackAlreadyRegisteredException : Exception { public CallbackAlreadyRegisteredException(string message) : base(message) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index a346913b..6506dfb4 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -303,7 +303,7 @@ public Client CreateClient callback) + public Timer CreateTimer(TimeSpan period, Action callback) { Timer timer = new Timer(Clock, period, callback); _timers.Add(timer); diff --git a/rcldotnet/Timer.cs b/rcldotnet/Timer.cs index 7c6f2303..27ff9c42 100644 --- a/rcldotnet/Timer.cs +++ b/rcldotnet/Timer.cs @@ -40,18 +40,18 @@ static TimerDelegates() public sealed class Timer { - private readonly Action _callback; + private readonly Action _callback; // The garbage collector will eventually try to clean up the delegate if nothing on the .NET side is holding on to it. private readonly TimerCallback _internalCallback; - internal Timer(Clock clock, Duration period, Action callback) + internal Timer(Clock clock, TimeSpan period, Action callback) { _callback = callback; SafeTimerHandle handle = new SafeTimerHandle(); _internalCallback = OnTimer; - RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref handle, clock.Handle, period, _internalCallback); + RCLRet ret = RCLdotnetDelegates.native_rcl_create_timer_handle(ref handle, clock.Handle, new Duration(period), _internalCallback); if (ret != RCLRet.Ok) { @@ -72,7 +72,7 @@ internal void Call() private void OnTimer(IntPtr handle, Duration elapsed) { - _callback?.Invoke(elapsed); + _callback?.Invoke(elapsed.AsTimespan); } } } diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index f7927b3b..a82e0950 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -7,10 +7,9 @@ public class RCLDotnetTalker { private readonly Node _node; private readonly Publisher _chatterPub; - private readonly Timer _timer; private int _i = 0; - std_msgs.msg.String _msg = new(); + private readonly std_msgs.msg.String _msg = new(); private RCLDotnetTalker() { @@ -19,10 +18,10 @@ private RCLDotnetTalker() _chatterPub = _node.CreatePublisher("chatter"); - _timer = _node.CreateTimer(new Duration(1.0), PublishChatter); + _node.CreateTimer(TimeSpan.FromSeconds(1.0), PublishChatter); } - private void PublishChatter(Duration elapsed) + private void PublishChatter(TimeSpan elapsed) { _msg.Data = $"Hello World: {_i}"; Console.WriteLine($"Publishing: \"{_msg.Data}\""); From fb9f8924ff939536afad37a7faf62a1bcd13fd7c Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Fri, 3 Nov 2023 11:43:35 +1000 Subject: [PATCH 17/20] Response to suggestions by @hoffmann-stefan --- rcldotnet/Clock.cs | 81 +++++++++++++++++++++++++++++++++------------- rcldotnet/Timer.cs | 2 +- 2 files changed, 59 insertions(+), 24 deletions(-) diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index 331eab02..72bfdc67 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -21,7 +21,7 @@ namespace ROS2 { - public static class TimeConstants + internal static class TimeConstants { public const long NanosecondsPerSecond = 1000L * 1000L * 1000L; public const long NanosecondsPerMillisecond = 1000000L; @@ -43,10 +43,9 @@ public static class TimeConstants /// public enum ClockType { - ClockUninitialized = 0, - ROSTime, - SystemTime, - SteadyTime + ROSTime = 1, + SystemTime = 2, + SteadyTime = 3 } /// Enumeration to describe the type of time jump. @@ -69,21 +68,31 @@ internal struct Duration { public long nanoseconds; - public TimeSpan AsTimespan => new TimeSpan(nanoseconds / TimeConstants.NanosecondsPerTimespanTick); - public Duration(TimeSpan timeSpan) { nanoseconds = (long)(timeSpan.TotalMilliseconds * TimeConstants.NanosecondsPerMillisecond); } + + public TimeSpan AsTimespan() + { + return new TimeSpan(nanoseconds / TimeConstants.NanosecondsPerTimespanTick); + } } [StructLayout(LayoutKind.Sequential)] - public struct TimeJump + public readonly struct TimeJump { - public ClockChange clockChange; - internal Duration delta; + private readonly ClockChange _clockChange; + private readonly Duration _delta; + + internal TimeJump(ClockChange clockChange, Duration delta) + { + _clockChange = clockChange; + _delta = delta; + } - public TimeSpan Delta => delta.AsTimespan; + public ClockChange ClockChange => _clockChange; + public TimeSpan Delta => _delta.AsTimespan(); } internal delegate void JumpCallbackInternal(IntPtr timeJumpPtr, bool beforeJump); @@ -91,18 +100,22 @@ public struct TimeJump public delegate void JumpCallback(TimeJump timeJump, bool beforeJump); [StructLayout(LayoutKind.Sequential)] - public struct JumpThreshold + public readonly struct JumpThreshold { - public bool onClockChange; - internal Duration minForward; - internal Duration minBackward; + internal readonly bool _onClockChange; + internal readonly Duration _minForward; + internal readonly Duration _minBackward; public JumpThreshold(bool onClockChange, TimeSpan minForward, TimeSpan minBackward) { - this.onClockChange = onClockChange; - this.minForward = new Duration(minForward); - this.minBackward = new Duration(minBackward); + _onClockChange = onClockChange; + _minForward = new Duration(minForward); + _minBackward = new Duration(minBackward); } + + public bool OnClockChange => _onClockChange; + public TimeSpan MinForward => _minForward.AsTimespan(); + public TimeSpan MinBackward => _minBackward.AsTimespan(); } [StructLayout(LayoutKind.Sequential)] @@ -187,6 +200,8 @@ public sealed class Clock { private readonly Dictionary _registeredJumpCallbacks = new Dictionary(); + private readonly object _lock = new object(); + internal Clock(SafeClockHandle handle) { Handle = handle; @@ -205,7 +220,11 @@ public Time Now() internal RCLRet EnableRosTimeOverride() { - RCLRet ret = ClockDelegates.native_rcl_enable_ros_time_override(Handle); + RCLRet ret; + lock (_lock) + { + ret = ClockDelegates.native_rcl_enable_ros_time_override(Handle); + } RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_enable_ros_time_override)}() failed."); @@ -214,7 +233,11 @@ internal RCLRet EnableRosTimeOverride() internal RCLRet DisableRosTimeOverride() { - RCLRet ret = ClockDelegates.native_rcl_disable_ros_time_override(Handle); + RCLRet ret; + lock (_lock) + { + ret = ClockDelegates.native_rcl_disable_ros_time_override(Handle); + } RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_disable_ros_time_override)}() failed."); @@ -223,7 +246,11 @@ internal RCLRet DisableRosTimeOverride() internal RCLRet SetRosTimeOverride(long timePointValue) { - RCLRet ret = ClockDelegates.native_rcl_set_ros_time_override(Handle, timePointValue); + RCLRet ret; + lock (_lock) + { + ret = ClockDelegates.native_rcl_set_ros_time_override(Handle, timePointValue); + } RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_set_ros_time_override)}() failed."); @@ -240,7 +267,11 @@ public void AddJumpCallback(JumpThreshold threshold, JumpCallback callback) JumpCallbackInternal callbackInternal = (timeJumpPtr, beforeJump) => callback(Marshal.PtrToStructure(timeJumpPtr), beforeJump); - RCLRet ret = ClockDelegates.native_rcl_clock_add_jump_callback(Handle, threshold, callbackInternal); + RCLRet ret; + lock (_lock) + { + ret = ClockDelegates.native_rcl_clock_add_jump_callback(Handle, threshold, callbackInternal); + } RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_add_jump_callback)}() failed."); @@ -251,7 +282,11 @@ public bool RemoveJumpCallback(JumpCallback callback) { if (!_registeredJumpCallbacks.TryGetValue(callback, out JumpCallbackInternal callbackInternal)) return false; - RCLRet ret = ClockDelegates.native_rcl_clock_remove_jump_callback(Handle, callbackInternal); + RCLRet ret; + lock (_lock) + { + ret = ClockDelegates.native_rcl_clock_remove_jump_callback(Handle, callbackInternal); + } RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClockDelegates.native_rcl_clock_remove_jump_callback)}() failed."); diff --git a/rcldotnet/Timer.cs b/rcldotnet/Timer.cs index 27ff9c42..5afde6c6 100644 --- a/rcldotnet/Timer.cs +++ b/rcldotnet/Timer.cs @@ -72,7 +72,7 @@ internal void Call() private void OnTimer(IntPtr handle, Duration elapsed) { - _callback?.Invoke(elapsed.AsTimespan); + _callback?.Invoke(elapsed.AsTimespan()); } } } From 4b272d24c542db49459d5c05ca6c85be61fed0a1 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Fri, 3 Nov 2023 14:28:16 +1000 Subject: [PATCH 18/20] Adjusting Duration initialisation --- rcldotnet/Clock.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index 72bfdc67..47165b05 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -64,18 +64,18 @@ public enum ClockChange // Internal as TimeSpan should be user-facing. [StructLayout(LayoutKind.Sequential)] - internal struct Duration + internal readonly struct Duration { - public long nanoseconds; + private readonly long _nanoseconds; public Duration(TimeSpan timeSpan) { - nanoseconds = (long)(timeSpan.TotalMilliseconds * TimeConstants.NanosecondsPerMillisecond); + _nanoseconds = checked(timeSpan.Ticks * TimeConstants.NanosecondsPerTimespanTick); } public TimeSpan AsTimespan() { - return new TimeSpan(nanoseconds / TimeConstants.NanosecondsPerTimespanTick); + return new TimeSpan(_nanoseconds / TimeConstants.NanosecondsPerTimespanTick); } } From 75f6cf2753e93d597c70896102890d02c04e2763 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Sun, 19 Nov 2023 14:07:21 +0100 Subject: [PATCH 19/20] refactor CreateJumpCallback to return a IDisposable --- rcldotnet/Clock.cs | 119 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 100 insertions(+), 19 deletions(-) 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. + } } } } From c46ca6d57e2dbe5e6eee3163c3ecebe89f22de06 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 20 Nov 2023 16:52:00 +1000 Subject: [PATCH 20/20] Response to suggestions by @hoffmann-stefan --- rcldotnet/Clock.cs | 2 +- rcldotnet/Node.cs | 2 +- rcldotnet/RCLdotnet.cs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rcldotnet/Clock.cs b/rcldotnet/Clock.cs index 56d032e7..e95ae0ea 100644 --- a/rcldotnet/Clock.cs +++ b/rcldotnet/Clock.cs @@ -43,7 +43,7 @@ internal static class TimeConstants /// public enum ClockType { - ROSTime = 1, + RosTime = 1, SystemTime = 2, SteadyTime = 3 } diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 6506dfb4..80b5549a 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -310,7 +310,7 @@ public Timer CreateTimer(TimeSpan period, Action callback) return timer; } - public bool DestroyTimer(Timer timer) + internal bool DestroyTimer(Timer timer) { if (!_timers.Contains(timer)) return false; diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index b44793b7..e52c03f5 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -774,7 +774,7 @@ public static void Spin(Node node) } } - public static Clock CreateClock(ClockType type = ClockType.ROSTime) + public static Clock CreateClock(ClockType type = ClockType.RosTime) { var clockHandle = new SafeClockHandle(); RCLRet ret = RCLdotnetDelegates.native_rcl_create_clock_handle(ref clockHandle, (int)type);