Implemented ROS actions

src/ros2cs/ros2cs_core/ActionServer.cs

@@ -0,0 +1,143 @@
+// Copyright 2019-2021 Robotec.ai
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//    http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+using System;
+using ROS2.Internal;
+using action_msgs.srv;
+using action_msgs.msg;
+
+namespace ROS2
+{
+
+  /// <summary> Enum to indicate handling of new goal </summary>
+  public enum ActionGoalResponse
+  {
+    REJECT,
+    ACCEPT_AND_EXECUTE,
+    ACCEPT_AND_DEFER
+  }
+
+  public class ActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse>: IActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse>
+    where TGoalRequest : Message, new()
+    where TGoalResponse : Message, new()
+    where TFeedback : Message, new()
+    where TResultRequest : Message, new()
+    where TResultResponse : Message, new()
+  {
+
+    /// <summary> Service to cancel action (same for every action) </summary>
+    private Service<CancelGoal_Request, CancelGoal_Response> serviceCancel;
+
+    /// <summary> Service to reply to a result request </summary>
+    private Service<TResultRequest, TResultResponse> serviceResult;
+
+    /// <summary> Service to register a new goal </summary>
+    private Service<TGoalRequest, TGoalResponse> serviceGoal;
+
+    /// <summary> Topic to which action status is published (same for every action) </summary>
+    private Publisher<GoalStatusArray> publisherStatus;
+
+    /// <summary> Topic to which action progress is published </summary>
+    private Publisher<TFeedback> publisherFeedback;
+
+    /// <summary>
+    /// Internal constructor
+    /// </summary>
+    /// <remarks>Use <see cref="INode.CreateActionServer"/> to construct new Instances</remarks>
+    internal ActionServer(
+      string topic,
+      Node node,
+      Func<TGoalRequest, ActionGoalResponse> handleGoal,
+      Func<CancelGoal_Request, CancelGoal_Response> handleCancel,
+      Action<TGoalRequest> handleAccepted
+    )
+    {
+      string prefix = topic + "_action/";
+
+      QualityOfServiceProfile qos_service = new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT);
+      QualityOfServiceProfile qos_status = new QualityOfServiceProfile(QosPresetProfile.ACTION_STATUS_DEFAULT);
+
+      // Default service and topic:
+
+      this.serviceCancel = node.CreateService<CancelGoal_Request, CancelGoal_Response>(
+        prefix + "cancel_goal",
+        handleCancel,
+        qos_service
+      );
+
+      this.publisherStatus = node.CreatePublisher<GoalStatusArray>(
+        prefix + "status",
+        qos_status
+      );
+
+      // Custom:
+
+      this.serviceGoal = node.CreateService<TGoalRequest, TGoalResponse>(
+        prefix + "send_goal",
+        onGoalReceive,
+        qos_service
+      );
+
+      this.serviceResult = node.CreateService<TResultRequest, TResultResponse>(
+        prefix + "get_result",
+        onResultReceive,
+        qos_service
+      );
+    }
+
+    public void Dispose()
+    {
+      DestroyActionServer();
+    }
+
+    /// <summary>
+    /// Callback for the goal service
+    ///
+    /// Most of the logic will be in the user defined function, but we wrap it here.
+    /// </summary>
+    private TGoalResponse onGoalReceive(TGoalRequest request)
+    {
+      TGoalResponse response = new TGoalResponse();
+      return response;
+    }
+
+    private TResultResponse onResultReceive(TResultRequest request)
+    {
+      TResultResponse response = new TResultResponse();
+      return response;
+    }
+
+    /// <summary>
+    /// Callback for cancel service
+    /// </summary>
+    private CancelGoal_Response defaultOnCancelReceive(CancelGoal_Request request)
+    {
+      CancelGoal_Response response = new CancelGoal_Response()
+      {
+        Return_code = CancelGoal_Response.ERROR_REJECTED
+      };
+      return response;
+    }
+
+
+    /// <inheritdoc/>
+    public bool IsDisposed { get { return IsDisposed; } }
+    private bool disposed = false;
+
+    private void DestroyActionServer()
+    {
+
+    }
+  }
+}

src/ros2cs/ros2cs_core/CMakeLists.txt

