actions: implement basic action server

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<TAction, TGoal, TResult, TFeedback> : ActionServerGoalHandle
@@ -60,26 +62,79 @@ public sealed class ActionServerGoalHandle<TAction, TGoal, TResult, TFeedback> :
         where TResult : IRosMessage, new()
         where TFeedback : IRosMessage, new()
     {
+        private readonly ActionServer<TAction, TGoal, TResult, TFeedback> _actionServer;
+
         // No public constructor.
-        internal ActionServerGoalHandle()
+        internal ActionServerGoalHandle(
+            SafeActionGoalHandle handle,
+            ActionServer<TAction, TGoal, TResult, TFeedback> actionServer,
+            Guid goalId,
+            TGoal goal)
         {
+            Handle = handle;
+            _actionServer = actionServer;
+            Goal = goal;
+            GoalId = goalId;
+            ResultTaskCompletionSource = new TaskCompletionSource<IRosActionGetResultResponse<TResult>>();
         }
 
         // `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<IRosActionGetResultResponse<TResult>> ResultTaskCompletionSource { get; }
 
-        public override bool IsCanceling => throw new NotImplementedException();
+        internal Task<IRosActionGetResultResponse<TResult>> 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<TFeedback> feedbackMessage =
+                ActionDefinitionStaticMemberCache<TAction, TGoal, TResult, TFeedback>.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.");
         }
     }
 }

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

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<ActionClient> _actionClients;
 
+        private readonly IList<ActionServer> _actionServers;
+
         internal Node(SafeNodeHandle handle)
         {
             Handle = handle;
@@ -181,6 +207,7 @@ internal Node(SafeNodeHandle handle)
             _clients = new List<Client>();
             _guardConditions = new List<GuardCondition>();
             _actionClients = new List<ActionClient>();
+            _actionServers = new List<ActionServer>();
         }
 
         public IList<Subscription> Subscriptions => _subscriptions;
@@ -196,6 +223,8 @@ internal Node(SafeNodeHandle handle)
 
         public IList<ActionClient> ActionClients => _actionClients;
 
+        public IList<ActionServer> 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<TAction, TGoal, TResult, TFeedback> CreateActionServer<TActi
             where TResult : IRosMessage, new()
             where TFeedback : IRosMessage, new()
         {
-            throw new NotImplementedException();
+            if (goalCallback == null)
+            {
+                goalCallback = DefaultGoalCallback;
+            }
+
+            if (cancelCallback == null)
+            {
+                cancelCallback = DefaultCancelCallback;
+            }
+
+            IntPtr typeSupport = ActionDefinitionStaticMemberCache<TAction, TGoal, TResult, TFeedback>.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<TAction, TGoal, TResult, TFeedback>(
+                actionServerHandle,
+                this,
+                acceptedCallback,
+                goalCallback,
+                cancelCallback);
+
+            _actionServers.Add(actionServer);
+            return actionServer;
+        }
+
+        private static GoalResponse DefaultGoalCallback<TGoal>(Guid goalId, TGoal goal)
+        {
+            return GoalResponse.AcceptAndExecute;
+        }
+
+        private static CancelResponse DefaultCancelCallback(ActionServerGoalHandle goalHandle)
+        {
+            return CancelResponse.Reject;
         }
     }
 }