Add support for ROS actions

[hoffmann-stefan]

extracted from https://github.com/ros2-dotnet/ros2_dotnet/pull/94 implements: https://github.com/ros2-dotnet/ros2_dotnet/issues/49

This PR adds support for action services and clients.

• [x] IDL message generation
• [x] Add basic action client
• [x] Add basic action service
• [x] Tests

Example

ActionClient

Note: For this example to work there needs to be an Thread/Task that runs RCLDotnet.Spin().

var node = RCLDotnet.CreateNode("test_node");

var actionClient = node.CreateActionClient<Fibonacci, Fibonacci_Goal, Fibonacci_Result, Fibonacci_Feedback>("fibonacci_action");

var goalHandle = await actionClient.SendGoalAsync(
    new Fibonacci_Goal() { Order = 10 },
    (Fibonacci_Feedback feedback) => Console.WriteLine( "Feedback: " + string.Join(", ", feedback.Sequence)));

Fibonacci_Result actionResult = await goalHandle.GetResultAsync();

Console.WriteLine("Result: " + string.Join(", ", actionResult.Sequence));
// or
await goalHandle.CancelGoalAsync();

ActionServer

public static class RCLDotnetActionServer
{
    public static void Main(string[] args)
    {
        RCLdotnet.Init();
        var node = RCLdotnet.CreateNode("action_server");
        var actionServer = node.CreateActionServer<Fibonacci, Fibonacci_Goal, Fibonacci_Result, Fibonacci_Feedback>("fibonacci", HandleAccepted, cancelCallback: HandleCancel);
        RCLdotnet.Spin(node);
    }

    private static void HandleAccepted(ActionServerGoalHandle<Fibonacci, Fibonacci_Goal, Fibonacci_Result, Fibonacci_Feedback> goalHandle)
    {
        // Don't block in the callback.
        // -> Don't wait for the returned Task.
        _ = DoWorkWithGoal(goalHandle);
    }

    private static CancelResponse HandleCancel(ActionServerGoalHandle<Fibonacci, Fibonacci_Goal, Fibonacci_Result, Fibonacci_Feedback> goalHandle)
    {
        return CancelResponse.Accept;
    }

    private static async Task DoWorkWithGoal(ActionServerGoalHandle<Fibonacci, Fibonacci_Goal, Fibonacci_Result, Fibonacci_Feedback> goalHandle)
    {
        Console.WriteLine("Executing goal...");
        var feedback = new Fibonacci_Feedback();

        feedback.Sequence = new List<int> { 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);
    }
}

Added public API

namespace ROS2
{
    public enum ActionGoalStatus
    {
        Unknown = 0,
        Accepted = 1,
        Executing = 2,
        Canceling = 3,
        Succeeded = 4,
        Canceled = 5,
        Aborted = 6,
    }

    public abstract class ActionClientGoalHandle
    {
        // no public constructor -> 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; }
    }

    public sealed class ActionClientGoalHandle<TAction, TGoal, TResult, TFeedback> : ActionClientGoalHandle
        where TAction : IRosActionDefinition<TGoal, TResult, TFeedback>
        where TGoal : IRosMessage, new()
        where TResult : IRosMessage, new()
        where TFeedback : IRosMessage, new()
    {
        public override Guid GoalId { get; }
        public override bool Accepted { get; }
        public override Time Stamp { get; }
        public override ActionGoalStatus Status { get; }

        public Task CancelGoalAsync();
        public Task<TResult> GetResultAsync();
    }

    public abstract class ActionClient
    {
        // no public constructor
        internal ActionClient() {}

        public abstract bool ServerIsReady();
    }

    public sealed class ActionClient<TAction, TGoal, TResult, TFeedback> : ActionClient
        where TAction : IRosActionDefinition<TGoal, TResult, TFeedback>
        where TGoal : IRosMessage, new()
        where TResult : IRosMessage, new()
        where TFeedback : IRosMessage, new()
    {
        public override bool ServerIsReady();

        public Task<ActionClientGoalHandle<TAction, TGoal, TResult, TFeedback>> SendGoalAsync(TGoal goal);
        public Task<ActionClientGoalHandle<TAction, TGoal, TResult, TFeedback>> SendGoalAsync(TGoal goal, Action<TFeedback> feedbackCallback);
    }   

    public enum GoalResponse
    {
        Default = 0,
        Reject = 1,
        AcceptAndExecute = 2,
        AcceptAndDefer = 3,
    }

