fix: Fixed race condition in action server between is_ready and take" (…

…#2495)

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.

Co-authored-by: William Woodall <william@osrfoundation.org>
Signed-off-by: Janosch Machowinski <J.Machowinski@cellumation.com>

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"
@@ -345,7 +346,8 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
 
  /// Signature of a callback that accepts or rejects goal requests.
  using GoalCallback = std::function<GoalResponse(
- const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)>;
+ const GoalUUID &,
+ std::shared_ptr<const typename ActionT::Goal>)>;
  /// Signature of a callback that accepts or rejects requests to cancel a goal.
  using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
  /// Signature of a callback that is used to notify when the goal has been accepted.
@@ -455,7 +457,8 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
  void
  call_goal_accepted_callback(
  std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
- GoalUUID uuid, std::shared_ptr<void> goal_request_message) override
+ GoalUUID uuid,
+ std::shared_ptr<void> goal_request_message) override
  {
  std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
  std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();