@@ -25,6 +25,7 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ros2cs_common REQUIRED)
+find_package(action_msgs REQUIRED)
 find_package(ament_cmake_export_assemblies REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(dotnet_cmake_module REQUIRED)
@@ -36,6 +37,7 @@ find_package(DotNETExtra REQUIRED)
 
 # Used by ros2cs_native
 find_package(rcl REQUIRED)
+find_package(rcl_action REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rosidl_generator_c REQUIRED)
@@ -63,6 +65,7 @@ add_library(
 
 ament_target_dependencies(ros2cs_native
   "rcl"
+  "rcl_action"
   "rcutils"
   "rmw"
   "rosidl_generator_c"
@@ -77,6 +80,7 @@ set(CS_INTERFACES
   interfaces/INode.cs
   interfaces/IClient.cs
   interfaces/IService.cs
+  interfaces/IActionServer.cs
   interfaces/IPublisher.cs
   interfaces/ISubscription.cs
 )
@@ -100,6 +104,7 @@ set(CS_SOURCES
   Clock.cs
   Client.cs
   Service.cs
+  ActionServer.cs
   Node.cs
   Publisher.cs
   QualityOfServiceProfile.cs
@@ -113,6 +118,10 @@ set(_assembly_deps_dll "")
 foreach(_assembly_dep ${ros2cs_common_ASSEMBLIES_DLL})
   list(APPEND _assembly_deps_dll "${_assembly_dep}")
 endforeach()
+foreach(_assembly_dep ${action_msgs_ASSEMBLIES_DLL})
+  list(APPEND _assembly_deps_dll "${_assembly_dep}")
+endforeach()
+
 
 add_dotnet_library(${PROJECT_NAME}
 SOURCES
@@ -125,8 +134,10 @@ install_dotnet(${PROJECT_NAME} DESTINATION lib/dotnet)
 ament_export_assemblies_dll("lib/dotnet/${PROJECT_NAME}.dll")
 
 ament_export_dependencies(ros2cs_common)
+ament_export_dependencies(action_msgs)
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(rcl)
+ament_export_dependencies(rcl_action)
 ament_export_dependencies(rosidl_generator_c)
 
 option(STANDALONE_BUILD "Deploy standalone libraries with build" OFF)

src/ros2cs/ros2cs_core/Node.cs

@@ -15,6 +15,8 @@
 using System;
 using System.Linq;
 using System.Collections.Generic;
+using action_msgs.srv;
+using action_msgs.msg;
 
 namespace ROS2
 {
@@ -65,6 +67,7 @@ internal List<IServiceBase> Services
     private HashSet<IPublisherBase> publishers;
     private HashSet<IClientBase> clients;
     private HashSet<IServiceBase> services;
+    private HashSet<IActionServerBase> action_servers;
     private readonly object mutex = new object();
     private bool disposed = false;
 
@@ -82,6 +85,7 @@ internal Node(string nodeName, ref rcl_context_t context)
       publishers = new HashSet<IPublisherBase>();
       clients = new HashSet<IClientBase>();
       services = new HashSet<IServiceBase>();
+      action_servers = new HashSet<IActionServerBase>();
 
       nodeHandle = NativeRcl.rcl_get_zero_initialized_node();
       defaultNodeOptions = NativeRclInterface.rclcs_node_create_default_options();
@@ -187,7 +191,7 @@ public bool RemoveClient(IClientBase client)
           return null;
         }
 
-	Service<I, O> service = new Service<I, O>(topic, this, callback, qos);
+        Service<I, O> service = new Service<I, O>(topic, this, callback, qos);
         services.Add(service);
         logger.LogInfo("Created service for topic " + topic);
         return service;
@@ -210,6 +214,41 @@ public bool RemoveService(IServiceBase service)
       }
     }
 
+    public ActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse>
+      CreateActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse>(
+        string topic,
+        Func<TGoalRequest, ActionGoalResponse> handleGoal,
+        Func<CancelGoal_Request, CancelGoal_Response> handleCancel,
+        Action<TGoalRequest> handleAccepted
+      )
+      where TGoalRequest : Message, new()
+      where TGoalResponse : Message, new()
+      where TFeedback : Message, new()
+      where TResultRequest : Message, new()
+      where TResultResponse : Message, new()
+    {
+      lock (mutex)
+      {
+        if (disposed || !Ros2cs.Ok())
+        {
+          logger.LogWarning("Cannot create action server as the class is already disposed or shutdown was called");
+          return null;
+        }
+
+        ActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse> action_server =
+          new ActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse>(
+            topic,
+            this,
+            handleGoal,
+            handleCancel,
+            handleAccepted
+          );
+        action_servers.Add(action_server);
+        logger.LogInfo("Created action server for topic " + topic);
+        return action_server;
+      }
+    }
+
     /// <summary> Create a publisher for this node for a given topic, qos and message type </summary>
     /// <see cref="INode.CreatePublisher"/>
     public Publisher<T> CreatePublisher<T>(string topic, QualityOfServiceProfile qos = null) where T : Message, new()

src/ros2cs/ros2cs_core/QualityOfServiceProfile.cs

@@ -25,7 +25,8 @@ public enum QosPresetProfile
     DEFAULT,
     SERVICES_DEFAULT,
     PARAMETER_EVENTS,
-    SYSTEM_DEFAULT
+    SYSTEM_DEFAULT,
+    ACTION_STATUS_DEFAULT,
   }
 
   public enum HistoryPolicy

src/ros2cs/ros2cs_core/interfaces/IActionServer.cs

@@ -0,0 +1,31 @@
+// Copyright 2019-2021 Robotec.ai
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//    http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+namespace ROS2
+{
+  public interface IActionServerBase : IExtendedDisposable
+  {
+
+  }
+
+  /// <summary> Generic base interface for all actions (either server or client) </summary>
+  public interface IActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse> : IActionServerBase
+    where TGoalRequest : Message, new()
+    where TGoalResponse : Message, new()
+    where TFeedback : Message, new()
+    where TResultRequest : Message, new()
+    where TResultResponse : Message, new()
+  {
+  }
+}

src/ros2cs/ros2cs_core/interfaces/INode.cs

@@ -14,6 +14,8 @@
 
 using System;
 using System.Collections.Generic;
+using action_msgs.srv;
+using action_msgs.msg;
 
 namespace ROS2
 {
@@ -47,6 +49,20 @@ public interface INode: IExtendedDisposable
     /// <returns> Service for the topic </returns>
     Service<I, O> CreateService<I, O>(string topic, Func<I, O> callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new();
 
+    /// <summary> Create an action server for this node </summary>
+    ActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse>
+      CreateActionServer<TGoalRequest, TGoalResponse, TFeedback, TResultRequest, TResultResponse>(
+        string topic,
+        Func<TGoalRequest, ActionGoalResponse> handleGoal,
+        Func<CancelGoal_Request, CancelGoal_Response> handleCancel,
+        Action<TGoalRequest> handleAccepted
+      )
+      where TGoalRequest : Message, new()
+      where TGoalResponse : Message, new()
+      where TFeedback : Message, new()
+      where TResultRequest : Message, new()
+      where TResultResponse : Message, new();
+
     /// <summary> Remove a service </summary>
     /// <remarks> Note that this does not call Dispose on Service </remarks>
     /// <param name="service"> Service created with earlier CreateService call </param>

src/ros2cs/ros2cs_core/native/rmw_native_interface.c

@@ -16,6 +16,7 @@
 #include <rmw/types.h>
 #include <rmw/rmw.h>
 #include <rcl/rcl.h>
+#include <rcl_action/default_qos.h>
 
 ROSIDL_GENERATOR_C_EXPORT
 rmw_qos_profile_t * rmw_native_interface_create_qos_profile(int profile)
@@ -27,7 +28,8 @@ rmw_qos_profile_t * rmw_native_interface_create_qos_profile(int profile)
      DEFAULT,
      SERVICES_DEFAULT,
      PARAMETER_EVENTS,
-     SYSTEM_DEFAULT
+     SYSTEM_DEFAULT,
+     ACTION_STATUS_DEFAULT
   };
 
   rmw_qos_profile_t * preset_profile = (rmw_qos_profile_t *)malloc(sizeof(rmw_qos_profile_t));
@@ -40,6 +42,7 @@ rmw_qos_profile_t * rmw_native_interface_create_qos_profile(int profile)
       case SERVICES_DEFAULT: *preset_profile = rmw_qos_profile_services_default; break;
       case PARAMETER_EVENTS: *preset_profile = rmw_qos_profile_parameter_events; break;
       case SYSTEM_DEFAULT: *preset_profile = rmw_qos_profile_system_default; break;
+      case ACTION_STATUS_DEFAULT: *preset_profile = rcl_action_qos_profile_status_default; break;
       default: *preset_profile = rmw_qos_profile_unknown; break;
   }