    public enum CancelResponse
    {
        Default = 0,
        Reject = 1,
        Accept = 2,
    }

    public abstract class ActionServer
    {
        // no public constructor -> only allow internal subclasses
        internal ActionServer() {}
    }

    public sealed class ActionServer<TAction, TGoal, TResult, TFeedback> : ActionServer
        where TAction : IRosActionDefinition<TGoal, TResult, TFeedback>
        where TGoal : IRosMessage, new()
        where TResult : IRosMessage, new()
        where TFeedback : IRosMessage, new()
    {
        // no public constructor
        internal ActionServer() {}
    }

    public abstract class ActionServerGoalHandle
    {
        // no public constructor -> only allow internal subclasses
        internal ActionServerGoalHandle() {}

        public abstract Guid GoalId { get; }
        public abstract bool IsActive { get; }
        public abstract bool IsCanceling { get; }
        public abstract bool IsExecuting { get; }
        public abstract ActionGoalStatus Status { get; }

        public abstract void Execute();
    }

    public sealed class ActionServerGoalHandle<TAction, TGoal, TResult, TFeedback> : ActionServerGoalHandle
        where TAction : IRosActionDefinition<TGoal, TResult, TFeedback>
        where TGoal : IRosMessage, new()
        where TResult : IRosMessage, new()
        where TFeedback : IRosMessage, new()
    {
        // no public constructor
        internal ActionServerGoalHandle() {}

        public TGoal Goal { get; }
        public override Guid GoalId { get; }
        public override bool IsActive { get; }
        public override bool IsCanceling { get; }
        public override bool IsExecuting { get; }
        public override ActionGoalStatus Status { get; }

        public override void Execute();

        public void PublishFeedback(TFeedback feedback);

        public void Succeed(TResult result);
        public void Abort(TResult result);
        public void Canceled(TResult result);
    }

    public sealed partial class Node
    {
        public ActionClient<TAction, TGoal, TResult, TFeedback> CreateActionClient<TAction, TGoal, TResult, TFeedback>(string actionName)
            where TAction : IRosActionDefinition<TGoal, TResult, TFeedback>
            where TGoal : IRosMessage, new()
            where TResult : IRosMessage, new()
            where TFeedback : IRosMessage, new();

        public ActionServer<TAction, TGoal, TResult, TFeedback> CreateActionServer<TAction, TGoal, TResult, TFeedback>(
            string actionName,
            Action<ActionServerGoalHandle<TAction, TGoal, TResult, TFeedback>> acceptedCallback,
            Func<Guid, TGoal, GoalResponse> goalCallback = null,
            Func<ActionServerGoalHandle<TAction, TGoal, TResult, TFeedback>, CancelResponse> cancelCallback = null
        )
            where TAction : IRosActionDefinition<TGoal, TResult, TFeedback>
            where TGoal : IRosMessage, new()
            where TResult : IRosMessage, new()
            where TFeedback : IRosMessage, new();
    }

    public static class GuidExtensions
    {
        public static UUID ToUuidMsg(this Guid guid);
        public static byte[] ToUuidByteArray(this Guid guid);
        public static Guid ToGuid(this UUID uuidMsg);
    }
}

Interfaces for message generation

namespace ROS2
{
    public interface IRosActionDefinition<TGoal, TResult, TFeedback>
        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<TGoal> __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<TResult> __CreateGetResultResponse();
        // public static abstract SafeHandle __CreateGetResultResponseHandle();

