From feb56f93f9f3815d88ec292360e26e5a81e2c1b9 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 15:58:29 +0200 Subject: [PATCH] actions: add example action client and server --- rcldotnet_examples/CMakeLists.txt | 18 +++++ rcldotnet_examples/RCLDotnetActionClient.cs | 87 +++++++++++++++++++++ rcldotnet_examples/RCLDotnetActionServer.cs | 70 +++++++++++++++++ rcldotnet_examples/package.xml | 4 + 4 files changed, 179 insertions(+) create mode 100644 rcldotnet_examples/RCLDotnetActionClient.cs create mode 100644 rcldotnet_examples/RCLDotnetActionServer.cs diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index 64d64f89..a4fb217e 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 @@ -45,9 +49,23 @@ add_dotnet_executable(rcldotnet_example_client ${_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_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..8308942c --- /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 RCLDotnetService + { + 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..e5097385 --- /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 RCLDotnetService + { + 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