diff --git a/README.md b/README.md index 1fbe9fb3..c1aeeeef 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,7 @@ The current set of features include: - Generation of all builtin ROS types - Support for publishers and subscriptions - Support for clients and services +- Support action clients and servers - Cross-platform support (Linux, Windows, Windows IoT Core, UWP) What's missing? @@ -30,7 +31,6 @@ Lots of things! - Unicode types - String constants (specifically BoundedString) - Component nodes -- Actions - Tests - Documentation - More examples (e.g. IoT, VB, UWP, HoloLens, etc.) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs new file mode 100644 index 00000000..9f9e362e --- /dev/null +++ b/rcldotnet/ActionClient.cs @@ -0,0 +1,457 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Collections.Concurrent; +using System.Runtime.InteropServices; +using System.Threading.Tasks; +using action_msgs.msg; +using action_msgs.srv; +using builtin_interfaces.msg; +using ROS2.Utils; +using unique_identifier_msgs.msg; + +namespace ROS2 +{ + internal static class ActionClientDelegates + { + internal static readonly DllLoadUtils _dllLoadUtils; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendGoalRequestType( + SafeActionClientHandle clientHandle, SafeHandle goalRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendGoalRequestType native_rcl_action_send_goal_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendResultRequestType( + SafeActionClientHandle clientHandle, SafeHandle resultRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendResultRequestType native_rcl_action_send_result_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendCancelRequestType( + SafeActionClientHandle clientHandle, SafeHandle cancelRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendCancelRequestType native_rcl_action_send_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerIsAvailableType( + SafeNodeHandle nodeHandle, SafeActionClientHandle clientHandle, out int isAvailable); + + internal static NativeRCLActionServerIsAvailableType native_rcl_action_server_is_available = null; + + static ActionClientDelegates() + { + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); + + IntPtr native_rcl_action_send_goal_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_goal_request"); + ActionClientDelegates.native_rcl_action_send_goal_request = + (NativeRCLActionSendGoalRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_goal_request_ptr, typeof(NativeRCLActionSendGoalRequestType)); + + IntPtr native_rcl_action_send_result_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_result_request"); + ActionClientDelegates.native_rcl_action_send_result_request = + (NativeRCLActionSendResultRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_result_request_ptr, typeof(NativeRCLActionSendResultRequestType)); + + IntPtr native_rcl_action_send_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_cancel_request"); + ActionClientDelegates.native_rcl_action_send_cancel_request = + (NativeRCLActionSendCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_cancel_request_ptr, typeof(NativeRCLActionSendCancelRequestType)); + + IntPtr native_rcl_action_server_is_available_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_is_available"); + ActionClientDelegates.native_rcl_action_server_is_available = + (NativeRCLActionServerIsAvailableType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_is_available_ptr, typeof(NativeRCLActionServerIsAvailableType)); + } + } + + public abstract class ActionClient + { + // Only allow internal subclasses. + internal ActionClient() + { + } + + // ActionClient does intentionally (for now) not implement IDisposable + // as this needs some extra consideration how the type works after its + // internal handle is disposed. By relying on the GC/Finalizer of + // SafeHandle the handle only gets Disposed if the action client is not + // live anymore. + internal abstract SafeActionClientHandle Handle { get; } + + public abstract bool ServerIsReady(); + + internal abstract IRosMessage CreateSendGoalResponse(); + + internal abstract SafeHandle CreateSendGoalResponseHandle(); + + internal abstract IRosMessage CreateGetResultResponse(); + + internal abstract SafeHandle CreateGetResultResponseHandle(); + + internal abstract IRosMessage CreateFeedbackMessage(); + + internal abstract SafeHandle CreateFeedbackMessageHandle(); + + internal abstract void HandleFeedbackMessage(IRosMessage feedbackMessage); + + internal abstract void HandleStatusMessage(GoalStatusArray statusMessage); + + internal abstract void HandleGoalResponse(long sequenceNumber, IRosMessage goalResponse); + + internal abstract void HandleCancelResponse(long sequenceNumber, CancelGoal_Response cancelResponse); + + internal abstract void HandleResultResponse(long sequenceNumber, IRosMessage resultResponse); + } + + public sealed class ActionClient : ActionClient + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // ros2_java uses a WeakReference here. Not sure if its needed or not. + private readonly Node _node; + + // TODO: (sh) rclpy uses week references to the goal handles here? Is this needed? + private readonly ConcurrentDictionary> _goalHandles + = new ConcurrentDictionary>(); + + private readonly ConcurrentDictionary _pendingGoalRequests + = new ConcurrentDictionary(); + + private readonly ConcurrentDictionary _pendingCancelRequests + = new ConcurrentDictionary(); + + private readonly ConcurrentDictionary _pendingResultRequests + = new ConcurrentDictionary(); + + // No public constructor. + internal ActionClient(SafeActionClientHandle handle, Node node) + { + Handle = handle; + _node = node; + } + + internal override SafeActionClientHandle Handle { get; } + + public override bool ServerIsReady() + { + RCLRet ret = ActionClientDelegates.native_rcl_action_server_is_available(_node.Handle, Handle, out int serverIsReady); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_server_is_available)}() failed."); + + return serverIsReady != 0; + } + + public Task> SendGoalAsync(TGoal goal) + { + return SendGoalAsyncInternal(goal, null); + } + + public Task> SendGoalAsync(TGoal goal, Action feedbackCallback) + { + return SendGoalAsyncInternal(goal, feedbackCallback); + } + + private Task> SendGoalAsyncInternal(TGoal goal, Action feedbackCallback) + { + long sequenceNumber; + + var goalId = Guid.NewGuid(); + var goalRequest = ActionDefinitionStaticMemberCache.CreateSendGoalRequest(); + goalRequest.GoalIdAsRosMessage = goalId.ToUuidMsg(); + goalRequest.Goal = goal; + + using (var goalRequestHandle = ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle()) + { + RCLdotnet.WriteToMessageHandle(goalRequest, goalRequestHandle); + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_goal_request(Handle, goalRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_goal_request)}() failed."); + } + + var taskCompletionSource = new TaskCompletionSource>(); + var pendingGoalRequest = new PendingGoalRequest(goalId, taskCompletionSource, feedbackCallback); + if (!_pendingGoalRequests.TryAdd(sequenceNumber, pendingGoalRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + return taskCompletionSource.Task; + } + + // TODO: (sh) Add CancelAllGoalsAsync(), like rclcpp Client.async_cancel_all_goals() + // TODO: (sh) Add CancelGoalsBeforeAsync(), like rclcpp Client.async_cancel_goals_before() + + // TODO: (sh) Maybe make this public. + // (but then check if goalHandle belongs to this actionClient is needed) + internal Task CancelGoalAsync(ActionClientGoalHandle goalHandle) + { + long sequenceNumber; + + var cancelRequest = new CancelGoal_Request(); + cancelRequest.GoalInfo.GoalId.Uuid = goalHandle.GoalId.ToUuidByteArray(); + + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(cancelRequest, cancelRequestHandle); + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_cancel_request(Handle, cancelRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_cancel_request)}() failed."); + } + + var taskCompletionSource = new TaskCompletionSource(); + var pendingCancelRequest = new PendingCancelRequest(taskCompletionSource); + if (!_pendingCancelRequests.TryAdd(sequenceNumber, pendingCancelRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + return taskCompletionSource.Task; + } + + internal Task GetResultAsync( + ActionClientGoalHandle goalHandle, + TaskCompletionSource resultTaskCompletionSource) + { + try + { + long sequenceNumber; + + var resultRequest = ActionDefinitionStaticMemberCache.CreateGetResultRequest(); + resultRequest.GoalIdAsRosMessage = goalHandle.GoalId.ToUuidMsg(); + + using (var resultRequestHandle = ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle()) + { + RCLdotnet.WriteToMessageHandle(resultRequest, resultRequestHandle); + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_result_request(Handle, resultRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_result_request)}() failed."); + } + + var pendingResultRequest = new PendingResultRequest(resultTaskCompletionSource); + if (!_pendingResultRequests.TryAdd(sequenceNumber, pendingResultRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + } + catch (Exception exception) + { + // Don't directly return Task.FromException() so that the tasks + // returned from other threads get the exception as well. + resultTaskCompletionSource.TrySetException(exception); + } + + return resultTaskCompletionSource.Task; + } + + internal override IRosMessage CreateSendGoalResponse() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponse(); + } + + internal override SafeHandle CreateSendGoalResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponseHandle(); + } + + internal override IRosMessage CreateGetResultResponse() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponse(); + } + + internal override SafeHandle CreateGetResultResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponseHandle(); + } + + internal override IRosMessage CreateFeedbackMessage() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessage(); + } + + internal override SafeHandle CreateFeedbackMessageHandle() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessageHandle(); + } + + internal override void HandleFeedbackMessage(IRosMessage feedbackMessage) + { + var feedbackMessageCasted = (IRosActionFeedbackMessage)feedbackMessage; + + var goalIdUuid = (UUID)feedbackMessageCasted.GoalIdAsRosMessage; + var goalId = goalIdUuid.ToGuid(); + + if (!_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + // TODO: (sh) Add error handling/logging. GoalId not found. + return; + } + + goalHandle.FeedbackCallback?.Invoke(feedbackMessageCasted.Feedback); + } + + internal override void HandleStatusMessage(GoalStatusArray statusMessage) + { + foreach (var statusMessageEntry in statusMessage.StatusList) + { + var goalId = statusMessageEntry.GoalInfo.GoalId.ToGuid(); + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + var status = statusMessageEntry.Status; + goalHandle.Status = ConvertGoalStatus(status); + + // rclpy does this, rclcpp does not. + if (status == GoalStatus.STATUS_SUCCEEDED + || status == GoalStatus.STATUS_CANCELED + || status == GoalStatus.STATUS_ABORTED) + { + // Remove "done" goals from the dictionary. + // TODO: (sh) Should we remove goalHandles that are not in the status list as well? After a timeout? + _goalHandles.TryRemove(goalId, out _); + } + } + } + } + + private static ActionGoalStatus ConvertGoalStatus(sbyte status) + { + switch (status) + { + case GoalStatus.STATUS_UNKNOWN: + return ActionGoalStatus.Unknown; + + case GoalStatus.STATUS_ACCEPTED: + return ActionGoalStatus.Accepted; + + case GoalStatus.STATUS_EXECUTING: + return ActionGoalStatus.Executing; + + case GoalStatus.STATUS_CANCELING: + return ActionGoalStatus.Canceling; + + case GoalStatus.STATUS_SUCCEEDED: + return ActionGoalStatus.Succeeded; + + case GoalStatus.STATUS_CANCELED: + return ActionGoalStatus.Canceled; + + case GoalStatus.STATUS_ABORTED: + return ActionGoalStatus.Aborted; + + default: + throw new ArgumentException("Unknown status.", nameof(status)); + } + } + + internal override void HandleGoalResponse(long sequenceNumber, IRosMessage goalResponse) + { + if (!_pendingGoalRequests.TryRemove(sequenceNumber, out var pendingGoalRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + var goalId = pendingGoalRequest.GoalId; + var goalResponseCasted = (IRosActionSendGoalResponse)goalResponse; + var accepted = goalResponseCasted.Accepted; + var stamp = (Time)goalResponseCasted.StampAsRosMessage; + + var goalHandle = new ActionClientGoalHandle(this, goalId, accepted, stamp, pendingGoalRequest.FeedbackCallback); + + if (accepted) + { + if (!_goalHandles.TryAdd(goalId, goalHandle)) + { + var exception = new Exception($"Two goals were accepted with the same Id '{goalId}'."); + pendingGoalRequest.TaskCompletionSource.TrySetException(exception); + return; + }; + } + + pendingGoalRequest.TaskCompletionSource.TrySetResult(goalHandle); + } + + internal override void HandleCancelResponse(long sequenceNumber, CancelGoal_Response cancelResponse) + { + if (!_pendingCancelRequests.TryRemove(sequenceNumber, out var pendingCancelRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + pendingCancelRequest.TaskCompletionSource.TrySetResult(cancelResponse); + } + + internal override void HandleResultResponse(long sequenceNumber, IRosMessage resultResponse) + { + if (!_pendingResultRequests.TryRemove(sequenceNumber, out var pendingResultRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + var resultResponseCasted = (IRosActionGetResultResponse)resultResponse; + var result = resultResponseCasted.Result; + + pendingResultRequest.TaskCompletionSource.TrySetResult(result); + } + + private sealed class PendingGoalRequest + { + public PendingGoalRequest( + Guid goalId, + TaskCompletionSource> taskCompletionSource, + Action feedbackCallback) + { + GoalId = goalId; + TaskCompletionSource = taskCompletionSource; + FeedbackCallback = feedbackCallback; + } + + public Guid GoalId { get; } + public TaskCompletionSource> TaskCompletionSource { get; } + public Action FeedbackCallback { get; } + } + + private sealed class PendingCancelRequest + { + public PendingCancelRequest(TaskCompletionSource taskCompletionSource) + { + TaskCompletionSource = taskCompletionSource; + } + + public TaskCompletionSource TaskCompletionSource { get; } + } + + private sealed class PendingResultRequest + { + public PendingResultRequest(TaskCompletionSource taskCompletionSource) + { + TaskCompletionSource = taskCompletionSource; + } + + public TaskCompletionSource TaskCompletionSource { get; } + } + } +} diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs new file mode 100644 index 00000000..34ca9baa --- /dev/null +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -0,0 +1,100 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading; +using System.Threading.Tasks; +using builtin_interfaces.msg; + +namespace ROS2 +{ + public abstract class ActionClientGoalHandle + { + // Only allow internal subclasses. + internal ActionClientGoalHandle() + { + } + + public abstract Guid GoalId { get; } + + public abstract bool Accepted { get; } + + public abstract Time Stamp { get; } + + public abstract ActionGoalStatus Status { get; internal set; } + } + + public sealed class ActionClientGoalHandle : ActionClientGoalHandle + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + private readonly ActionClient _actionClient; + + private TaskCompletionSource _resultTaskCompletionSource; + + // No public constructor. + internal ActionClientGoalHandle( + ActionClient actionClient, + Guid goalId, + bool accepted, + Time stamp, + Action feedbackCallback) + { + GoalId = goalId; + Accepted = accepted; + Stamp = stamp; + FeedbackCallback = feedbackCallback; + _actionClient = actionClient; + } + + public override Guid GoalId { get; } + + public override bool Accepted { get; } + + public override Time Stamp { get; } + + public override ActionGoalStatus Status { get; internal set; } + + internal Action FeedbackCallback { get; } + + // TODO: (sh) should we return the CancelGoal_Response? Wrap it in type with dotnet enum/Guid? + public Task CancelGoalAsync() + { + return _actionClient.CancelGoalAsync(this); + } + + public Task GetResultAsync() + { + // Fast case to avoid allocation and calling Interlocked.CompareExchange. + if (_resultTaskCompletionSource != null) + { + return _resultTaskCompletionSource.Task; + } + + var resultTaskCompletionSource = new TaskCompletionSource(); + + var oldResultTcs = Interlocked.CompareExchange(ref _resultTaskCompletionSource, resultTaskCompletionSource, null); + if (oldResultTcs != null) + { + // Some other thread was first. + return oldResultTcs.Task; + } + + return _actionClient.GetResultAsync(this, resultTaskCompletionSource); + } + } +} diff --git a/rcldotnet/ActionDefinitionStaticMemberCache.cs b/rcldotnet/ActionDefinitionStaticMemberCache.cs new file mode 100644 index 00000000..c07c3015 --- /dev/null +++ b/rcldotnet/ActionDefinitionStaticMemberCache.cs @@ -0,0 +1,259 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Reflection; +using System.Runtime.InteropServices; + +namespace ROS2 +{ + internal static class ActionDefinitionStaticMemberCache + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + private static readonly IntPtr s_typeSupport; + private static readonly Func> s_createSendGoalRequest; + private static readonly Func s_createSendGoalRequestHandle; + private static readonly Func s_createSendGoalResponse; + private static readonly Func s_createSendGoalResponseHandle; + private static readonly Func s_createGetResultRequest; + private static readonly Func s_createGetResultRequestHandle; + private static readonly Func> s_createGetResultResponse; + private static readonly Func s_createGetResultResponseHandle; + private static readonly Func> s_createFeedbackMessage; + private static readonly Func s_createFeedbackMessageHandle; + + static ActionDefinitionStaticMemberCache() + { + TypeInfo typeInfo = typeof(TAction).GetTypeInfo(); + + MethodInfo getTypeSupport = typeInfo.GetDeclaredMethod("__GetTypeSupport"); + if (getTypeSupport != null) + { + try + { + s_typeSupport = (IntPtr)getTypeSupport.Invoke(null, new object[] { }); + } + catch + { + s_typeSupport = IntPtr.Zero; + } + } + else + { + s_typeSupport = IntPtr.Zero; + } + + s_createSendGoalRequest = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateSendGoalRequest"); + + s_createSendGoalRequestHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalRequestHandle"); + + s_createSendGoalResponse = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalResponse"); + + s_createSendGoalResponseHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalResponseHandle"); + + s_createGetResultRequest = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultRequest"); + + s_createGetResultRequestHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultRequestHandle"); + + s_createGetResultResponse = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateGetResultResponse"); + + s_createGetResultResponseHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultResponseHandle"); + + s_createFeedbackMessage = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateFeedbackMessage"); + + s_createFeedbackMessageHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateFeedbackMessageHandle"); + } + + public static IntPtr GetTypeSupport() + { + // This is a Method because it could throw. + if (s_typeSupport == IntPtr.Zero) + { + throw CreateMethodNotDefinedCorrectlyException("__GetTypeSupport"); + } + + return s_typeSupport; + } + + public static IRosActionSendGoalRequest CreateSendGoalRequest() + { + if (s_createSendGoalRequest != null) + { + return s_createSendGoalRequest(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalRequest"); + } + } + + public static SafeHandle CreateSendGoalRequestHandle() + { + if (s_createSendGoalRequestHandle != null) + { + return s_createSendGoalRequestHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalRequestHandle"); + } + } + + public static IRosActionSendGoalResponse CreateSendGoalResponse() + { + if (s_createSendGoalResponse != null) + { + return s_createSendGoalResponse(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalResponse"); + } + } + + public static SafeHandle CreateSendGoalResponseHandle() + { + if (s_createSendGoalResponseHandle != null) + { + return s_createSendGoalResponseHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalResponseHandle"); + } + } + + public static IRosActionGetResultRequest CreateGetResultRequest() + { + if (s_createGetResultRequest != null) + { + return s_createGetResultRequest(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultRequest"); + } + } + + public static SafeHandle CreateGetResultRequestHandle() + { + if (s_createGetResultRequestHandle != null) + { + return s_createGetResultRequestHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultRequestHandle"); + } + } + + public static IRosActionGetResultResponse CreateGetResultResponse() + { + if (s_createGetResultResponse != null) + { + return s_createGetResultResponse(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultResponse"); + } + } + + public static SafeHandle CreateGetResultResponseHandle() + { + if (s_createGetResultResponseHandle != null) + { + return s_createGetResultResponseHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultResponseHandle"); + } + } + + public static IRosActionFeedbackMessage CreateFeedbackMessage() + { + if (s_createFeedbackMessage != null) + { + return s_createFeedbackMessage(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateFeedbackMessage"); + } + } + + public static SafeHandle CreateFeedbackMessageHandle() + { + if (s_createFeedbackMessageHandle != null) + { + return s_createFeedbackMessageHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateFeedbackMessageHandle"); + } + } + + private static TDelegate CreateDelegateForDeclaredMethod(TypeInfo typeInfo, string methodName) + where TDelegate : Delegate + { + MethodInfo methodInfo = typeInfo.GetDeclaredMethod(methodName); + if (methodInfo != null) + { + try + { + return (TDelegate)methodInfo.CreateDelegate(typeof(TDelegate)); + } + catch + { + return null; + } + } + else + { + return null; + } + } + + private static Exception CreateMethodNotDefinedCorrectlyException(string methodName) + { + return new InvalidOperationException($"Type '{typeof(TAction).FullName}' did not define a correct {methodName} method."); + } + } +} diff --git a/rcldotnet/ActionGoalStatus.cs b/rcldotnet/ActionGoalStatus.cs new file mode 100644 index 00000000..52a5d0b8 --- /dev/null +++ b/rcldotnet/ActionGoalStatus.cs @@ -0,0 +1,60 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public enum ActionGoalStatus + { + // see definition here: https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/msg/GoalStatus.msg + + // + // Indicates status has not been properly set. + // + Unknown = 0, + + // + // The goal has been accepted and is awaiting execution. + // + Accepted = 1, + + // + // The goal is currently being executed by the action server. + // + Executing = 2, + + // + // The client has requested that the goal be canceled and the action + // server has accepted the cancel request. + // + Canceling = 3, + + // + // The goal was achieved successfully by the action server. + // + Succeeded = 4, + + // + // The goal was canceled after an external request from an + // action client. + // + Canceled = 5, + + // + // The goal was terminated by the action server without an + // external request. + // + Aborted = 6, + } +} diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs new file mode 100644 index 00000000..18a1caca --- /dev/null +++ b/rcldotnet/ActionServer.cs @@ -0,0 +1,478 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.InteropServices; +using System.Threading.Tasks; +using action_msgs.msg; +using action_msgs.srv; +using unique_identifier_msgs.msg; + +namespace ROS2 +{ + /// + /// A response returned by an action server callback when a goal is requested. + /// + public enum GoalResponse + { + /// + /// Invalid default value. + /// + Default = 0, + + /// + /// The goal is rejected and will not be executed. + /// + Reject = 1, + + // `rclpy` doesn't provide the option to defer the the execution (unless you override the `acceptCallback`) + // Accept = 2, + // Alternative from `rclcpp`: + + /// + /// The server accepts the goal, and is going to begin execution immediately. + /// + AcceptAndExecute = 2, + + /// + /// The server accepts the goal, and is going to execute it later. + /// + AcceptAndDefer = 3, + } + + /// + /// A response returned by an action server callback when a goal has been asked to be canceled. + /// + public enum CancelResponse + { + /// + /// Invalid default value. + /// + Default = 0, + + /// + /// The server will not try to cancel the goal. + /// + Reject = 1, + + /// + /// The server has agreed to try to cancel the goal. + /// + Accept = 2, + } + + internal enum ActionGoalEvent + { + // These values need to mirror rcl_action_goal_event_t. + + Execute = 0, + CancelGoal, + Succeed, + Abort, + Canceled, + } + + public abstract class ActionServer + { + // Only allow internal subclasses. + internal ActionServer() + { + } + + // ActionServer does intentionally (for now) not implement IDisposable + // as this needs some extra consideration how the type works after its + // internal handle is disposed. By relying on the GC/Finalizer of + // SafeHandle the handle only gets Disposed if the action client is not + // live anymore. + internal abstract SafeActionServerHandle Handle { get; } + + internal abstract IRosMessage CreateSendGoalRequest(); + internal abstract SafeHandle CreateSendGoalRequestHandle(); + + internal abstract IRosActionGetResultRequest CreateGetResultRequest(); + internal abstract SafeHandle CreateGetResultRequestHandle(); + + internal abstract void HandleGoalRequest(SafeRequestIdHandle requestIdHandle, IRosMessage goalRequest); + + internal abstract void HandleCancelRequest(SafeRequestIdHandle requestIdHandle, CancelGoal_Request cancelRequest); + + internal abstract void HandleResultRequest(SafeRequestIdHandle requestIdHandle, IRosActionGetResultRequest resultRequest); + + internal abstract void HandleGoalExpired(); + } + + public sealed class ActionServer : ActionServer + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + private readonly Action> _acceptedCallback; + + private readonly Func _goalCallback; + + private readonly Func, CancelResponse> _cancelCallback; + + private readonly ConcurrentDictionary> _goalHandles + = new ConcurrentDictionary>(); + + // No public constructor. + internal ActionServer( + SafeActionServerHandle handle, + Action> acceptedCallback, + Func goalCallback, + Func, CancelResponse> cancelCallback) + { + Handle = handle; + _acceptedCallback = acceptedCallback; + _goalCallback = goalCallback; + _cancelCallback = cancelCallback; + } + + internal override SafeActionServerHandle Handle { get; } + + internal override IRosMessage CreateSendGoalRequest() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalRequest(); + } + + internal override SafeHandle CreateSendGoalRequestHandle() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle(); + } + + internal override IRosActionGetResultRequest CreateGetResultRequest() + { + return ActionDefinitionStaticMemberCache.CreateGetResultRequest(); + } + + internal override SafeHandle CreateGetResultRequestHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle(); + } + + internal IRosActionGetResultResponse CreateGetResultResponse() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponse(); + } + + private SafeHandle CreateGetResultResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponseHandle(); + } + + private IRosActionSendGoalResponse CreateSendGoalResponse() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponse(); + } + + private SafeHandle CreateSendGoalResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponseHandle(); + } + + private SafeHandle CreateFeedbackMessageHandle() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessageHandle(); + } + + internal override void HandleGoalRequest(SafeRequestIdHandle requestIdHandle, IRosMessage goalRequest) + { + var goalRequestCasted = (IRosActionSendGoalRequest)goalRequest; + + var goal = goalRequestCasted.Goal; + var uuid = (UUID)goalRequestCasted.GoalIdAsRosMessage; + var goalId = uuid.ToGuid(); + + // TODO: check if a goalHandle for the goalId already exists. + // + // rclpy does this before calling the goalCallback, rclcpp doesn't + // do this and relies on rcl_action_accept_new_goal to return NULL. + // In the rclpy case goalCallback doesn't get called, in rclcpp it + // does. + // rclpy logs this, sends a not accepted response. + // rclcpp throws an error without logging and sending an response. + // Currently this behaves like rclcpp. + + var goalResponse = _goalCallback(goalId, goal); + + var accepted = + goalResponse == GoalResponse.AcceptAndExecute || + goalResponse == GoalResponse.AcceptAndDefer; + + ActionServerGoalHandle goalHandle = null; + + // goalInfo.Stamp will be filled in by native_rcl_action_accept_new_goal(). + GoalInfo goalInfo = null; + + if (accepted) + { + goalInfo = new GoalInfo(); + + goalInfo.GoalId.Uuid = goalId.ToUuidByteArray(); + + using (var goalInfoHandle = GoalInfo.__CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(goalInfo, goalInfoHandle); + + var actionGoalHandle = new SafeActionGoalHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_action_accept_new_goal(ref actionGoalHandle, Handle, goalInfoHandle); + if (ret != RCLRet.Ok) + { + actionGoalHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_accept_new_goal)}() failed."); + } + + // Read back goalInfo.Stamp. + RCLdotnet.ReadFromMessageHandle(goalInfo, goalInfoHandle); + + goalHandle = new ActionServerGoalHandle(actionGoalHandle, this, goalId, goal); + + if (!_goalHandles.TryAdd(goalId, goalHandle)) + { + throw new Exception($"Two goals were accepted with the same Id '{goalId}'."); + }; + + if (goalResponse == GoalResponse.AcceptAndExecute) + { + ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(goalHandle.Handle, ActionGoalEvent.Execute); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_update_goal_state)}() failed."); + } + } + } + + var response = CreateSendGoalResponse(); + response.Accepted = accepted; + + if (accepted) + { + Debug.Assert(goalInfo != null); + + // TODO: (sh) File Issues for different handling of this in rclcpp or rclpy? + // rclpy fills in the response.Stamp but with a different timestamp than rcl_action_accept_new_goal() restamps. + // rclcpp doesn't fill in the stamp at all. + + // Return the stamp that native_rcl_action_accept_new_goal filled in. + response.StampAsRosMessage = goalInfo.Stamp; + } + + SendGoalResponse(requestIdHandle, response); + + if (accepted) + { + Debug.Assert(goalHandle != null); + + PublishStatus(); + + _acceptedCallback(goalHandle); + } + } + + private void SendGoalResponse(SafeRequestIdHandle requestHeaderHandle, IRosMessage goalResponse) + { + using (var goalResponseHandle = CreateSendGoalResponseHandle()) + { + RCLdotnet.WriteToMessageHandle(goalResponse, goalResponseHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_send_goal_response(Handle, requestHeaderHandle, goalResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_goal_response)}() failed."); + } + } + + internal override void HandleCancelRequest(SafeRequestIdHandle requestIdHandle, CancelGoal_Request cancelRequest) + { + var cancelResponse = new CancelGoal_Response(); + + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + using (var cancelResponseHandle = CancelGoal_Response.__CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(cancelRequest, cancelRequestHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_process_cancel_request(Handle, cancelRequestHandle, cancelResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_process_cancel_request)}() failed."); + + RCLdotnet.ReadFromMessageHandle(cancelResponse, cancelResponseHandle); + + var goalsCancelingFromRcl = cancelResponse.GoalsCanceling; + + // Create and fill in new list for the goals the callback accepts to cancel. + cancelResponse.GoalsCanceling = new List(); + + foreach (var goalInfo in goalsCancelingFromRcl) + { + var goalId = goalInfo.GoalId.ToGuid(); + + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + var response = _cancelCallback(goalHandle); + + if (response == CancelResponse.Accept) + { + ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(goalHandle.Handle, ActionGoalEvent.CancelGoal); + if (ret == RCLRet.Ok) + { + cancelResponse.GoalsCanceling.Add(goalInfo); + } + else + { + // If the goal's just succeeded after user cancel callback + // that will generate an exception from invalid transition. + // -> Remove from response since goal has been succeeded. + + // TODO: (sh) Logging. + + // Don't add to goalsForResponse. + } + } + } + else + { + // Possibly the user doesn't care to track the goal handle + // Remove from response. + // or: + // We can't cancel a goal for a goalHandle we don't have. + + // Don't add to goalsForResponse. + } + } + + // TODO: (sh) rclpy doesn't set this? -> Create issue there? + // If the user rejects all individual requests to cancel goals, + // then we consider the top-level cancel request as rejected. + if (goalsCancelingFromRcl.Count > 0 && cancelResponse.GoalsCanceling.Count == 0) + { + cancelResponse.ReturnCode = CancelGoal_Response.ERROR_REJECTED; + } + + if (cancelResponse.GoalsCanceling.Count > 0) + { + // At least one goal state changed, publish a new status message. + PublishStatus(); + } + + RCLdotnet.WriteToMessageHandle(cancelResponse, cancelResponseHandle); + + ret = RCLdotnetDelegates.native_rcl_action_send_cancel_response(Handle, requestIdHandle, cancelResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_cancel_response)}() failed."); + } + } + + internal override void HandleResultRequest(SafeRequestIdHandle requestIdHandle, IRosActionGetResultRequest resultRequest) + { + var uuid = (UUID)resultRequest.GoalIdAsRosMessage; + var goalId = uuid.ToGuid(); + + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + // Using Task.ContinueWith here should deal with all the multi-threaded race conditions. + goalHandle.ResultTask.ContinueWith(ResultTaskContinuationAction, requestIdHandle, TaskContinuationOptions.ExecuteSynchronously); + } + else + { + try + { + var resultResponse = CreateGetResultResponse(); + resultResponse.Status = GoalStatus.STATUS_UNKNOWN; + SendResultResponse(requestIdHandle, resultResponse); + } + finally + { + requestIdHandle.Dispose(); + } + } + } + + private void ResultTaskContinuationAction(Task> resultTask, object state) + { + var requestIdHandle = (SafeRequestIdHandle)state; + + try + { + SendResultResponse(requestIdHandle, resultTask.Result); + } + finally + { + requestIdHandle.Dispose(); + } + } + + private void SendResultResponse(SafeRequestIdHandle requestHeaderHandle, IRosActionGetResultResponse resultResponse) + { + using (var resultResponseHandle = CreateGetResultResponseHandle()) + { + RCLdotnet.WriteToMessageHandle(resultResponse, resultResponseHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_send_result_response(Handle, requestHeaderHandle, resultResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_result_response)}() failed."); + } + } + + internal override void HandleGoalExpired() + { + // This expects only one goal to expire at a time (rclcpp does the same). + // Loop until no new goals are expired. + var goalInfo = new GoalInfo(); + int numExpired; + + using (var goalInfoHandle = GoalInfo.__CreateMessageHandle()) + { + do + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_expire_goals(Handle, goalInfoHandle, out numExpired); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_expire_goals)}() failed."); + + if (numExpired > 0) + { + RCLdotnet.ReadFromMessageHandle(goalInfo, goalInfoHandle); + Guid goalId = goalInfo.GoalId.ToGuid(); + + _goalHandles.TryRemove(goalId, out _); + } + } while (numExpired > 0); + } + } + + internal void HandleTerminalState( + ActionServerGoalHandle actionServerGoalHandle, + IRosActionGetResultResponse response) + { + PublishStatus(); + actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_notify_goal_done(Handle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_notify_goal_done)}() failed."); + } + + internal void PublishStatus() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_publish_status(Handle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_publish_status)}() failed."); + } + + internal void PublishFeedbackMessage(IRosMessage feedbackMessage) + { + using (var feedbackMessageHandle = CreateFeedbackMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(feedbackMessage, feedbackMessageHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_publish_feedback(Handle, feedbackMessageHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_publish_feedback)}() failed."); + } + } + } +} diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs new file mode 100644 index 00000000..cd87d68c --- /dev/null +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -0,0 +1,182 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading.Tasks; +using action_msgs.msg; +using unique_identifier_msgs.msg; + +namespace ROS2 +{ + public abstract class ActionServerGoalHandle + { + // Only allow internal subclasses. + internal ActionServerGoalHandle() + { + } + + public abstract Guid GoalId { get; } + + /// + /// Indicate if goal is pending or executing. + /// + /// False if goal has reached a terminal state. + public abstract bool IsActive { get; } + + /// + /// Indicate if client has requested this goal be cancelled. + /// + /// True if a cancellation request has been accepted for this goal. + public abstract bool IsCanceling { get; } + + /// + /// Indicate if goal is executing. + /// + /// True only if the goal is in an executing state. + public abstract bool IsExecuting { get; } + + public abstract ActionGoalStatus Status { get; } + + internal abstract SafeActionGoalHandle Handle { get; } + + // In `rclpy` this calls the `executeCallback` after setting the state to executing. + // In `rclcpp` this does not call the `executeCallback` (as there is none) but sets the state for the explicit `AcceptAndDefer` `GoalResponse`. + public abstract void Execute(); + } + + public sealed class ActionServerGoalHandle : ActionServerGoalHandle + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + private readonly ActionServer _actionServer; + + // No public constructor. + internal ActionServerGoalHandle( + SafeActionGoalHandle handle, + ActionServer actionServer, + Guid goalId, + TGoal goal) + { + Handle = handle; + _actionServer = actionServer; + Goal = goal; + GoalId = goalId; + ResultTaskCompletionSource = new TaskCompletionSource>(); + } + + // `rclpy` uses the name `Request`, but the name from `rclcpp` `Goal` fits better. + public TGoal Goal { get; } + + public override Guid GoalId { get; } + + public override bool IsActive + { + get + { + int isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); + return isActive != 0; + } + } + + public override bool IsCanceling => Status == ActionGoalStatus.Canceling; + + public override bool IsExecuting => Status == ActionGoalStatus.Executing; + + public override ActionGoalStatus Status + { + get + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_goal_handle_get_status(Handle, out byte status); + + // In .NET properties should not throw exceptions. Should this + // be converted to a method for this reason? -> No as the rcl + // methods only do argument null checks for values which + // rcldotnet should ensure that they are not null. + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_goal_handle_get_status)}() failed."); + + return (ActionGoalStatus)status; + } + } + + internal override SafeActionGoalHandle Handle { get; } + + internal TaskCompletionSource> ResultTaskCompletionSource { get; } + + internal Task> ResultTask => ResultTaskCompletionSource.Task; + + public override void Execute() + { + UpdateGoalState(ActionGoalEvent.Execute); + + _actionServer.PublishStatus(); + } + + public void PublishFeedback(TFeedback feedback) + { + IRosActionFeedbackMessage feedbackMessage = + ActionDefinitionStaticMemberCache.CreateFeedbackMessage(); + + var goalId = (UUID)feedbackMessage.GoalIdAsRosMessage; + goalId.Uuid = GoalId.ToUuidByteArray(); + feedbackMessage.Feedback = feedback; + _actionServer.PublishFeedbackMessage(feedbackMessage); + } + + // TODO: (sh) Decide which (or both?) of these methods should be exposed. + // // "rclpy style" + // public void Succeed() => throw new NotImplementedException(); + // public void Abort() => throw new NotImplementedException(); + // public void Canceled() => throw new NotImplementedException(); + + // "rclcpp style" + public void Succeed(TResult result) + { + UpdateGoalState(ActionGoalEvent.Succeed); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_SUCCEEDED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); + } + + public void Abort(TResult result) + { + UpdateGoalState(ActionGoalEvent.Abort); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_ABORTED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); + } + + public void Canceled(TResult result) + { + UpdateGoalState(ActionGoalEvent.Canceled); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_CANCELED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); + } + + private void UpdateGoalState(ActionGoalEvent actionGoalEvent) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(Handle, actionGoalEvent); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_update_goal_state)}() failed."); + } + } +} diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index afbdf6a6..883bbe85 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -6,7 +6,11 @@ find_package(ament_cmake_export_assemblies REQUIRED) find_package(ament_cmake REQUIRED) find_package(rcl REQUIRED) +find_package(rcl_action REQUIRED) +find_package(action_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(unique_identifier_msgs REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rmw_implementation_cmake REQUIRED) @@ -28,8 +32,15 @@ endif() set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") set(CS_SOURCES + ActionClient.cs + ActionClientGoalHandle.cs + ActionDefinitionStaticMemberCache.cs + ActionGoalStatus.cs + ActionServer.cs + ActionServerGoalHandle.cs Client.cs GuardCondition.cs + GuidExtensions.cs MessageStaticMemberCache.cs Node.cs Publisher.cs @@ -37,6 +48,9 @@ set(CS_SOURCES RCLdotnet.cs RCLExceptionHelper.cs RCLRet.cs + SafeActionClientHandle.cs + SafeActionGoalHandle.cs + SafeActionServerHandle.cs SafeClientHandle.cs SafeGuardConditionHandle.cs SafeNodeHandle.cs @@ -52,21 +66,26 @@ set(CS_SOURCES ) find_package(rcldotnet_common REQUIRED) -foreach(_assembly_dep ${rcldotnet_common_ASSEMBLIES_DLL}) - list(APPEND _assembly_deps_dll "${_assembly_dep}") -endforeach() + +set(_assemblies_dep_dlls + ${action_msgs_ASSEMBLIES_DLL} + ${builtin_interfaces_ASSEMBLIES_DLL} + ${rcldotnet_common_ASSEMBLIES_DLL} + ${unique_identifier_msgs_ASSEMBLIES_DLL} +) add_dotnet_library(${PROJECT_NAME}_assemblies SOURCES ${CS_SOURCES} INCLUDE_DLLS - ${_assembly_deps_dll} + ${_assemblies_dep_dlls} ) install_dotnet(${PROJECT_NAME}_assemblies DESTINATION lib/${PROJECT_NAME}/dotnet) ament_export_assemblies_dll("lib/${PROJECT_NAME}/dotnet/${PROJECT_NAME}_assemblies.dll") add_library(${PROJECT_NAME}_native SHARED + rcldotnet_action_client.c rcldotnet_client.c rcldotnet_guard_condition.c rcldotnet_node.c @@ -75,8 +94,11 @@ add_library(${PROJECT_NAME}_native SHARED ) ament_target_dependencies(${PROJECT_NAME}_native + "action_msgs" "builtin_interfaces" + "unique_identifier_msgs" "rcl" + "rcl_action" "rosidl_generator_c" "rosidl_typesupport_c" ) @@ -104,11 +126,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() - set(_assemblies_dep_dlls - ${rcldotnet_common_ASSEMBLIES_DLL} + set(_test_assemblies_dep_dlls ${std_msgs_ASSEMBLIES_DLL} ${test_msgs_ASSEMBLIES_DLL} - ${builtin_interfaces_ASSEMBLIES_DLL} ) # Used by CI to test the different target frameworks in a matrix. @@ -123,11 +143,13 @@ if(BUILD_TESTING) ${RCLDOTNET_TEST_TARGET_FRAMEWORK} SOURCES ${CS_SOURCES} + test/test_actions.cs test/test_guard_conditions.cs test/test_messages.cs test/test_services.cs INCLUDE_DLLS ${_assemblies_dep_dlls} + ${_test_assemblies_dep_dlls} INCLUDE_REFERENCES "Microsoft.NET.Test.Sdk=15.9.0" "xunit=2.4.1" diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 643552d6..f0ff7ad0 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -108,24 +108,7 @@ public Task SendRequestAsync(TRequest request) using (var requestHandle = MessageStaticMemberCache.CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - requestHandle.DangerousAddRef(ref mustRelease); - request.__WriteToHandle(requestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - requestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(request, requestHandle); RCLRet ret = ClientDelegates.native_rcl_send_request(Handle, requestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_send_request)}() failed."); diff --git a/rcldotnet/GuidExtensions.cs b/rcldotnet/GuidExtensions.cs new file mode 100644 index 00000000..c75c0a68 --- /dev/null +++ b/rcldotnet/GuidExtensions.cs @@ -0,0 +1,71 @@ +using System; +using System.Linq; +using unique_identifier_msgs.msg; + +namespace ROS2 +{ + public static class GuidExtensions + { + public static UUID ToUuidMsg(this Guid guid) + { + // Microsoft uses some mixed endian representation for Guid in + // Windows and .NET, which the Guid.ToByteArray() does as well. + // Most/All other implementations of UUID/GUIDs use an unique big + // endian representation on the wire. See this StackOverflow answer + // for more context: https://stackoverflow.com/a/9195681 + // + // So there is a need to swap some bytes before converting them to + // the UUID ROS message. + // + // Even on BigEndian machines .NET does the swapping of the first + // three fields to little endian, so it is at least consistent on + // all platforms, no need to check the endianness of the runtime. + // https://github.com/dotnet/runtime/blob/1ba0394d71a4ea6bee7f6b28a22d666b7b56f913/src/libraries/System.Private.CoreLib/src/System/Guid.cs#L819 + var guidBytes = guid.ToByteArray(); + EndianSwap(guidBytes); + + var uuidMsg = new UUID(); + uuidMsg.Uuid = guidBytes; + return uuidMsg; + } + + public static byte[] ToUuidByteArray(this Guid guid) + { + // See comments in `ToUuidMsg` for info on byte swapping. + + var guidBytes = guid.ToByteArray(); + EndianSwap(guidBytes); + + return guidBytes; + } + + public static Guid ToGuid(this UUID uuidMsg) + { + // See comments in `ToUuidMsg` for info on byte swapping. + + var uuidBytesCopy = uuidMsg.Uuid.ToArray(); + EndianSwap(uuidBytesCopy); + + var guid = new Guid(uuidBytesCopy); + return guid; + } + + private static void EndianSwap(byte[] guid) + { + // taken from https://github.com/StephenCleary/Guids/blob/a9bd91835d536636d13249bd1bfbb1cd76c43533/src/Nito.Guids/GuidUtility.cs#L47 + Swap(guid, 0, 3); + Swap(guid, 1, 2); + + Swap(guid, 4, 5); + + Swap(guid, 6, 7); + } + + private static void Swap(byte[] array, int index1, int index2) + { + var temp = array[index1]; + array[index1] = array[index2]; + array[index2] = temp; + } + } +} diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index c0b537e6..1439d56b 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -72,6 +72,30 @@ internal delegate RCLRet NativeRCLDestroyClientHandleType( internal static NativeRCLDestroyClientHandleType native_rcl_destroy_client_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionCreateClientHandleType( + ref SafeActionClientHandle actionClientHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); + + internal static NativeRCLActionCreateClientHandleType native_rcl_action_create_client_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyClientHandleType( + IntPtr actionClientHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLActionDestroyClientHandleType native_rcl_action_destroy_client_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionCreateServerHandleType( + ref SafeActionServerHandle actionServerHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); + + internal static NativeRCLActionCreateServerHandleType native_rcl_action_create_server_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyServerHandleType( + IntPtr actionServerHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLActionDestroyServerHandleType native_rcl_action_destroy_server_handle = null; + static NodeDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -132,6 +156,32 @@ static NodeDelegates() 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)); } } @@ -145,6 +195,10 @@ public sealed class Node private readonly IList _guardConditions; + private readonly IList _actionClients; + + private readonly IList _actionServers; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -152,6 +206,8 @@ internal Node(SafeNodeHandle handle) _services = new List(); _clients = new List(); _guardConditions = new List(); + _actionClients = new List(); + _actionServers = new List(); } public IList Subscriptions => _subscriptions; @@ -165,6 +221,10 @@ internal Node(SafeNodeHandle handle) public IList GuardConditions => _guardConditions; + public IList ActionClients => _actionClients; + + public IList ActionServers => _actionServers; + // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its // internal handle is disposed. @@ -299,5 +359,86 @@ public GuardCondition CreateGuardCondition(Action callback) _guardConditions.Add(guardCondition); return guardCondition; } + + public ActionClient CreateActionClient(string actionName) + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + IntPtr typeSupport = ActionDefinitionStaticMemberCache.GetTypeSupport(); + + var actionClientHandle = new SafeActionClientHandle(); + RCLRet ret = NodeDelegates.native_rcl_action_create_client_handle(ref actionClientHandle, Handle, actionName, typeSupport); + actionClientHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + actionClientHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_action_create_client_handle)}() failed."); + } + + // TODO: (sh) Add actionName to ActionClient. + var actionClient = new ActionClient(actionClientHandle, this); + _actionClients.Add(actionClient); + return actionClient; + } + + // TODO: (sh) Make acceptedCallback an async callback? Add as overload? (`Func`) + // TODO: (sh) Make acceptedCallback an async callback which returns the `TResult` (`Func>`)? + // Add as overload? + // In that case this would be more or less an `executeCallback` from `rclpy`. + public ActionServer CreateActionServer( + string actionName, + Action> acceptedCallback, + // This has an additional `goalId` parameter compared to `rclpy`. + Func goalCallback = null, + Func, CancelResponse> cancelCallback = null + ) + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + if (goalCallback == null) + { + goalCallback = DefaultGoalCallback; + } + + if (cancelCallback == null) + { + cancelCallback = DefaultCancelCallback; + } + + IntPtr typeSupport = ActionDefinitionStaticMemberCache.GetTypeSupport(); + + var actionServerHandle = new SafeActionServerHandle(); + RCLRet ret = NodeDelegates.native_rcl_action_create_server_handle(ref actionServerHandle, Handle, actionName, typeSupport); + actionServerHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + actionServerHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_action_create_server_handle)}() failed."); + } + + // TODO: (sh) Add actionName to ActionServer. + var actionServer = new ActionServer( + actionServerHandle, + acceptedCallback, + goalCallback, + cancelCallback); + + _actionServers.Add(actionServer); + return actionServer; + } + + private static GoalResponse DefaultGoalCallback(Guid goalId, TGoal goal) + { + return GoalResponse.AcceptAndExecute; + } + + private static CancelResponse DefaultCancelCallback(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Reject; + } } } diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index ef99966d..02198057 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -73,24 +73,7 @@ public void Publish(T message) { using (var messageHandle = MessageStaticMemberCache.CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__WriteToHandle(messageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(message, messageHandle); RCLRet ret = PublisherDelegates.native_rcl_publish(Handle, messageHandle); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(PublisherDelegates.native_rcl_publish)}() failed."); diff --git a/rcldotnet/RCLRet.cs b/rcldotnet/RCLRet.cs index 95c2f6de..2f529d2a 100644 --- a/rcldotnet/RCLRet.cs +++ b/rcldotnet/RCLRet.cs @@ -50,6 +50,8 @@ internal enum RCLRet InvalidParamRule = 1010, InvalidLogLevelRule = 1020, EventInvalid = 2000, - EventTakeFailed = 2001 + EventTakeFailed = 2001, + ActionClientTakeFailed = 2103, + ActionServerTakeFailed = 2201, } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 3cc56ac1..3a1136a1 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -15,6 +15,8 @@ using System; using System.Runtime.InteropServices; +using action_msgs.msg; +using action_msgs.srv; using ROS2.Utils; namespace ROS2 @@ -172,6 +174,157 @@ internal delegate RCLRet NativeRCLCreateRequestIdHandleType( internal static NativeRCLTakeResponseType native_rcl_take_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionClientWaitSetGetNumEntriesType( + SafeActionClientHandle actionClientHandle, out int numSubscriptions, out int numGuardConditions, out int numTimers, out int numClients, out int numServices); + + internal static NativeRCLActionClientWaitSetGetNumEntriesType native_rcl_action_client_wait_set_get_num_entries = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionWaitSetAddActionClientType( + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle); + + internal static NativeRCLActionWaitSetAddActionClientType native_rcl_action_wait_set_add_action_client = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle, out int isFeedbackReady, out int isStatusReady, out int isGoalResponseReady, out int isCancelResponseReady, out int isResultResponseReady); + + internal static NativeRCLActionClientWaitSetGetEntitiesReadyType native_rcl_action_client_wait_set_get_entities_ready = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerWaitSetGetNumEntriesType( + SafeActionServerHandle actionServerHandle, out int numSubscriptions, out int numGuardConditions, out int numTimers, out int numClients, out int numServices); + + internal static NativeRCLActionServerWaitSetGetNumEntriesType native_rcl_action_server_wait_set_get_num_entries = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionWaitSetAddActionServerType( + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle); + + internal static NativeRCLActionWaitSetAddActionServerType native_rcl_action_wait_set_add_action_server = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerWaitSetGetEntitiesReadyType( + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle, out int isGoalRequestReady, out int isCancelRequestReady, out int isResultRequestReady, out int isGoalExpired); + + internal static NativeRCLActionServerWaitSetGetEntitiesReadyType native_rcl_action_server_wait_set_get_entities_ready = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeFeedbackType( + SafeActionClientHandle actionClientHandle, SafeHandle feedbackMessageHandle); + + internal static NativeRCLActionTakeFeedbackType native_rcl_action_take_feedback = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeStatusType( + SafeActionClientHandle actionClientHandle, SafeHandle statusMessageHandle); + + internal static NativeRCLActionTakeStatusType native_rcl_action_take_status = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeGoalResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle goalResponseHandle); + + internal static NativeRCLActionTakeGoalResponseType native_rcl_action_take_goal_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeCancelResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionTakeCancelResponseType native_rcl_action_take_cancel_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeResultResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultResponseHandle); + + internal static NativeRCLActionTakeResultResponseType native_rcl_action_take_result_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeGoalRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle goalRequestHandle); + + internal static NativeRCLActionTakeGoalRequestType native_rcl_action_take_goal_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendGoalResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle gaolResponseHandle); + + internal static NativeRCLActionSendGoalResponseType native_rcl_action_send_goal_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionAcceptNewGoalType( + ref SafeActionGoalHandle actionGoalHandleHandle, SafeActionServerHandle actionServerHandle, SafeHandle goalInfoHandle); + + internal static NativeRCLActionAcceptNewGoalType native_rcl_action_accept_new_goal = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyGoalHandleType(IntPtr actionGoalHandleHandle); + + internal static NativeRCLActionDestroyGoalHandleType native_rcl_action_destroy_goal_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionUpdateGoalStateType( + SafeActionGoalHandle actionGoalHandleHandle, ActionGoalEvent goalEvent); + + internal static NativeRCLActionUpdateGoalStateType native_rcl_action_update_goal_state = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionPublishStatusType(SafeActionServerHandle actionServerHandle); + internal static NativeRCLActionPublishStatusType native_rcl_action_publish_status = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionPublishFeedbackType(SafeActionServerHandle actionServerHandle, SafeHandle feedbackMessageHandle); + internal static NativeRCLActionPublishFeedbackType native_rcl_action_publish_feedback = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeCancelRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelRequestHandle); + + internal static NativeRCLActionTakeCancelRequestType native_rcl_action_take_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionProcessCancelRequestType( + SafeActionServerHandle actionServerHandle, SafeHandle cancelRequestHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionProcessCancelRequestType native_rcl_action_process_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendCancelResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionSendCancelResponseType native_rcl_action_send_cancel_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeResultRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultRequestHandle); + + internal static NativeRCLActionTakeResultRequestType native_rcl_action_take_result_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendResultResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultResponseHandle); + + internal static NativeRCLActionSendResultResponseType native_rcl_action_send_result_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionNotifyGoalDoneType(SafeActionServerHandle actionServerHandle); + internal static NativeRCLActionNotifyGoalDoneType native_rcl_action_notify_goal_done = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionExpireGoalsType( + SafeActionServerHandle actionServerHandle, SafeHandle goalInfoHandle, out int numExpired); + + internal static NativeRCLActionExpireGoalsType native_rcl_action_expire_goals = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate int NativeRCLActionGoalHandleIsActiveType(SafeActionGoalHandle actionGoalHandleHandle); + internal static NativeRCLActionGoalHandleIsActiveType native_rcl_action_goal_handle_is_active = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionGoalHandleGetStatusType(SafeActionGoalHandle actionGoalHandleHandle, out byte status); + internal static NativeRCLActionGoalHandleGetStatusType native_rcl_action_goal_handle_get_status = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreateQosProfileHandleType(ref SafeQosProfileHandle qosProfileHandle); internal static NativeRCLCreateQosProfileHandleType native_rcl_create_qos_profile_handle = null; @@ -371,6 +524,168 @@ static RCLdotnetDelegates() (NativeRCLTakeResponseType)Marshal.GetDelegateForFunctionPointer( native_rcl_take_response_ptr, typeof(NativeRCLTakeResponseType)); + IntPtr native_rcl_action_client_wait_set_get_num_entries_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_client_wait_set_get_num_entries"); + RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries = + (NativeRCLActionClientWaitSetGetNumEntriesType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_client_wait_set_get_num_entries_ptr, typeof(NativeRCLActionClientWaitSetGetNumEntriesType)); + + IntPtr native_rcl_action_wait_set_add_action_client_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_wait_set_add_action_client"); + RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client = + (NativeRCLActionWaitSetAddActionClientType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_wait_set_add_action_client_ptr, typeof(NativeRCLActionWaitSetAddActionClientType)); + + IntPtr native_rcl_action_client_wait_set_get_entities_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_client_wait_set_get_entities_ready"); + RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready = + (NativeRCLActionClientWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_client_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionClientWaitSetGetEntitiesReadyType)); + + IntPtr native_rcl_action_server_wait_set_get_num_entries_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_wait_set_get_num_entries"); + RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries = + (NativeRCLActionServerWaitSetGetNumEntriesType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_wait_set_get_num_entries_ptr, typeof(NativeRCLActionServerWaitSetGetNumEntriesType)); + + IntPtr native_rcl_action_wait_set_add_action_server_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_wait_set_add_action_server"); + RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server = + (NativeRCLActionWaitSetAddActionServerType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_wait_set_add_action_server_ptr, typeof(NativeRCLActionWaitSetAddActionServerType)); + + IntPtr native_rcl_action_server_wait_set_get_entities_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_wait_set_get_entities_ready"); + RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready = + (NativeRCLActionServerWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionServerWaitSetGetEntitiesReadyType)); + + IntPtr native_rcl_action_take_feedback_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_feedback"); + RCLdotnetDelegates.native_rcl_action_take_feedback + = (NativeRCLActionTakeFeedbackType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_feedback_ptr, typeof(NativeRCLActionTakeFeedbackType)); + + IntPtr native_rcl_action_take_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_status"); + RCLdotnetDelegates.native_rcl_action_take_status = + (NativeRCLActionTakeStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_status_ptr, typeof(NativeRCLActionTakeStatusType)); + + IntPtr native_rcl_action_take_goal_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_goal_response"); + RCLdotnetDelegates.native_rcl_action_take_goal_response = + (NativeRCLActionTakeGoalResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_goal_response_ptr, typeof(NativeRCLActionTakeGoalResponseType)); + + IntPtr native_rcl_action_take_cancel_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_cancel_response"); + RCLdotnetDelegates.native_rcl_action_take_cancel_response = + (NativeRCLActionTakeCancelResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_cancel_response_ptr, typeof(NativeRCLActionTakeCancelResponseType)); + + IntPtr native_rcl_action_take_result_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_result_response"); + RCLdotnetDelegates.native_rcl_action_take_result_response = + (NativeRCLActionTakeResultResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_result_response_ptr, typeof(NativeRCLActionTakeResultResponseType)); + + IntPtr native_rcl_action_take_goal_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_goal_request"); + RCLdotnetDelegates.native_rcl_action_take_goal_request = + (NativeRCLActionTakeGoalRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_goal_request_ptr, typeof(NativeRCLActionTakeGoalRequestType)); + + IntPtr native_rcl_action_send_goal_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_goal_response"); + RCLdotnetDelegates.native_rcl_action_send_goal_response = + (NativeRCLActionSendGoalResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_goal_response_ptr, typeof(NativeRCLActionSendGoalResponseType)); + + IntPtr native_rcl_action_accept_new_goal_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_accept_new_goal"); + RCLdotnetDelegates.native_rcl_action_accept_new_goal = + (NativeRCLActionAcceptNewGoalType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_accept_new_goal_ptr, typeof(NativeRCLActionAcceptNewGoalType)); + + IntPtr native_rcl_action_destroy_goal_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_destroy_goal_handle"); + RCLdotnetDelegates.native_rcl_action_destroy_goal_handle = + (NativeRCLActionDestroyGoalHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_destroy_goal_handle_ptr, typeof(NativeRCLActionDestroyGoalHandleType)); + + IntPtr native_rcl_action_update_goal_state_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_update_goal_state"); + RCLdotnetDelegates.native_rcl_action_update_goal_state = + (NativeRCLActionUpdateGoalStateType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_update_goal_state_ptr, typeof(NativeRCLActionUpdateGoalStateType)); + + IntPtr native_rcl_action_publish_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_publish_status"); + RCLdotnetDelegates.native_rcl_action_publish_status = + (NativeRCLActionPublishStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_publish_status_ptr, typeof(NativeRCLActionPublishStatusType)); + + IntPtr native_rcl_action_publish_feedback_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_publish_feedback"); + RCLdotnetDelegates.native_rcl_action_publish_feedback = + (NativeRCLActionPublishFeedbackType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_publish_feedback_ptr, typeof(NativeRCLActionPublishFeedbackType)); + + IntPtr native_rcl_action_take_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_cancel_request"); + RCLdotnetDelegates.native_rcl_action_take_cancel_request = + (NativeRCLActionTakeCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_cancel_request_ptr, typeof(NativeRCLActionTakeCancelRequestType)); + + IntPtr native_rcl_action_process_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_process_cancel_request"); + RCLdotnetDelegates.native_rcl_action_process_cancel_request = + (NativeRCLActionProcessCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_process_cancel_request_ptr, typeof(NativeRCLActionProcessCancelRequestType)); + + IntPtr native_rcl_action_send_cancel_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_cancel_response"); + RCLdotnetDelegates.native_rcl_action_send_cancel_response = + (NativeRCLActionSendCancelResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_cancel_response_ptr, typeof(NativeRCLActionSendCancelResponseType)); + + IntPtr native_rcl_action_take_result_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_result_request"); + RCLdotnetDelegates.native_rcl_action_take_result_request = + (NativeRCLActionTakeResultRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_result_request_ptr, typeof(NativeRCLActionTakeResultRequestType)); + + IntPtr native_rcl_action_send_result_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_result_response"); + RCLdotnetDelegates.native_rcl_action_send_result_response = + (NativeRCLActionSendResultResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_result_response_ptr, typeof(NativeRCLActionSendResultResponseType)); + + IntPtr native_rcl_action_notify_goal_done_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_notify_goal_done"); + RCLdotnetDelegates.native_rcl_action_notify_goal_done = + (NativeRCLActionNotifyGoalDoneType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_notify_goal_done_ptr, typeof(NativeRCLActionNotifyGoalDoneType)); + + IntPtr native_rcl_action_expire_goals_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_expire_goals"); + RCLdotnetDelegates.native_rcl_action_expire_goals = + (NativeRCLActionExpireGoalsType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_expire_goals_ptr, typeof(NativeRCLActionExpireGoalsType)); + + IntPtr native_rcl_action_goal_handle_is_active_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_goal_handle_is_active"); + RCLdotnetDelegates.native_rcl_action_goal_handle_is_active = + (NativeRCLActionGoalHandleIsActiveType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_goal_handle_is_active_ptr, typeof(NativeRCLActionGoalHandleIsActiveType)); + + IntPtr native_rcl_action_goal_handle_get_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_goal_handle_get_status"); + RCLdotnetDelegates.native_rcl_action_goal_handle_get_status = + (NativeRCLActionGoalHandleGetStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_goal_handle_get_status_ptr, typeof(NativeRCLActionGoalHandleGetStatusType)); + IntPtr native_rcl_create_qos_profile_handle_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_qos_profile_handle"); RCLdotnetDelegates.native_rcl_create_qos_profile_handle = @@ -479,6 +794,18 @@ private static void WaitSetAddGuardCondition(SafeWaitSetHandle waitSetHandle, Sa RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_guard_condition)}() failed."); } + private static void WaitSetAddActionClient(SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client(waitSetHandle, actionClientHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client)}() failed."); + } + + private static void WaitSetAddActionServer(SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server(waitSetHandle, actionServerHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server)}() failed."); + } + /// /// Block until the wait set is ready or until the timeout has been exceeded. /// @@ -520,25 +847,7 @@ private static bool Take(Subscription subscription, IRosMessage message) switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__ReadFromHandle(messageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(message, messageHandle); return true; case RCLRet.SubscriptionTakeFailed: @@ -558,25 +867,7 @@ private static bool TakeRequest(Service service, SafeRequestIdHandle requestHead switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - requestHandle.DangerousAddRef(ref mustRelease); - request.__ReadFromHandle(requestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - requestHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(request, requestHandle); return true; case RCLRet.ServiceTakeFailed: @@ -596,25 +887,7 @@ private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeade switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__ReadFromHandle(responseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(response, responseHandle); return true; case RCLRet.ClientTakeFailed: @@ -626,33 +899,181 @@ private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeade } } + private static bool TakeFeedbackMessage(ActionClient actionClient, IRosMessage feedbackMessage) + { + using (var feedbackMessageHandle = actionClient.CreateFeedbackMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_feedback(actionClient.Handle, feedbackMessageHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(feedbackMessage, feedbackMessageHandle); + return true; + + case RCLRet.SubscriptionTakeFailed: + case RCLRet.ActionClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_feedback)}() failed."); + } + } + } + + private static bool TakeStatusMessage(ActionClient actionClient, IRosMessage statusMessage) + { + using (var statusMessageHandle = GoalStatusArray.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_status(actionClient.Handle, statusMessageHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(statusMessage, statusMessageHandle); + return true; + + case RCLRet.SubscriptionTakeFailed: + case RCLRet.ActionClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_status)}() failed."); + } + } + } + + private static bool TakeGoalResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage goalResponse) + { + using (var goalResponseHandle = actionClient.CreateSendGoalResponseHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_goal_response(actionClient.Handle, requestHeaderHandle, goalResponseHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(goalResponse, goalResponseHandle); + return true; + + case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_goal_response)}() failed."); + } + } + } + + private static bool TakeCancelResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage cancelResponse) + { + using (var cancelResponseHandle = CancelGoal_Response.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_cancel_response(actionClient.Handle, requestHeaderHandle, cancelResponseHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(cancelResponse, cancelResponseHandle); + return true; + + case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_cancel_response)}() failed."); + } + } + } + + private static bool TakeResultResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage resultResponse) + { + using (var resultResponseHandle = actionClient.CreateGetResultResponseHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_result_response(actionClient.Handle, requestHeaderHandle, resultResponseHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(resultResponse, resultResponseHandle); + return true; + + case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_result_response)}() failed."); + } + } + } + private static void SendResponse(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { using (var responseHandle = service.CreateResponseHandle()) { - bool mustRelease = false; - try + WriteToMessageHandle(response, responseHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); + } + } + + private static bool TakeGoalRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage goalRequest) + { + using (var goalRequestHandle = actionServer.CreateSendGoalRequestHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_goal_request(actionServer.Handle, requestHeaderHandle, goalRequestHandle); + switch (ret) { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__WriteToHandle(responseHandle.DangerousGetHandle()); + case RCLRet.Ok: + ReadFromMessageHandle(goalRequest, goalRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_goal_request)}() failed."); } - finally + } + } + + private static bool TakeCancelRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage cancelRequest) + { + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_cancel_request(actionServer.Handle, requestHeaderHandle, cancelRequestHandle); + switch (ret) { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } + case RCLRet.Ok: + ReadFromMessageHandle(cancelRequest, cancelRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_cancel_request)}() failed."); } + } + } - RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); - if (ret != RCLRet.Ok) + private static bool TakeResultRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage resultRequest) + { + using (var resultRequestHandle = actionServer.CreateGetResultRequestHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_result_request(actionServer.Handle, requestHeaderHandle, resultRequestHandle); + switch (ret) { - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); + case RCLRet.Ok: + ReadFromMessageHandle(resultRequest, resultRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_result_request)}() failed."); } } } @@ -666,6 +1087,44 @@ public static void SpinOnce(Node node, long timeout) int numberOfServices = node.Services.Count; int numberOfEvents = 0; + foreach (var actionClient in node.ActionClients) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries( + actionClient.Handle, + out int acNumberOfSubscriptions, + out int acNumberOfGuardConditions, + out int acNumberOfTimers, + out int acNumberOfClients, + out int acNumberOfServices); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries)}() failed."); + + numberOfSubscriptions += acNumberOfSubscriptions; + numberOfGuardConditions += acNumberOfGuardConditions; + numberOfTimers += acNumberOfTimers; + numberOfClients += acNumberOfClients; + numberOfServices += acNumberOfServices; + } + + foreach (var actionServer in node.ActionServers) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries( + actionServer.Handle, + out int asNumberOfSubscriptions, + out int asNumberOfGuardConditions, + out int asNumberOfTimers, + out int asNumberOfClients, + out int asNumberOfServices); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries)}() failed."); + + numberOfSubscriptions += asNumberOfSubscriptions; + numberOfGuardConditions += asNumberOfGuardConditions; + numberOfTimers += asNumberOfTimers; + numberOfClients += asNumberOfClients; + numberOfServices += asNumberOfServices; + } + bool waitSetEmpty = numberOfSubscriptions == 0 && numberOfGuardConditions == 0 && numberOfTimers == 0 @@ -711,6 +1170,16 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddGuardCondition(waitSetHandle, guardCondition.Handle); } + foreach (var actionClient in node.ActionClients) + { + WaitSetAddActionClient(waitSetHandle, actionClient.Handle); + } + + foreach (var actionServer in node.ActionServers) + { + WaitSetAddActionServer(waitSetHandle, actionServer.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { @@ -773,6 +1242,139 @@ public static void SpinOnce(Node node, long timeout) clientIndex++; } + + foreach (var actionClient in node.ActionClients) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready( + waitSetHandle, + actionClient.Handle, + out int isFeedbackReady, + out int isStatusReady, + out int isGoalResponseReady, + out int isCancelResponseReady, + out int isResultResponseReady); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready)}() failed."); + + // Check isGoalResponseReady so that a new goalHandle is + // already created if the status or feedback for the new + // goal is received. + if (isGoalResponseReady != 0) + { + var goalResponse = actionClient.CreateSendGoalResponse(); + + var result = TakeGoalResponse(actionClient, requestIdHandle, goalResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleGoalResponse(sequenceNumber, goalResponse); + } + } + + // Check isStatusReady before isFeedbackReady so that + // the feedback callback already has the newest status + // information if updates are received at the same time. + if (isStatusReady != 0) + { + var statusMessage = new GoalStatusArray(); + + var result = TakeStatusMessage(actionClient, statusMessage); + if (result) + { + actionClient.HandleStatusMessage(statusMessage); + } + } + + if (isFeedbackReady != 0) + { + var feedbackMessage = actionClient.CreateFeedbackMessage(); + + var result = TakeFeedbackMessage(actionClient, feedbackMessage); + if (result) + { + actionClient.HandleFeedbackMessage(feedbackMessage); + } + } + + if (isCancelResponseReady != 0) + { + var cancelResponse = new CancelGoal_Response(); + + var result = TakeCancelResponse(actionClient, requestIdHandle, cancelResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleCancelResponse(sequenceNumber, cancelResponse); + } + } + + if (isResultResponseReady != 0) + { + var resultResponse = actionClient.CreateGetResultResponse(); + + var result = TakeResultResponse(actionClient, requestIdHandle, resultResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleResultResponse(sequenceNumber, resultResponse); + } + } + } + + foreach (var actionServer in node.ActionServers) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready( + waitSetHandle, + actionServer.Handle, + out int isGoalRequestReady, + out int isCancelRequestReady, + out int isResultRequestReady, + out int isGoalExpired); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready)}() failed."); + + if (isGoalRequestReady != 0) + { + var goalRequest = actionServer.CreateSendGoalRequest(); + + var result = TakeGoalRequest(actionServer, requestIdHandle, goalRequest); + if (result) + { + actionServer.HandleGoalRequest(requestIdHandle, goalRequest); + } + } + + if (isCancelRequestReady != 0) + { + var cancelRequest = new CancelGoal_Request(); + + var result = TakeCancelRequest(actionServer, requestIdHandle, cancelRequest); + if (result) + { + actionServer.HandleCancelRequest(requestIdHandle, cancelRequest); + } + } + + if (isResultRequestReady != 0) + { + var resultRequest = actionServer.CreateGetResultRequest(); + + // We can't reuse requestIdHandle here as it is needed later when the result is ready. + // Create a new one for each GetResultRequest. + SafeRequestIdHandle resultRequestIdHandle = CreateRequestId(); + + var result = TakeResultRequest(actionServer, resultRequestIdHandle, resultRequest); + if (result) + { + actionServer.HandleResultRequest(resultRequestIdHandle, resultRequest); + } + } + + if (isGoalExpired != 0) + { + actionServer.HandleGoalExpired(); + } + } } int guardConditionIndex = 0; @@ -807,5 +1409,53 @@ public static string GetRMWIdentifier() string rmw_identifier = Marshal.PtrToStringAnsi(ptr); return rmw_identifier; } + + internal static void ReadFromMessageHandle(IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as + // this needs to be handled in generated code across multiple + // assemblies. Array and collection indexing would need to + // create SafeHandles everywhere. It's not worth it, especially + // considering the extra allocations for SafeHandles in arrays + // or collections that don't really represent their own native + // resource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__ReadFromHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } + + internal static void WriteToMessageHandle(IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as + // this needs to be handled in generated code across multiple + // assemblies. Array and collection indexing would need to + // create SafeHandles everywhere. It's not worth it, especially + // considering the extra allocations for SafeHandles in arrays + // or collections that don't really represent their own native + // resource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__WriteToHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } } } diff --git a/rcldotnet/SafeActionClientHandle.cs b/rcldotnet/SafeActionClientHandle.cs new file mode 100644 index 00000000..ba42db11 --- /dev/null +++ b/rcldotnet/SafeActionClientHandle.cs @@ -0,0 +1,90 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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_action_client_t + /// + /// + /// Since we need to delete the action client handle before the node is released we need to actually hold a + /// pointer to a rcl_action_client_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the client handle and node handle. + /// + internal sealed class SafeActionClientHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_client_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeActionClientHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_action_destroy_client_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/SafeActionGoalHandle.cs b/rcldotnet/SafeActionGoalHandle.cs new file mode 100644 index 00000000..838083c5 --- /dev/null +++ b/rcldotnet/SafeActionGoalHandle.cs @@ -0,0 +1,45 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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_action_goal_handle_t + /// This is only used to implement the action server. + /// It is not used for the action client. + /// + // Could be called SafeActionGoalHandleHandle as well: + // Safe${c_type}Handle where c_type = ActionGoalHandle ;) + internal sealed class SafeActionGoalHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeActionGoalHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_destroy_goal_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/SafeActionServerHandle.cs b/rcldotnet/SafeActionServerHandle.cs new file mode 100644 index 00000000..36831444 --- /dev/null +++ b/rcldotnet/SafeActionServerHandle.cs @@ -0,0 +1,90 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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_action_server_t + /// + /// + /// Since we need to delete the action client handle before the node is released we need to actually hold a + /// pointer to a rcl_action_server_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the client handle and node handle. + /// + internal sealed class SafeActionServerHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_client_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeActionServerHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_action_destroy_server_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index 5aa04a4b..88be5f0f 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -19,9 +19,17 @@ rmw_implementation_cmake rcl + rcl_action rmw + action_msgs + builtin_interfaces + unique_identifier_msgs rcl + rcl_action + action_msgs + builtin_interfaces + unique_identifier_msgs std_msgs test_msgs diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 04ffc51c..ffd268dd 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "rosidl_runtime_c/message_type_support_struct.h" @@ -25,6 +26,7 @@ #include "rcldotnet.h" static rcl_context_t context; +static rcl_clock_t clock; int32_t native_rcl_init() { // TODO(esteve): parse args @@ -38,9 +40,18 @@ int32_t native_rcl_init() { } const char ** arg_values = NULL; ret = rcl_init(num_args, arg_values, &init_options, &context); + if (ret != RCL_RET_OK) { + return ret; + } + + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); return ret; } +rcl_clock_t *native_rcl_get_default_clock() { + return &clock; +} + const char *native_rcl_get_rmw_identifier() { return rmw_get_implementation_identifier(); } @@ -174,6 +185,88 @@ int32_t native_rcl_wait_set_add_guard_condition(void *wait_set_handle, void *gua return ret; } +int32_t native_rcl_action_client_wait_set_get_num_entries( + void *action_client_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services) +{ + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + + size_t num_subscriptions_as_size_t; + size_t num_guard_conditions_as_size_t; + size_t num_timers_as_size_t; + size_t num_clients_as_size_t; + size_t num_services_as_size_t; + + rcl_ret_t ret = rcl_action_client_wait_set_get_num_entities( + action_client, + &num_subscriptions_as_size_t, + &num_guard_conditions_as_size_t, + &num_timers_as_size_t, + &num_clients_as_size_t, + &num_services_as_size_t); + + *num_subscriptions = (int32_t)num_subscriptions_as_size_t; + *num_guard_conditions = (int32_t)num_guard_conditions_as_size_t; + *num_timers = (int32_t)num_timers_as_size_t; + *num_clients = (int32_t)num_clients_as_size_t; + *num_services = (int32_t)num_services_as_size_t; + + return ret; +} + +int32_t native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + rcl_ret_t ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); + + return ret; +} + +int32_t native_rcl_action_server_wait_set_get_num_entries( + void *action_server_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services) +{ + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + + size_t num_subscriptions_as_size_t; + size_t num_guard_conditions_as_size_t; + size_t num_timers_as_size_t; + size_t num_clients_as_size_t; + size_t num_services_as_size_t; + + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + action_server, + &num_subscriptions_as_size_t, + &num_guard_conditions_as_size_t, + &num_timers_as_size_t, + &num_clients_as_size_t, + &num_services_as_size_t); + + *num_subscriptions = (int32_t)num_subscriptions_as_size_t; + *num_guard_conditions = (int32_t)num_guard_conditions_as_size_t; + *num_timers = (int32_t)num_timers_as_size_t; + *num_clients = (int32_t)num_clients_as_size_t; + *num_services = (int32_t)num_services_as_size_t; + + return ret; +} + +int32_t native_rcl_action_wait_set_add_action_server(void *wait_set_handle, void *action_server_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); + + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); @@ -229,6 +322,74 @@ int32_t native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t return result ? 1 : 0; } +int32_t native_rcl_action_client_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_client_handle, + int32_t *is_feedback_ready, + int32_t *is_status_ready, + int32_t *is_goal_response_ready, + int32_t *is_cancel_response_ready, + int32_t *is_result_response_ready) +{ + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + + bool is_feedback_ready_as_bool; + bool is_status_ready_as_bool; + bool is_goal_response_ready_as_bool; + bool is_cancel_response_ready_as_bool; + bool is_result_response_ready_as_bool; + + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( + wait_set, + action_client, + &is_feedback_ready_as_bool, + &is_status_ready_as_bool, + &is_goal_response_ready_as_bool, + &is_cancel_response_ready_as_bool, + &is_result_response_ready_as_bool); + + *is_feedback_ready = is_feedback_ready_as_bool ? 1 : 0; + *is_status_ready = is_status_ready_as_bool ? 1 : 0; + *is_goal_response_ready = is_goal_response_ready_as_bool ? 1 : 0; + *is_cancel_response_ready = is_cancel_response_ready_as_bool ? 1 : 0; + *is_result_response_ready = is_result_response_ready_as_bool ? 1 : 0; + + return ret; +} + +int32_t native_rcl_action_server_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_server_handle, + int32_t *is_goal_request_ready, + int32_t *is_cancel_request_ready, + int32_t *is_result_request_ready, + int32_t *is_goal_expired) +{ + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + + bool is_goal_request_ready_as_bool; + bool is_cancel_request_ready_as_bool; + bool is_result_request_ready_as_bool; + bool is_goal_expired_as_bool; + + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + action_server, + &is_goal_request_ready_as_bool, + &is_cancel_request_ready_as_bool, + &is_result_request_ready_as_bool, + &is_goal_expired_as_bool); + + *is_goal_request_ready = is_goal_request_ready_as_bool ? 1 : 0; + *is_cancel_request_ready = is_cancel_request_ready_as_bool ? 1 : 0; + *is_result_request_ready = is_result_request_ready_as_bool ? 1 : 0; + *is_goal_expired = is_goal_expired_as_bool ? 1 : 0; + + return ret; +} + int32_t native_rcl_take(void *subscription_handle, void *message_handle) { rcl_subscription_t * subscription = (rcl_subscription_t *)subscription_handle; @@ -278,6 +439,241 @@ int32_t native_rcl_take_response(void *client_handle, void *request_header_handl return ret; } +int32_t native_rcl_action_take_feedback(void *action_client_handle, void *feedback_message_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_take_feedback(action_client, feedback_message_handle); + return ret; +} + +int32_t native_rcl_action_take_status(void *action_client_handle, void *status_message_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_take_status(action_client, status_message_handle); + return ret; +} + +int32_t native_rcl_action_take_goal_response(void *action_client_handle, void *request_header_handle, void *goal_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_goal_response(action_client, request_header, goal_response_handle); + return ret; +} + +int32_t native_rcl_action_take_cancel_response(void *action_client_handle, void *request_header_handle, void *cancel_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_cancel_response(action_client, request_header, cancel_response_handle); + return ret; +} + +int32_t native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_result_response(action_client, request_header, result_response_handle); + return ret; +} + +int32_t native_rcl_action_take_goal_request(void *action_server_handle, void *request_header_handle, void *goal_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_goal_request(action_server, request_header, goal_request_handle); + return ret; +} + +int32_t native_rcl_action_send_goal_response(void *action_server_handle, void *request_header_handle, void *goal_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_goal_response(action_server, request_header, goal_response_handle); + return ret; +} + +int32_t native_rcl_action_accept_new_goal(void **action_goal_handle_handle, void *action_server_handle, void *goal_info_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rcl_action_goal_info_t * goal_info = (rcl_action_goal_info_t *)goal_info_handle; + + rcl_action_goal_handle_t *action_goal_handle = + (rcl_action_goal_handle_t *)malloc(sizeof(rcl_action_goal_handle_t)); + + *action_goal_handle = rcl_action_get_zero_initialized_goal_handle(); + + rcl_action_goal_handle_t *rcl_action_goal_handle = rcl_action_accept_new_goal(action_server, goal_info); + rcl_ret_t ret; + if (rcl_action_goal_handle == NULL) + { + ret = RCL_RET_ERROR; + } + else + { + // Copy out goal handle since action server storage disappears when it is fini'd. + *action_goal_handle = *rcl_action_goal_handle; + + // Get the goal_info from the goal_handle to return the stamp back to native code. + ret = rcl_action_goal_handle_get_info(action_goal_handle, goal_info); + } + + *action_goal_handle_handle = (void *)action_goal_handle; + + return ret; +} + +int32_t native_rcl_action_destroy_goal_handle(void *action_goal_handle) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle; + + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + free(goal_handle); + + return ret; +} + +int32_t native_rcl_action_update_goal_state(void *action_goal_handle_handle, int32_t goal_event) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, (rcl_action_goal_event_t)goal_event); + return ret; +} + +int32_t native_rcl_action_publish_status(void *action_server_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + + rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = rcl_action_publish_status(action_server, &status_message); + + rcl_ret_t cleanup_ret; + cleanup_ret = rcl_action_goal_status_array_fini(&status_message); + if (cleanup_ret != RCL_RET_OK) + { + // If we got two unexpected errors, return the earlier error. + if (ret == RCL_RET_OK) { + // Error message already set. + ret = cleanup_ret; + } + } + + return ret; +} + +int32_t native_rcl_action_publish_feedback(void *action_server_handle, void *feedback_message_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_publish_feedback(action_server, feedback_message_handle); + return ret; +} + +int32_t native_rcl_action_take_cancel_request(void *action_server_handle, void *request_header_handle, void *cancel_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_cancel_request(action_server, request_header, cancel_request_handle); + return ret; +} + +int32_t native_rcl_action_process_cancel_request(void *action_server_handle, void *cancel_request_handle, void *cancel_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + // the rcl_action_cancel_request_t is a direct typedef to + // action_msgs__srv__CancelGoal_Request + + // rcl_action_cancel_response_t is a wrapper struct around + // action_msgs__srv__CancelGoal_Response with an additional allocator field. + // -> Don't cast in this case! + + rcl_action_cancel_response_t tmp_cancel_response = rcl_action_get_zero_initialized_cancel_response(); + + rcl_ret_t ret = rcl_action_process_cancel_request(action_server, cancel_request_handle, &tmp_cancel_response); + + // TODO: (sh) would be better to copy the list over element by element? + + // HACK: Don't deallocate but instead move reference to data into incoming cancel_response_handle. + // rcl_action_cancel_response_fini(&cancel_response); + action_msgs__srv__CancelGoal_Response * cancel_response = (action_msgs__srv__CancelGoal_Response *)cancel_response_handle; + *cancel_response = tmp_cancel_response.msg; + + // HACK: as rcl_action_cancel_response_init doesn't fill in capacity... + cancel_response->goals_canceling.capacity = cancel_response->goals_canceling.size; + + return ret; +} + +int32_t native_rcl_action_send_cancel_response(void *action_server_handle, void *request_header_handle, void *cancel_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_cancel_response(action_server, request_header, cancel_response_handle); + return ret; +} + +int32_t native_rcl_action_take_result_request(void *action_server_handle, void *request_header_handle, void *result_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_result_request(action_server, request_header, result_request_handle); + return ret; +} + +int32_t native_rcl_action_send_result_response(void *action_server_handle, void *request_header_handle, void *result_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_result_response(action_server, request_header, result_response_handle); + return ret; +} + +int32_t native_rcl_action_notify_goal_done(void *action_server_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_notify_goal_done(action_server); + return ret; +} + +int32_t native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired) +{ + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + size_t num_expired_as_size_t; + + // Only provide one goal_info to the underlying function. + // rclcpp does the same as only one goal is expected to expire at the same time. + // The out parameter num_expired can be used with a loop to expire all goals. + // This does also help to avoid implementing a new SafeHandle and accessor methods for a list of `GoalHandle`s. + rcl_ret_t ret = rcl_action_expire_goals(action_server, goal_info_handle, 1, &num_expired_as_size_t); + + *num_expired = (int32_t)num_expired_as_size_t; + + return ret; +} + +int32_t native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + bool result = rcl_action_goal_handle_is_active(goal_handle); + return result ? 1 : 0; +} + +int32_t native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + rcl_action_goal_state_t status_as_rcl_type; + + rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status_as_rcl_type); + + *status = (int8_t)status_as_rcl_type; + + return ret; +} + int32_t native_rcl_create_qos_profile_handle(void **qos_profile_handle) { rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)malloc(sizeof(rmw_qos_profile_t)); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 288e134e..6c4252ff 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -20,6 +20,8 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_init(); +rcl_clock_t *native_rcl_get_default_clock(); + RCLDOTNET_EXPORT const char * RCLDOTNET_CDECL native_rcl_get_rmw_identifier(); @@ -72,6 +74,30 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_client(void *wait_set_handle, vo RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_guard_condition_handle(void *wait_set_handle, void *guard_condition_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_num_entries( + void *action_client_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_num_entries( + void *action_server_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_server(void *wait_set_handle, void *action_server_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); @@ -87,6 +113,25 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_client_handle, + int32_t *is_feedback_ready, + int32_t *is_status_ready, + int32_t *is_goal_response_ready, + int32_t *is_cancel_response_ready, + int32_t *is_result_response_ready); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_server_handle, + int32_t *is_goal_request_ready, + int32_t *is_cancel_request_ready, + int32_t *is_result_request_ready, + int32_t *is_goal_expired); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -108,6 +153,69 @@ int32_t RCLDOTNET_CDECL native_rcl_send_response(void *service_handle, void *req RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take_response(void *client_handle, void *request_header_handle, void *response_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_feedback(void *action_client_handle, void *feedback_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_status(void *action_client_handle, void *status_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_goal_response(void *action_client_handle, void *request_header_handle, void *goal_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_response(void *action_client_handle, void *request_header_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_goal_request(void *action_server_handle, void *request_header_handle, void *goal_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_goal_response(void *action_server_handle, void *request_header_handle, void *goal_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_accept_new_goal(void **action_goal_handle_handle, void *action_server_handle, void *goal_info_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_goal_handle(void *action_goal_handle_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_update_goal_state(void *action_goal_handle_handle, int32_t goal_event); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_publish_status(void *action_server_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_publish_feedback(void *action_server_handle, void *feedback_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_request(void *action_server_handle, void *request_header_handle, void *cancel_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_process_cancel_request(void *action_server_handle, void *cancel_request_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_response(void *action_server_handle, void *request_header_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_result_request(void *action_server_handle, void *request_header_handle, void *result_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_result_response(void *action_server_handle, void *request_header_handle, void *result_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_notify_goal_done(void *action_server_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_qos_profile_handle(void **qos_profile_handle); diff --git a/rcldotnet/rcldotnet_action_client.c b/rcldotnet/rcldotnet_action_client.c new file mode 100644 index 00000000..25d4ea52 --- /dev/null +++ b/rcldotnet/rcldotnet_action_client.c @@ -0,0 +1,61 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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 +#include +#include +#include +#include +#include +#include + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcldotnet_client.h" + +int32_t native_rcl_action_send_goal_request(void *action_client_handle, void *goal_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_goal_request(action_client, goal_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_send_result_request(void *action_client_handle, void *result_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_result_request(action_client, result_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_cancel_request(action_client, cancel_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_server_is_available(void *node_handle, void *client_handle, int32_t *is_available) { + rcl_node_t * node = (rcl_node_t *)node_handle; + rcl_action_client_t * client = (rcl_action_client_t *)client_handle; + + bool is_available_as_bool; + rcl_ret_t ret = rcl_action_server_is_available(node, client, &is_available_as_bool); + *is_available = is_available_as_bool ? 1 : 0; + + return ret; +} diff --git a/rcldotnet/rcldotnet_action_client.h b/rcldotnet/rcldotnet_action_client.h new file mode 100644 index 00000000..4fbd873f --- /dev/null +++ b/rcldotnet/rcldotnet_action_client.h @@ -0,0 +1,32 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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_CLIENT_H +#define RCLDOTNET_CLIENT_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_goal_request(void *action_client_handle, void *goal_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_result_request(void *action_client_handle, void *result_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_is_available(void *node_handle, void *action_client_handle, int32_t *is_available); + +#endif // RCLDOTNET_CLIENT_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index f430ec9e..3dfa900a 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -20,9 +20,11 @@ #include #include #include +#include #include "rosidl_runtime_c/message_type_support_struct.h" +#include "rcldotnet.h" #include "rcldotnet_node.h" int32_t native_rcl_create_publisher_handle(void **publisher_handle, @@ -153,7 +155,7 @@ int32_t native_rcl_create_client_handle(void **client_handle, rcl_client_options_t client_ops = rcl_client_get_default_options(); - rcl_ret_t ret = + rcl_ret_t ret = rcl_client_init(client, node, ts, service_name, &client_ops); *client_handle = (void *)client; @@ -170,3 +172,71 @@ int32_t native_rcl_destroy_client_handle(void *client_handle, void *node_handle) return ret; } + +int32_t native_rcl_action_create_client_handle(void **action_client_handle, + void *node_handle, + const char *action_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_action_type_support_t *ts = + (rosidl_action_type_support_t *)typesupport; + + rcl_action_client_t *action_client = + (rcl_action_client_t *)malloc(sizeof(rcl_action_client_t)); + *action_client = rcl_action_get_zero_initialized_client(); + rcl_action_client_options_t action_client_ops = + rcl_action_client_get_default_options(); + + rcl_ret_t ret = + rcl_action_client_init(action_client, node, ts, action_name, &action_client_ops); + + *action_client_handle = (void *)action_client; + + return ret; +} + +int32_t native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle) { + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_action_client_fini(action_client, node); + free(action_client); + + return ret; +} + +int32_t native_rcl_action_create_server_handle(void **action_server_handle, + void *node_handle, + const char *action_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_action_type_support_t *ts = + (rosidl_action_type_support_t *)typesupport; + + rcl_action_server_t *action_server = + (rcl_action_server_t *)malloc(sizeof(rcl_action_server_t)); + *action_server = rcl_action_get_zero_initialized_server(); + 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); + + *action_server_handle = (void *)action_server; + + return ret; +} + +int32_t native_rcl_action_destroy_server_handle(void *action_server_handle, void *node_handle) { + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_action_server_fini(action_server, node); + free(action_server); + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index c640a30a..f28da07e 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -53,4 +53,22 @@ int32_t RCLDOTNET_CDECL native_rcl_create_client_handle(void **client_handle, RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_destroy_client_handle(void *client_handle, void *node_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_create_client_handle(void **action_client_handle, + void *node_handle, + const char *action_name, + void *typesupport); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_create_server_handle(void **action_server_handle, + void *node_handle, + const char *action_name, + void *typesupport); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_server_handle(void *action_server_handle, void *node_handle); + #endif // RCLDOTNET_NODE_H diff --git a/rcldotnet/test/test_actions.cs b/rcldotnet/test/test_actions.cs new file mode 100644 index 00000000..78acf162 --- /dev/null +++ b/rcldotnet/test/test_actions.cs @@ -0,0 +1,240 @@ +/* Copyright 2023 Stefan Hoffmann + * + * 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.Collections.Generic; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; +using Xunit; + +namespace RCLdotnetTests +{ + // TODO: (sh) Notes for extended tests: + // - Add tests for: + // - ActionClientGoalHandle.Stamp + // - ActionServerGoalHandle.IsActive, .IsExecuting, .Status, .Execute() + // - Node.CreateActionServer() that overrides goalCallback + // - Node.CreateActionServer() that overrides cancelCallback that doesn't + // allow cancel. + // - Extend test coverage for: + // - ActionClientGoalHandle.Status + // - Test that invalid state-transitions throw exceptions. + // (e.g. ActionServerGoalHandle.Canceled() when the cancelCallback didn't + // allow cancellation) + // - Test like rclpy (look into rclcpp as well) with mock server/client to + // test the cases with different callback orderings. + // - https://github.com/ros2/rclpy/blob/rolling/rclpy/test/test_action_client.py + // - https://github.com/ros2/rclpy/blob/rolling/rclpy/test/test_action_server.py + + public sealed class TestActions + { + private readonly Node _serverNode; + private readonly Node _clientNode; + + // Each test method get's its own instance of this class, so no need to + // reset the fields here. + private Guid _serverGoalId; + private int _serverGoalOrder; + private List _clientFeedbackSequence = null; + + public TestActions() + { + RCLdotnet.Init(); + _serverNode = RCLdotnet.CreateNode("action_server_node"); + _clientNode = RCLdotnet.CreateNode("action_client_node"); + } + + [Fact] + public void TestActionClientServerIsReady() + { + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + SpinEachNodeOnce(); + Assert.False(actionClient.ServerIsReady()); + + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAccepted); + + SpinEachNodeOnce(); + Assert.True(actionClient.ServerIsReady()); + } + + [Fact] + public void TestActionSucceed() + { + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAccepted); + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + var goal = new Fibonacci_Goal(); + goal.Order = 3; + var goalHandleTask = actionClient.SendGoalAsync(goal, HandleFeedback); + + SpinEachNodeUntilTaskCompleted(goalHandleTask); + Assert.Equal(3, _serverGoalOrder); + Assert.NotNull(_clientFeedbackSequence); + Assert.Equal(new List { 0, 1 }, _clientFeedbackSequence); + + var goalHandle = goalHandleTask.GetAwaiter().GetResult(); + Assert.True(goalHandle.Accepted); + Assert.NotEqual(Guid.Empty, goalHandle.GoalId); + Assert.Equal(_serverGoalId, goalHandle.GoalId); + + var resultTask = goalHandle.GetResultAsync(); + SpinEachNodeUntilTaskCompleted(resultTask); + var result = resultTask.GetAwaiter().GetResult(); + Assert.Equal(new List { 0, 1 }, result.Sequence); + Assert.Equal(ActionGoalStatus.Succeeded, goalHandle.Status); + } + + [Fact] + public void TestActionAbort() + { + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAcceptedWithAbort); + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + var goal = new Fibonacci_Goal(); + goal.Order = 23; + var goalHandleTask = actionClient.SendGoalAsync(goal); + + SpinEachNodeUntilTaskCompleted(goalHandleTask); + Assert.Equal(23, _serverGoalOrder); + + var goalHandle = goalHandleTask.GetAwaiter().GetResult(); + Assert.True(goalHandle.Accepted); + Assert.NotEqual(Guid.Empty, goalHandle.GoalId); + Assert.Equal(_serverGoalId, goalHandle.GoalId); + + var resultTask = goalHandle.GetResultAsync(); + SpinEachNodeUntilTaskCompleted(resultTask); + var result = resultTask.GetAwaiter().GetResult(); + Assert.Equal(new List { -1 }, result.Sequence); + Assert.Equal(ActionGoalStatus.Aborted, goalHandle.Status); + } + + [Fact] + public void TestActionCanceled() + { + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAcceptedWithCanceled, cancelCallback: HandleCancel); + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + var goal = new Fibonacci_Goal(); + goal.Order = int.MaxValue; + var goalHandleTask = actionClient.SendGoalAsync(goal); + + SpinEachNodeUntilTaskCompleted(goalHandleTask); + Assert.Equal(int.MaxValue, _serverGoalOrder); + + var goalHandle = goalHandleTask.GetAwaiter().GetResult(); + Assert.True(goalHandle.Accepted); + Assert.NotEqual(Guid.Empty, goalHandle.GoalId); + Assert.Equal(_serverGoalId, goalHandle.GoalId); + + var cancelGoalTask = goalHandle.CancelGoalAsync(); + SpinEachNodeUntilTaskCompleted(cancelGoalTask); + cancelGoalTask.GetAwaiter().GetResult(); + + var resultTask = goalHandle.GetResultAsync(); + SpinEachNodeUntilTaskCompleted(resultTask); + var result = resultTask.GetAwaiter().GetResult(); + Assert.Equal(new List { 42 }, result.Sequence); + Assert.Equal(ActionGoalStatus.Canceled, goalHandle.Status); + } + + private void HandleAccepted(ActionServerGoalHandle goalHandle) + { + _serverGoalId = goalHandle.GoalId; + _serverGoalOrder = goalHandle.Goal.Order; + + var feedback = new Fibonacci_Feedback(); + feedback.Sequence = new List { 0, 1 }; + goalHandle.PublishFeedback(feedback); + + SpinEachNodeOnce(); + + var result = new Fibonacci_Result(); + result.Sequence = feedback.Sequence; + goalHandle.Succeed(result); + } + + private void HandleAcceptedWithAbort(ActionServerGoalHandle goalHandle) + { + _serverGoalId = goalHandle.GoalId; + _serverGoalOrder = goalHandle.Goal.Order; + + var result = new Fibonacci_Result(); + result.Sequence = new List { -1 }; + goalHandle.Abort(result); + } + + private void HandleAcceptedWithCanceled(ActionServerGoalHandle goalHandle) + { + _ = HandleAcceptedWithCanceledAsync(goalHandle); + } + + private async Task HandleAcceptedWithCanceledAsync(ActionServerGoalHandle goalHandle) + { + try + { + _serverGoalId = goalHandle.GoalId; + _serverGoalOrder = goalHandle.Goal.Order; + + var result = new Fibonacci_Result(); + result.Sequence = new List { 42 }; + + for (int i = 0; i < 20; i++) + { + if (goalHandle.IsCanceling) + { + goalHandle.Canceled(result); + return; + } + + await Task.Delay(50); + } + + // didn't get to the `IsCanceling` state -> abort that the final assertion fails + goalHandle.Abort(result); + } + catch (Exception exception) + { + Console.WriteLine(exception.Message); + } + } + + private CancelResponse HandleCancel(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Accept; + } + + private void HandleFeedback(Fibonacci_Feedback feedback) + { + _clientFeedbackSequence = feedback.Sequence; + } + + private void SpinEachNodeUntilTaskCompleted(Task task) + { + while (!task.IsCompleted) + { + SpinEachNodeOnce(); + } + } + + private void SpinEachNodeOnce() + { + RCLdotnet.SpinOnce(_serverNode, 50); + RCLdotnet.SpinOnce(_clientNode, 50); + } + } +} diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index 246c87b2..eb2f4387 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -10,6 +10,12 @@ find_package(DotNETExtra REQUIRED) set(CS_SOURCES DllLoadUtils.cs + IRosActionDefinition.cs + IRosActionFeedbackMessage.cs + IRosActionGetResultRequest.cs + IRosActionGetResultResponse.cs + IRosActionSendGoalRequest.cs + IRosActionSendGoalResponse.cs IRosMessage.cs IRosServiceDefinition.cs ) diff --git a/rcldotnet_common/IRosActionDefinition.cs b/rcldotnet_common/IRosActionDefinition.cs new file mode 100644 index 00000000..470df3db --- /dev/null +++ b/rcldotnet_common/IRosActionDefinition.cs @@ -0,0 +1,42 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // must be implemented on deriving types, gets called via reflection + // (static abstract interface members are not supported yet.) + // public static abstract IntPtr __GetTypeSupport(); + + // public static abstract IRosActionSendGoalRequest __CreateSendGoalRequest(); + // public static abstract SafeHandle __CreateSendGoalRequestHandle(); + + // public static abstract IRosActionSendGoalResponse __CreateSendGoalResponse(); + // public static abstract SafeHandle __CreateSendGoalResponseHandle(); + + // public static abstract IRosActionGetResultRequest __CreateGetResultRequest(); + // public static abstract SafeHandle __CreateGetResultRequestHandle(); + + // public static abstract IRosActionGetResultResponse __CreateGetResultResponse(); + // public static abstract SafeHandle __CreateGetResultResponseHandle(); + + // public static abstract IRosActionFeedbackMessage __CreateFeedbackMessage(); + // public static abstract SafeHandle __CreateFeedbackMessageHandle(); + } +} diff --git a/rcldotnet_common/IRosActionFeedbackMessage.cs b/rcldotnet_common/IRosActionFeedbackMessage.cs new file mode 100644 index 00000000..95ce0a5a --- /dev/null +++ b/rcldotnet_common/IRosActionFeedbackMessage.cs @@ -0,0 +1,30 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionFeedbackMessage : IRosMessage + where TFeedback : IRosMessage, new() + { + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } + + TFeedback Feedback { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionGetResultRequest.cs b/rcldotnet_common/IRosActionGetResultRequest.cs new file mode 100644 index 00000000..6313d6f3 --- /dev/null +++ b/rcldotnet_common/IRosActionGetResultRequest.cs @@ -0,0 +1,27 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionGetResultRequest : IRosMessage + { + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionGetResultResponse.cs b/rcldotnet_common/IRosActionGetResultResponse.cs new file mode 100644 index 00000000..283ab298 --- /dev/null +++ b/rcldotnet_common/IRosActionGetResultResponse.cs @@ -0,0 +1,25 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionGetResultResponse : IRosMessage + where TResult : IRosMessage, new() + { + sbyte Status { get; set; } + + TResult Result { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionSendGoalRequest.cs b/rcldotnet_common/IRosActionSendGoalRequest.cs new file mode 100644 index 00000000..52af436c --- /dev/null +++ b/rcldotnet_common/IRosActionSendGoalRequest.cs @@ -0,0 +1,49 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionSendGoalRequest : IRosMessage + where TGoal : IRosMessage, new() + { + // NOTICE: This would cause a cyclic reference: + // + // - `unique_identifier_msgs.msg.UUID` in the `unique_identifier_msgs` + // assembly references `IRosMessage` in the `rcldotnet_common` + // assembly. + // - `IRosActionSendGoalRequest` in the `rcldotnet_common` + // assembly references `unique_identifier_msgs.msg.UUID` in the + // `unique_identifier_msgs` assembly. + // + // So we need a workaround: + // - Use reflection later on to get to this. + // - Or use types like `byte[]` (or ValueTuple for + // `builtin_interfaces.msg.Time`) and generate accessor methods that + // convert and use those types. + // - Or provide property `IRosMessage GoalIdRosMessage { get; set; }` + // and cast to the concrete type on usage. + // + // The later one was chosen as it gives the most control over object + // references and avoids using of reflection. + // + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } + + TGoal Goal { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionSendGoalResponse.cs b/rcldotnet_common/IRosActionSendGoalResponse.cs new file mode 100644 index 00000000..c906400d --- /dev/null +++ b/rcldotnet_common/IRosActionSendGoalResponse.cs @@ -0,0 +1,29 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionSendGoalResponse : IRosMessage + { + bool Accepted { get; set; } + + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // builtin_interfaces.msg.Time Stamp { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage StampAsRosMessage { get; set; } + } +} diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index a5518444..7b95e6cc 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -7,8 +7,10 @@ find_package(rcldotnet REQUIRED) find_package(rcldotnet_common REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(test_msgs REQUIRED) find_package(rcldotnet REQUIRED) find_package(dotnet_cmake_module REQUIRED) @@ -19,8 +21,10 @@ find_package(DotNETExtra REQUIRED) set(_assemblies_dep_dlls ${rcldotnet_common_ASSEMBLIES_DLL} ${rcldotnet_ASSEMBLIES_DLL} + ${builtin_interfaces_ASSEMBLIES_DLL} ${std_msgs_ASSEMBLIES_DLL} ${std_srvs_ASSEMBLIES_DLL} + ${test_msgs_ASSEMBLIES_DLL} ) add_dotnet_executable(rcldotnet_talker @@ -51,10 +55,24 @@ add_dotnet_executable(rcldotnet_guard_condition ${_assemblies_dep_dlls} ) +add_dotnet_executable(rcldotnet_example_action_server + RCLDotnetActionServer.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + +add_dotnet_executable(rcldotnet_example_action_client + RCLDotnetActionClient.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + install_dotnet(rcldotnet_talker DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_listener DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_example_service DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_example_client DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_guard_condition DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_action_server DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_action_client DESTINATION lib/${PROJECT_NAME}/dotnet) ament_package() diff --git a/rcldotnet_examples/RCLDotnetActionClient.cs b/rcldotnet_examples/RCLDotnetActionClient.cs new file mode 100644 index 00000000..e90475f3 --- /dev/null +++ b/rcldotnet_examples/RCLDotnetActionClient.cs @@ -0,0 +1,87 @@ +using System; +using System.Threading; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; + +namespace ConsoleApplication +{ + public static class RCLDotnetActionClient + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("action_client"); + + var actionClient = node.CreateActionClient("fibonacci"); + + var cts = new CancellationTokenSource(); + var task = DoWorkAsync(actionClient, cts.Token); + + while (RCLdotnet.Ok()) + { + RCLdotnet.SpinOnce(node, 500); + + if (task.IsCompletedSuccessfully) + { + break; + } + else if (task.IsFaulted) + { + Console.WriteLine($"Task faulted. Exception {task.Exception}"); + break; + } + else if (task.IsCanceled) + { + Console.WriteLine("Task canceled."); + break; + } + } + + cts.Cancel(); + task.GetAwaiter().GetResult(); + } + + private static async Task DoWorkAsync( + ActionClient actionClient, + CancellationToken cancellationToken) + { + while (!actionClient.ServerIsReady()) + { + cancellationToken.ThrowIfCancellationRequested(); + + // NOTE: This causes the code to resume in an background worker Thread. + // Consider this when copying code from the example if additional synchronization is needed. + await Task.Delay(1000, cancellationToken); + } + + var goal = new Fibonacci_Goal(); + goal.Order = 10; + + Console.WriteLine("SendGoal"); + + ActionClientGoalHandle goalHandleForCallback = null; + + var goalHandle = await actionClient.SendGoalAsync(goal, (Fibonacci_Feedback feedback) => + { + Console.WriteLine($"Feedback: {string.Join(", ", feedback.Sequence)}"); + Console.WriteLine($"Status after Feedback: {goalHandleForCallback.Status}"); + }); + + goalHandleForCallback = goalHandle; + + if (goalHandle.Accepted) + { + Console.WriteLine("Goal accepted."); + + var result = await goalHandle.GetResultAsync(); + Console.WriteLine($"Result: {string.Join(", ", result.Sequence)}"); + Console.WriteLine($"Status: {goalHandle.Status}"); + } + else + { + Console.WriteLine("Goal not accepted."); + } + } + } +} diff --git a/rcldotnet_examples/RCLDotnetActionServer.cs b/rcldotnet_examples/RCLDotnetActionServer.cs new file mode 100644 index 00000000..5cb9f576 --- /dev/null +++ b/rcldotnet_examples/RCLDotnetActionServer.cs @@ -0,0 +1,70 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; + +namespace ConsoleApplication +{ + public static class RCLDotnetActionServer + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("action_server"); + + var actionServer = node.CreateActionServer("fibonacci", HandleAccepted, cancelCallback: HandleCancel); + + RCLdotnet.Spin(node); + } + + private static void HandleAccepted(ActionServerGoalHandle goalHandle) + { + // Don't block in the callback. + // -> Don't wait for the returned Task. + _ = DoWorkWithGoal(goalHandle); + } + + private static CancelResponse HandleCancel(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Accept; + } + + private static async Task DoWorkWithGoal(ActionServerGoalHandle goalHandle) + { + Console.WriteLine("Executing goal..."); + var feedback = new Fibonacci_Feedback(); + + feedback.Sequence = new List { 0, 1 }; + + for (int i = 1; i < goalHandle.Goal.Order; i++) + { + if (goalHandle.IsCanceling) + { + var cancelResult = new Fibonacci_Result(); + cancelResult.Sequence = feedback.Sequence; + + Console.WriteLine($"Canceled Result: {string.Join(", ", cancelResult.Sequence)}"); + goalHandle.Canceled(cancelResult); + return; + } + + feedback.Sequence.Add(feedback.Sequence[i] + feedback.Sequence[i - 1]); + + Console.WriteLine($"Feedback: {string.Join(", ", feedback.Sequence)}"); + goalHandle.PublishFeedback(feedback); + + // NOTE: This causes the code to resume in an background worker Thread. + // Consider this when copying code from the example if additional synchronization is needed. + await Task.Delay(1000); + } + + var result = new Fibonacci_Result(); + result.Sequence = feedback.Sequence; + + Console.WriteLine($"Result: {string.Join(", ", result.Sequence)}"); + goalHandle.Succeed(result); + } + } +} diff --git a/rcldotnet_examples/package.xml b/rcldotnet_examples/package.xml index 1a4ef781..602e7a01 100644 --- a/rcldotnet_examples/package.xml +++ b/rcldotnet_examples/package.xml @@ -13,18 +13,22 @@ rcldotnet_common rosidl_cmake + builtin_interfaces example_interfaces rcldotnet std_msgs std_srvs sensor_msgs + test_msgs rcldotnet_common + builtin_interfaces example_interfaces rcldotnet std_msgs std_srvs sensor_msgs + test_msgs ament_cmake diff --git a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake index 617654ce..52e9aadc 100644 --- a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake +++ b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake @@ -68,6 +68,17 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_module_name}.c" ) elseif(_parent_folder STREQUAL "action") + list(APPEND _generated_cs_files + "${_output_path}/${_parent_folder}/${_module_name}.cs" + ) + + list(APPEND _generated_h_files + "${_output_path}/${_parent_folder}/rcldotnet_${_module_name}.h" + ) + + list(APPEND _generated_c_ts_files + "${_output_path}/${_parent_folder}/${_module_name}.c" + ) else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") endif() @@ -87,13 +98,18 @@ endforeach() set(target_dependencies "${rosidl_generator_dotnet_BIN}" ${rosidl_generator_dotnet_GENERATOR_FILES} - "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.c.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.h.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.cs.em" - "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.h.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.h.em" ${rosidl_generate_interfaces_ABS_IDL_FILES} ${_dependency_files}) foreach(dep ${target_dependencies}) diff --git a/rosidl_generator_dotnet/resource/action.c.em b/rosidl_generator_dotnet/resource/action.c.em new file mode 100644 index 00000000..a9b7843d --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.c.em @@ -0,0 +1,8 @@ +@{ +type_name = action.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +}@ +const void * @(action_typename)__get_typesupport(void) { + const void * ptr = ROSIDL_TYPESUPPORT_INTERFACE__ACTION_SYMBOL_NAME(rosidl_typesupport_c, @(', '.join(action.namespaced_type.namespaced_name())))(); + return ptr; +} diff --git a/rosidl_generator_dotnet/resource/action.cs.em b/rosidl_generator_dotnet/resource/action.cs.em new file mode 100644 index 00000000..36bdb2bf --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.cs.em @@ -0,0 +1,143 @@ +@{ +from rosidl_generator_dotnet import get_field_name +from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_dotnet_type_for_message +from rosidl_generator_dotnet import get_builtin_dotnet_type +from rosidl_generator_dotnet import constant_value_to_dotnet + +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractString +from rosidl_parser.definition import AbstractWString +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import Array +from rosidl_parser.definition import BasicType +from rosidl_parser.definition import NamespacedType + +type_name = action.namespaced_type.name + +goal_type_name = get_dotnet_type_for_message(action.goal) +result_type_name = get_dotnet_type_for_message(action.result) +feedback_type_name = get_dotnet_type_for_message(action.feedback) + +send_goal_request_type_name = get_dotnet_type_for_message(action.send_goal_service.request_message) +send_goal_response_type_name = get_dotnet_type_for_message(action.send_goal_service.response_message) +get_result_request_type_name = get_dotnet_type_for_message(action.get_result_service.request_message) +get_result_response_type_name = get_dotnet_type_for_message(action.get_result_service.response_message) +feedback_message_type_name = get_dotnet_type_for_message(action.feedback_message) + +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +} +namespace @('.'.join(action.namespaced_type.namespaces)) +{ +@# sealed class with private constructor -> no instance can be created +@# static classes can't implement an interface (or any other basetype), +@# but we need some type that can hold the typesupport and be passed to the Node.CreateActionServce/Client() method. +@# So sealed + private constructor is as static as it gets. +@# static abstract interface members are currently in preview, so maybe we could use the feature in the future. +@# (if hey add support to derive from static only interfaces in static classes) +@# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. + public sealed class @(type_name) : global::ROS2.IRosActionDefinition< + global::@(goal_type_name), + global::@(result_type_name), + global::@(feedback_type_name)> + { + private static readonly DllLoadUtils dllLoadUtils; + + private @(type_name)() + { + } + + static @(type_name)() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativelibrary = dllLoadUtils.LoadLibrary("@(package_name)__dotnetext"); + + IntPtr native_get_typesupport_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(action_typename)__get_typesupport"); + + @(type_name).native_get_typesupport = (NativeGetTypeSupportType)Marshal.GetDelegateForFunctionPointer( + native_get_typesupport_ptr, typeof(NativeGetTypeSupportType)); + } + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + private delegate IntPtr NativeGetTypeSupportType(); + + private static NativeGetTypeSupportType native_get_typesupport = null; + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static IntPtr __GetTypeSupport() { + return native_get_typesupport(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionSendGoalRequest __CreateSendGoalRequest() + { + return new global::@(send_goal_request_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateSendGoalRequestHandle() + { + return global::@(send_goal_request_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionSendGoalResponse __CreateSendGoalResponse() + { + return new global::@(send_goal_response_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateSendGoalResponseHandle() + { + return global::@(send_goal_response_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionGetResultRequest __CreateGetResultRequest() + { + return new global::@(get_result_request_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateGetResultRequestHandle() + { + return global::@(get_result_request_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionGetResultResponse __CreateGetResultResponse() + { + return new global::@(get_result_response_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateGetResultResponseHandle() + { + return global::@(get_result_response_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionFeedbackMessage __CreateFeedbackMessage() + { + return new global::@(feedback_message_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateFeedbackMessageHandle() + { + return global::@(feedback_message_type_name).__CreateMessageHandle(); + } + } +} diff --git a/rosidl_generator_dotnet/resource/action.h.em b/rosidl_generator_dotnet/resource/action.h.em new file mode 100644 index 00000000..dab48679 --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.h.em @@ -0,0 +1,33 @@ +@{ +type_name = action.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +action_prefix = "RCLDOTNET_{0}_{1}_{2}".format(package_name, '_'.join(action.namespaced_type.namespaces), type_name).upper() +}@ +#if defined(_MSC_VER) + // Microsoft + #define @(action_prefix)_EXPORT __declspec(dllexport) + #define @(action_prefix)_IMPORT __declspec(dllimport) + #if defined(_M_IX86) + #define @(action_prefix)_CDECL __cdecl + #else + #define @(action_prefix)_CDECL + #endif +#elif defined(__GNUC__) + // GCC + #define @(action_prefix)_EXPORT __attribute__((visibility("default"))) + #define @(action_prefix)_IMPORT + #if defined(__i386__) + #define @(action_prefix)_CDECL __attribute__((__cdecl__)) + #else + #define @(action_prefix)_CDECL + #endif +#else + // do nothing and hope for the best? + #define @(action_prefix)_EXPORT + #define @(action_prefix)_IMPORT + #define @(action_prefix)_CDECL + #pragma warning Unknown dynamic link import/export semantics. +#endif + +@(action_prefix)_EXPORT +const void * @(action_prefix)_CDECL @(action_typename)__get_typesupport(void); diff --git a/rosidl_generator_dotnet/resource/idl.c.em b/rosidl_generator_dotnet/resource/idl.c.em index 14efb40c..2b2f4231 100644 --- a/rosidl_generator_dotnet/resource/idl.c.em +++ b/rosidl_generator_dotnet/resource/idl.c.em @@ -87,5 +87,74 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@#TODO - actions not implemented -@ \ No newline at end of file +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.c.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ +@ diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index 3cfc1ede..18a9b314 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -1,6 +1,9 @@ // generated from rosidl_generator_dotnet/resource/idl.cs.em // with input from @(package_name):@(interface_path) // generated code does not contain a copyright notice +@{ +from rosidl_generator_dotnet import get_dotnet_type_for_message +}@ @ @####################################################################### @# EmPy template for generating .cs files @@ -62,5 +65,84 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@#TODO - actions not implemented -@ \ No newline at end of file +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message, + action_interface='global::ROS2.IRosActionSendGoalRequest' % get_dotnet_type_for_message(action.goal) +) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message, + action_interface='global::ROS2.IRosActionSendGoalResponse' +) +}@ + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message, + action_interface='global::ROS2.IRosActionGetResultRequest' +) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message, + action_interface='global::ROS2.IRosActionGetResultResponse' % get_dotnet_type_for_message(action.result) +) +}@ + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message, + action_interface='global::ROS2.IRosActionFeedbackMessage' % get_dotnet_type_for_message(action.feedback) +) +}@ + +@{ +TEMPLATE( + 'action.cs.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ +@ diff --git a/rosidl_generator_dotnet/resource/idl.h.em b/rosidl_generator_dotnet/resource/idl.h.em index cf1fc096..e1439b98 100644 --- a/rosidl_generator_dotnet/resource/idl.h.em +++ b/rosidl_generator_dotnet/resource/idl.h.em @@ -68,7 +68,76 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@# TODO - actions not implemented +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.h.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.h.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.h.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ @ #endif // @(header_guard) diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 69350cde..5d4ca694 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -17,11 +17,17 @@ from rosidl_parser.definition import NamespacedType type_name = message.structure.namespaced_type.name msg_typename = '%s__%s' % ('__'.join(message.structure.namespaced_type.namespaces), type_name) + +if 'action_interface' not in locals(): + action_interface = None + +additional_interfaces_str = ', ' + action_interface if action_interface is not None else '' + } namespace @('.'.join(message.structure.namespaced_type.namespaces)) { -public class @(type_name) : global::ROS2.IRosMessage { +public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) { private static readonly DllLoadUtils dllLoadUtils; @[for member in message.structure.members]@ @@ -277,7 +283,7 @@ public class @(type_name) : global::ROS2.IRosMessage { IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @(get_field_name(type_name, member.name))[i__local_variable] = Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name))); @[ elif isinstance(member.type.value_type, AbstractWString)]@ - // TODO: Unicode types are not supported + // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name))[i__local_variable] = new @(get_dotnet_type(member.type.value_type))(); @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @@ -296,7 +302,7 @@ public class @(type_name) : global::ROS2.IRosMessage { IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @(get_field_name(type_name, member.name)).Add(Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name)))); @[ elif isinstance(member.type.value_type, AbstractWString)]@ - // TODO: Unicode types are not supported + // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name)).Add(new @(get_dotnet_type(member.type.value_type))()); @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @@ -348,7 +354,7 @@ public class @(type_name) : global::ROS2.IRosMessage { @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable), value__local_variable); @[ elif isinstance(member.type.value_type, AbstractWString)] -// TODO: Unicode types are not supported +// TODO: Unicode types are not supported @[ else]@ value__local_variable.__WriteToHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if]@ @@ -382,6 +388,25 @@ public class @(type_name) : global::ROS2.IRosMessage { @[ end if]@ @[end for]@ +@[if action_interface is not None and ( + action_interface.startswith("global::ROS2.IRosActionSendGoalRequest") or + action_interface.startswith("global::ROS2.IRosActionGetResultRequest") or + action_interface.startswith("global::ROS2.IRosActionFeedbackMessage") +)]@ + global::ROS2.IRosMessage @(action_interface).GoalIdAsRosMessage + { + get => GoalId; + set => GoalId = (global::unique_identifier_msgs.msg.UUID)value; + } +@[end if]@ +@[if action_interface is not None and action_interface.startswith("global::ROS2.IRosActionSendGoalResponse")]@ + global::ROS2.IRosMessage @(action_interface).StampAsRosMessage + { + get => Stamp; + set => Stamp = (global::builtin_interfaces.msg.Time)value; + } +@[end if]@ + private sealed class Safe@(type_name)Handle : global::System.Runtime.InteropServices.SafeHandle { public Safe@(type_name)Handle() : base(global::System.IntPtr.Zero, true) { } diff --git a/rosidl_generator_dotnet/resource/srv.cs.em b/rosidl_generator_dotnet/resource/srv.cs.em index bd541280..8231e40f 100644 --- a/rosidl_generator_dotnet/resource/srv.cs.em +++ b/rosidl_generator_dotnet/resource/srv.cs.em @@ -1,6 +1,7 @@ @{ from rosidl_generator_dotnet import get_field_name from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_dotnet_type_for_message from rosidl_generator_dotnet import get_builtin_dotnet_type from rosidl_generator_dotnet import constant_value_to_dotnet @@ -14,8 +15,10 @@ from rosidl_parser.definition import BasicType from rosidl_parser.definition import NamespacedType type_name = service.namespaced_type.name -request_type_name = service.request_message.structure.namespaced_type.name -response_type_name = service.response_message.structure.namespaced_type.name + +request_type_name = get_dotnet_type_for_message(service.request_message) +response_type_name = get_dotnet_type_for_message(service.response_message) + srv_typename = '%s__%s' % ('__'.join(service.namespaced_type.namespaces), type_name) } namespace @('.'.join(service.namespaced_type.namespaces)) @@ -27,7 +30,9 @@ namespace @('.'.join(service.namespaced_type.namespaces)) @# static abstract interface members are currently in preview, so maybe we could use the feature in the future. @# (if hey add support to derive from static only interfaces in static classes) @# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. - public sealed class @(type_name) : global::ROS2.IRosServiceDefinition<@(request_type_name), @(response_type_name)> + public sealed class @(type_name) : global::ROS2.IRosServiceDefinition< + global::@(request_type_name), + global::@(response_type_name)> { private static readonly DllLoadUtils dllLoadUtils; diff --git a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py index 95310ecc..39b67272 100644 --- a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py +++ b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py @@ -142,6 +142,11 @@ def get_dotnet_type(type_, use_primitives=True): return get_builtin_dotnet_type(type_.typename, use_primitives=use_primitives) +def get_dotnet_type_for_message(message): + return '%s.%s' % ( + '.'.join(message.structure.namespaced_type.namespaces), + message.structure.namespaced_type.name) + def msg_type_to_c(type_): if isinstance(type_, AbstractString): return 'char *'