        // public static abstract IRosActionFeedbackMessage<TFeedback> __CreateFeedbackMessage();
        // public static abstract SafeHandle __CreateFeedbackMessageHandle();
    }

    public interface IRosActionSendGoalRequest<TGoal> : 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<TGoal>` 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<int, int> 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; }
    }

    public interface IRosActionSendGoalResponse : IRosMessage
    {
        bool Accepted { get; set; }

        // NOTICE: cyclic reference, see `IRosActionSendGoalRequest<TGoal>`
        // 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; }
    }

    public interface IRosActionGetResultRequest : IRosMessage
    {
        // NOTICE: cyclic reference, see `IRosActionSendGoalRequest<TGoal>`
        // 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; }
    }

    public interface IRosActionGetResultResponse<TResult> : IRosMessage
        where TResult : IRosMessage, new()
    {
        sbyte Status { get; set; }

        TResult Result { get; set; }
    }

    public interface IRosActionFeedbackMessage<TFeedback> : IRosMessage
        where TFeedback : IRosMessage, new()
    {
        // NOTICE: cyclic reference, see `IRosActionSendGoalRequest<TGoal>`
        // 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; }
    }
}

[samiamlabs]

Hi @hoffmann-stefan!

Great to see that someone is finally adding actions!

I tried to build this in humble with Ubuntu 22.04 and got this:

Starting >>> rcldotnet
Starting >>> rosidl_generator_dotnet
Finished <<< rosidl_generator_dotnet [0.60s]                                                       
--- stderr: rcldotnet                              
CMake Warning (dev) at /usr/share/cmake-3.22/Modules/FindPackageHandleStandardArgs.cmake:438 (message):
  The package name passed to `find_package_handle_standard_args`
  (DOTNET_CORE) does not match the name of the calling package (DotNetCore).
  This can lead to problems in calling code that expects `find_package`
  result variables (e.g., `_FOUND`) to follow a certain pattern.
Call Stack (most recent call first):
  /opt/dependencies_ws/install/share/dotnet_cmake_module/cmake/Modules/dotnet/FindDotNetCore.cmake:37 (find_package_handle_standard_args)
  /opt/dependencies_ws/install/share/dotnet_cmake_module/cmake/Modules/FindCSBuild.cmake:20 (find_package)
  /opt/dependencies_ws/install/share/dotnet_cmake_module/cmake/Modules/FindDotNETExtra.cmake:15 (find_package)
  CMakeLists.txt:20 (find_package)
This warning is for project developers.  Use -Wno-dev to suppress it.

gmake[2]: *** [CMakeFiles/test_messages.dir/build.make:110: CMakeFiles/test_messages] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:193: CMakeFiles/test_messages.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< rcldotnet [5.47s, exited with code 2]

Any idea what the issue could be?

(edit) Commenting out the tests for rcldotnet made the build pass but I still get:

Finished <<< rcldotnet [1.75s]                     
Starting >>> rcldotnet_examples
--- stderr: rcldotnet_examples                              
gmake[2]: *** [CMakeFiles/rcldotnet_example_action_server.dir/build.make:76: CMakeFiles/rcldotnet_example_action_server] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:279: CMakeFiles/rcldotnet_example_action_server.dir/all] Error 2
gmake[1]: *** Waiting for unfinished jobs....
gmake[2]: *** [CMakeFiles/rcldotnet_example_action_client.dir/build.make:76: CMakeFiles/rcldotnet_example_action_client] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:305: CMakeFiles/rcldotnet_example_action_client.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< rcldotnet_examples [4.51s, exited with code 2]

(edit)

Haha, ok. I was missing that you need to rebuild the messages also. Still getting this error though....

Starting >>> lifecycle_msgs
--- stderr: builtin_interfaces                                                                                                                    
/bin/sh: 1: /home/ros/install/share/rosidl_generator_dotnet/cmake/../../../lib/rosidl_generator_dotnet/rosidl_generator_dotnet: Permission denied
gmake[2]: *** [CMakeFiles/builtin_interfaces__dotnet.dir/build.make:93: rosidl_generator_dotnet/builtin_interfaces/msg/duration.cs] Error 126
gmake[1]: *** [CMakeFiles/Makefile2:426: CMakeFiles/builtin_interfaces__dotnet.dir/all] Error 2
gmake[1]: *** Waiting for unfinished jobs....
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< builtin_interfaces [0.19s, exited with code 2]

(edit) This seemed to fix the problem:

ros@sam-blade:~/src/src/ros2_dotnet/rosidl_generator_dotnet/bin$ ll
total 12
drwxr-xr-x 2 ros ros 4096 Jun 15 16:32 ./
drwxr-xr-x 6 ros ros 4096 Jun 15 15:50 ../
-rw-r--r-- 1 ros ros 1390 Jun 15 15:50 rosidl_generator_dotnet
ros@sam-blade:~/src/src/ros2_dotnet/rosidl_generator_dotnet/bin$ sudo chmod u+x rosidl_generator_dotnet 
ros@sam-blade:~/src/src/ros2_dotnet/rosidl_generator_dotnet/bin$ ./rosidl_generator_dotnet 
usage: rosidl_generator_dotnet [-h] --generator-arguments-file GENERATOR_ARGUMENTS_FILE --typesupport-impls TYPESUPPORT_IMPLS
rosidl_generator_dotnet: error: the following arguments are required: --generator-arguments-file, --typesupport-impls

Still getting some errors when building rcldotnet_examples though :/

^[[39;49m^[[39;49m^[[91m/home/ros/src/src/ros2_dotnet/rcldotnet_examples/RCLDotnetActionClient.cs(5,17): error CS0234: The type or namespace name 'action' does not exist in the namespace 'test_msgs' (are you missing an assembly reference?) [/home/ros/build/rcldotnet_examples/rcldotnet_example_action_client/rcldotnet_example_action_client_dotnetcore.csproj]
^[[39;49m^[[39;49m^[[91m/home/ros/src/src/ros2_dotnet/rcldotnet_examples/RCLDotnetActionClient.cs(46,26): error CS0246: The type or namespace name 'Fibonacci' could not be found (are you missing a using directive or an assembly reference?) [/home/ros/build/rcldotnet_examples/rcldotnet_example_action_client/rcldotnet_example_action_client_dotnetcore.csproj]
^[[39;49m^[[39;49m^[[91m/home/ros/src/src/ros2_dotnet/rcldotnet_examples/RCLDotnetActionClient.cs(5,17): error CS0234: The type or namespace name 'action' does not exist in the namespace 'test_msgs' (are you missing an assembly reference?) [/home/ros/build/rcldotnet_examples/rcldotnet_example_action_client/rcldotnet_example_action_client_dotnetcore.csproj]
^[[39;49m^[[39;49m^[[91m/home/ros/src/src/ros2_dotnet/rcldotnet_examples/RCLDotnetActionClient.cs(46,26): error CS0246: The type or namespace name 'Fibonacci' could not be found (are you missing a using directive or an assembly reference?) [/home/ros/build/rcldotnet_examples/rcldotnet_example_action_client/rcldotnet_example_action_client_dotnetcore.csproj]

[hoffmann-stefan]

Hi @samiamlabs

Thanks for checking this out and testing :)

Could you try to build this with colcon build --event-handlers console_cohesion+, as otherwise the dotnet build errors don't show up in the output? (dotnet build dosn't print errors to stderr, colcon only displays stderr by default)

Also, could you try to delete the build and install folder as well to make a absolutely clean build? Most of those erros tend to go away if you do a complete rebuild. There seem to be some situations where things don't get rebuilt by cmake correclty, haven't figured this out yet though.

...
~/src/src/ros2_dotnet/rosidl_generator_dotnet/bin$ ./rosidl_generator_dotnet
...

I wouldn't try to run this script manually, it is intended to be called from the "IDL pipline" in ament.

[samiamlabs]

I wouldn't try to run this script manually, it is intended to be called from the "IDL pipline" in ament.

I did not run it manually, it was run by colcon and somehow got a permissions error.

I restarted the docker container I was using, and now it seems to work... strange. I can run the rcldotnet_examples action server and client at least. Great work!

I'm hoping to be able to use this with Behavior Designer in Unity 3D and finally get a proper behavior tree implementation to use for our robots.

A bit of topic, but I made this script for creating a standalone Unity plugin from ros2_dotnet from my fork of the project a while back: https://github.com/samiamlabs/ros2_dotnet/blob/cyclone/rcldotnet_utils/rcldotnet_utils/create_unity_plugin.py Do you know if there is something like that for the official ros2_dotnet that is up to date? Figuring out every file that needs to be copied over manually is very time-consuming and I'm guessing you still need to do a couple of rpath hacks to get around the LD_LIBRARY_PATH issues etc...

[hoffmann-stefan]

I restarted the docker container I was using, and now it seems to work... strange. I can run the rcldotnet_examples action server and client at least. Great work!

Thanks!

A bit of topic, but I made this script for creating a standalone Unity plugin from ros2_dotnet from my fork of the project a while back: https://github.com/samiamlabs/ros2_dotnet/blob/cyclone/rcldotnet_utils/rcldotnet_utils/create_unity_plugin.py Do you know if there is something like that for the official ros2_dotnet that is up to date? Figuring out every file that needs to be copied over manually is very time-consuming and I'm guessing you still need to do a couple of rpath hacks to get around the LD_LIBRARY_PATH issues etc...

Haven't done anything with Unity so far, I come from another background. So I'm not aware of it, there is only the section in the README as far as I know: https://github.com/ros2-dotnet/ros2_dotnet#using-generated-dlls-in-your-uwp-application-from-unity

As you said, having some sort of script for this would be nice. I would welcome this contribution :) Could you open a PR for this? Thanks in advance :)