fix: Fixed race condition in action server between is_ready and take_…

…data and execute

Some background information: is_ready, take_data and execute data
may be called from different threads in any order. The code in the old
state expected them to be called in series, without interruption.
This lead to multiple race conditions, as the state of the pimpl objects
was altered by the three functions in a non thread safe way.

This commit fixed this by
 - Introducing a data queue that is filled in is_ready
 - take_data from now only pops the front element of the data queue
 - A backlog of events if held in num_unreported_events_ which will be
   reported by future calls of is_ready

Signed-off-by: Janosch Machowinski <[email protected]>

rclcpp_action/include/rclcpp_action/server.hpp

@@ -22,6 +22,7 @@
 #include <unordered_map>
 #include <utility>
 
+#include "action_msgs/srv/cancel_goal.hpp"
 #include "rcl/event_callback.h"
 #include "rcl_action/action_server.h"
 #include "rosidl_runtime_c/action_type_support_struct.h"
@@ -279,19 +280,25 @@ class ServerBase : public rclcpp::Waitable
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute_goal_request_received(std::shared_ptr<void> & data);
+  execute_goal_request_received(
+    rcl_ret_t ret, rcl_action_goal_info_t goal_info, rmw_request_id_t request_header,
+    std::shared_ptr<void> message);
 
   /// Handle a request to cancel goals on the server
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute_cancel_request_received(std::shared_ptr<void> & data);
+  execute_cancel_request_received(
+    rcl_ret_t ret, std::shared_ptr<action_msgs::srv::CancelGoal::Request> request,
+    rmw_request_id_t request_header);
 
   /// Handle a request to get the result of an action
   /// \internal
   RCLCPP_ACTION_PUBLIC
   void
-  execute_result_request_received(std::shared_ptr<void> & data);
+  execute_result_request_received(
+    rcl_ret_t ret, std::shared_ptr<void> result_request,
+    rmw_request_id_t request_header);
 
   /// Handle a timeout indicating a completed goal should be forgotten by the server
   /// \internal