diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs index c6179589..c358f599 100644 --- a/rcldotnet/ActionServer.cs +++ b/rcldotnet/ActionServer.cs @@ -13,6 +13,16 @@ * 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 { /// @@ -66,12 +76,44 @@ public enum CancelResponse 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 @@ -80,9 +122,362 @@ public sealed class ActionServer : ActionSer 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; + + private readonly Action> _acceptedCallback; + + private readonly Func _goalCallback; + + private readonly Func, CancelResponse> _cancelCallback; + + private readonly ConcurrentDictionary> _goalHandles + = new ConcurrentDictionary>(); + // No public constructor. - internal ActionServer() + internal ActionServer( + SafeActionServerHandle handle, + Node node, + Action> acceptedCallback, + Func goalCallback, + Func, CancelResponse> cancelCallback) + { + Handle = handle; + _node = node; + _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) + { + actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); + PublishStatus(); + + 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 index 3a4198c6..f5d3e626 100644 --- a/rcldotnet/ActionServerGoalHandle.cs +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -14,6 +14,9 @@ */ using System; +using System.Threading.Tasks; +using action_msgs.msg; +using unique_identifier_msgs.msg; namespace ROS2 { @@ -46,12 +49,11 @@ internal ActionServerGoalHandle() 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 void Execute() - { - throw new NotImplementedException(); - } + public abstract void Execute(); } public sealed class ActionServerGoalHandle : ActionServerGoalHandle @@ -60,26 +62,79 @@ public sealed class ActionServerGoalHandle : where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + private readonly ActionServer _actionServer; + // No public constructor. - internal ActionServerGoalHandle() + 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 => throw new NotImplementedException(); + public override Guid GoalId { get; } + + public override bool IsActive + { + get + { + bool isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); + return isActive; + } + } + + 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; } - public override bool IsActive => throw new NotImplementedException(); + internal TaskCompletionSource> ResultTaskCompletionSource { get; } - public override bool IsCanceling => throw new NotImplementedException(); + internal Task> ResultTask => ResultTaskCompletionSource.Task; - public override bool IsExecuting => throw new NotImplementedException(); + public override void Execute() + { + UpdateGoalState(ActionGoalEvent.Execute); - public override ActionGoalStatus Status => throw new NotImplementedException(); + _actionServer.PublishStatus(); + } - public void PublishFeedback(TFeedback feedback) => throw new NotImplementedException(); + 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" @@ -90,17 +145,38 @@ internal ActionServerGoalHandle() // "rclcpp style" public void Succeed(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Succeed); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_SUCCEEDED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); } public void Abort(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Abort); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_ABORTED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); } public void Canceled(TResult result) { - throw new NotImplementedException(); + 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 957fc5e3..c6c3dd9d 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -48,6 +48,8 @@ set(CS_SOURCES RCLExceptionHelper.cs RCLRet.cs SafeActionClientHandle.cs + SafeActionGoalHandle.cs + SafeActionServerHandle.cs SafeClientHandle.cs SafeGuardConditionHandle.cs SafeNodeHandle.cs diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 8ca141bd..611b18ce 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -84,6 +84,18 @@ internal delegate RCLRet NativeRCLActionDestroyClientHandleType( 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(); @@ -158,6 +170,18 @@ static NodeDelegates() 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)); } } @@ -173,6 +197,8 @@ public sealed class Node private readonly IList _actionClients; + private readonly IList _actionServers; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -181,6 +207,7 @@ internal Node(SafeNodeHandle handle) _clients = new List(); _guardConditions = new List(); _actionClients = new List(); + _actionServers = new List(); } public IList Subscriptions => _subscriptions; @@ -196,6 +223,8 @@ internal Node(SafeNodeHandle handle) 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. @@ -338,7 +367,47 @@ public ActionServer CreateActionServer.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, + this, + 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/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 2f4ac0de..694c9846 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -192,6 +192,24 @@ internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( 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 bool isGoalRequestReady, out bool isCancelRequestReady, out bool isResultRequestReady, out bool 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); @@ -223,6 +241,90 @@ internal delegate RCLRet NativeRCLActionTakeResultResponseType( 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 bool 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; static RCLdotnetDelegates() { @@ -415,6 +517,24 @@ static RCLdotnetDelegates() (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 @@ -445,6 +565,101 @@ static RCLdotnetDelegates() (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)); } } @@ -542,6 +757,12 @@ private static void WaitSetAddActionClient(SafeWaitSetHandle waitSetHandle, Safe 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. /// @@ -746,6 +967,66 @@ private static void SendResponse(Service service, SafeRequestIdHandle requestHea } } + 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) + { + case RCLRet.Ok: + ReadFromMessageHandle(goalRequest, goalRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_goal_request)}() failed."); + } + } + } + + 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) + { + case RCLRet.Ok: + ReadFromMessageHandle(cancelRequest, cancelRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_cancel_request)}() failed."); + } + } + } + + 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) + { + case RCLRet.Ok: + ReadFromMessageHandle(resultRequest, resultRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_result_request)}() failed."); + } + } + } + public static void SpinOnce(Node node, long timeout) { int numberOfSubscriptions = node.Subscriptions.Count; @@ -774,6 +1055,25 @@ public static void SpinOnce(Node node, long timeout) 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 @@ -824,6 +1124,11 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddActionClient(waitSetHandle, actionClient.Handle); } + foreach (var actionServer in node.ActionServers) + { + WaitSetAddActionServer(waitSetHandle, actionServer.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { @@ -958,6 +1263,61 @@ public static void SpinOnce(Node node, long timeout) } } } + + foreach (var actionServer in node.ActionServers) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready( + waitSetHandle, + actionServer.Handle, + out bool isGoalRequestReady, + out bool isCancelRequestReady, + out bool isResultRequestReady, + out bool isGoalExpired); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready)}() failed."); + + if (isGoalRequestReady) + { + var goalRequest = actionServer.CreateSendGoalRequest(); + + var result = TakeGoalRequest(actionServer, requestIdHandle, goalRequest); + if (result) + { + actionServer.HandleGoalRequest(requestIdHandle, goalRequest); + } + } + + if (isCancelRequestReady) + { + var cancelRequest = new CancelGoal_Request(); + + var result = TakeCancelRequest(actionServer, requestIdHandle, cancelRequest); + if (result) + { + actionServer.HandleCancelRequest(requestIdHandle, cancelRequest); + } + } + + if (isResultRequestReady) + { + 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) + { + actionServer.HandleGoalExpired(); + } + } } int guardConditionIndex = 0; 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/rcldotnet.c b/rcldotnet/rcldotnet.c index 0e172d3a..cb47b3d9 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -26,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 @@ -39,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(); } @@ -213,6 +223,47 @@ int32_t native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void 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); @@ -288,6 +339,28 @@ int32_t native_rcl_action_client_wait_set_get_entities_ready( return ret; } +int32_t native_rcl_action_server_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_server_handle, + bool *is_goal_request_ready, + bool *is_cancel_request_ready, + bool *is_result_request_ready, + bool *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; + + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + action_server, + is_goal_request_ready, + is_cancel_request_ready, + is_result_request_ready, + is_goal_expired); + + return ret; +} + int32_t native_rcl_take(void *subscription_handle, void *message_handle) { rcl_subscription_t * subscription = (rcl_subscription_t *)subscription_handle; @@ -374,3 +447,199 @@ int32_t native_rcl_action_take_result_response(void *action_client_handle, void 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; +} + +bool 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; + + return rcl_action_goal_handle_is_active(goal_handle); +} + +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; +} diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index ff06497c..c4504440 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(); @@ -84,6 +86,18 @@ int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_num_entries( 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); @@ -109,6 +123,15 @@ int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( bool *is_cancel_response_ready, bool *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, + bool *is_goal_request_ready, + bool *is_cancel_request_ready, + bool *is_result_request_ready, + bool *is_goal_expired); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -145,4 +168,52 @@ int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_response(void *action_clie 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 +bool 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); + #endif // RCLDOTNET_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index 1844c033..aa7ca1a2 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -24,6 +24,7 @@ #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, @@ -188,3 +189,38 @@ int32_t native_rcl_action_destroy_client_handle(void *action_client_handle, void 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 707cd149..99c20cde 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -60,4 +60,13 @@ int32_t RCLDOTNET_CDECL native_rcl_action_create_client_handle(void **action_cli 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