rclcpp_action Server implementation (#593)

* Commiting to back up work, does not function

* Can call user callback when goal request received

* fini action server in destructor

* rename user callback virtual functions

* handle_execute test passes

* Remove out of date comment

* Refactor execute into three functions

* Remove unused file

* Add failing cancel test

* Cancel test passes

* Remove out of date comments

* Make sure server publishes status when accepting a goal

* Send status when goals transition to cancelling

* Refactored sending goal request to its own function

* Refactor cancel request into it's own function

* Comment with remaining tests

* Executing and terminal state statuses

* publish feedback works

* server sends result to clients that request it

* Remove out of date comment

* Add ServerGoalHandle::is_active()

* Cleanup when goals expire

* Can pass in action server options

* cpplint and uncrustify fixes

* Fix clang warnings

* Copy rcl goal handle

* Fix clang warning

* Use intermediate value to avoid left shift on 32bit integer

* RCLCPP_ACTION_PUBLIC everwhere

* Change callback parameter from C type to C++

* Add accessors for request and uuid

* Feedback must include goal id

* Document Server<> and ServerBase<>

* handle_execute -> handle_accepted

* Test deferred execution

* only publish feedback if goal is executing

* Documentation for ServerGoalHandle

* document msg parameters

* remove unnecessary fini

* notify_goal_done only takes server

* Use unique_indentifier_msgs

* create_server accepts group and removes waitable

* uncrustify

* Use weak ptr to avoid crash if goal handle lives longer than server

* Handle goal callback const message

* Goal handle doesn't have server pointer anymore

* Lock goal_handles_ on Server<>

* rcl_action_server_t protected with mutex

* ServerBase results protected with mutex

* protect rcl goal handle with mutex

* is_cancel_request -> is_canceling

* Add missing include

* use GoalID and change uuid -> goal_id

* Keep rcl goal handle alive until it expires on server

* uncrustify

* Move UUID hash

* Log messages in server

* ACTION -> ActionT

* Cancel abandoned goal handles

* Add convert() for C and C++ goal id

* Remove unused variable

* Constant reference

* Move variable declaration down

* is_ready if goal expired

* map[] default constructs if it doesn't exist

* Use rcl_action_get_goal_status_array()

* Array -> GoalID

* Use reentrant mutex for everything

* comment

* scope exit to fini cancel response

* using GoalID

rclcpp_action/CMakeLists.txt

@@ -22,6 +22,7 @@ set(${PROJECT_NAME}_SRCS
   src/client.cpp
   src/server.cpp
   src/server_goal_handle.cpp
+  src/types.cpp
 )
 
 add_library(${PROJECT_NAME}

rclcpp_action/include/rclcpp_action/create_server.hpp

@@ -15,7 +15,13 @@
 #ifndef RCLCPP_ACTION__CREATE_SERVER_HPP_
 #define RCLCPP_ACTION__CREATE_SERVER_HPP_
 
+#include <rcl_action/action_server.h>
+
 #include <rclcpp/node.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_clock_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <rclcpp/node_interfaces/node_waitables_interface.hpp>
 
 #include <memory>
 #include <string>
@@ -25,19 +31,59 @@
 
 namespace rclcpp_action
 {
-template<typename ACTION>
+template<typename ActionT>
-typename Server<ACTION>::SharedPtr
+typename Server<ActionT>::SharedPtr
 create_server(
-  rclcpp::Node * node,
+  rclcpp::Node::SharedPtr node,
   const std::string & name,
-  typename Server<ACTION>::GoalCallback handle_goal,
+  typename Server<ActionT>::GoalCallback handle_goal,
-  typename Server<ACTION>::CancelCallback handle_cancel)
+  typename Server<ActionT>::CancelCallback handle_cancel,
+  typename Server<ActionT>::AcceptedCallback handle_accepted,
+  const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
 {
-  return Server<ACTION>::make_shared(
+  std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
+    node->get_node_waitables_interface();
+  std::weak_ptr<rclcpp::callback_group::CallbackGroup> weak_group = group;
+  bool group_is_null = (nullptr == group.get());
+
+  auto deleter = [weak_node, weak_group, group_is_null](Server<ActionT> * ptr)
+    {
+      if (nullptr == ptr) {
+        return;
+      }
+      auto shared_node = weak_node.lock();
+      if (!shared_node) {
+        return;
+      }
+      // API expects a shared pointer, give it one with a deleter that does nothing.
+      std::shared_ptr<Server<ActionT>> fake_shared_ptr(ptr, [](Server<ActionT> *) {});
+
+      if (group_is_null) {
+        // Was added to default group
+        shared_node->remove_waitable(fake_shared_ptr, nullptr);
+      } else {
+        // Was added to a specfic group
+        auto shared_group = weak_group.lock();
+        if (shared_group) {
+          shared_node->remove_waitable(fake_shared_ptr, shared_group);
+        }
+      }
+      delete ptr;
+    };
+
+  std::shared_ptr<Server<ActionT>> action_server(new Server<ActionT>(
       node->get_node_base_interface(),
+      node->get_node_clock_interface(),
+      node->get_node_logging_interface(),
       name,
+      options,
       handle_goal,
-    handle_cancel);
+      handle_cancel,
+      handle_accepted), deleter);
+
+  node->get_node_waitables_interface()->add_waitable(action_server, group);
+  return action_server;
 }
 }  // namespace rclcpp_action
 #endif  // RCLCPP_ACTION__CREATE_SERVER_HPP_

rclcpp_action/include/rclcpp_action/server.hpp

@@ -15,81 +15,471 @@
 #ifndef RCLCPP_ACTION__SERVER_HPP_
 #define RCLCPP_ACTION__SERVER_HPP_
 
+#include <rcl_action/action_server.h>
 #include <rosidl_generator_c/action_type_support_struct.h>
 #include <rosidl_typesupport_cpp/action_type_support.hpp>
 #include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_clock_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <rclcpp/waitable.hpp>
 
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <string>
+#include <unordered_map>
+#include <utility>
 
 #include "rclcpp_action/visibility_control.hpp"
 #include "rclcpp_action/server_goal_handle.hpp"
+#include "rclcpp_action/types.hpp"
 
 namespace rclcpp_action
 {
 // Forward declaration
 class ServerBaseImpl;
 
+/// A response returned by an action server callback when a goal is requested.
+enum class GoalResponse : int8_t
+{
+  /// The goal is rejected and will not be executed.
+  REJECT = 1,
+  /// The server accepts the goal, and is going to begin execution immediately.
+  ACCEPT_AND_EXECUTE = 2,
+  /// The server accepts the goal, and is going to execute it later.
+  ACCEPT_AND_DEFER = 3,
+};
+
+/// A response returned by an action server callback when a goal has been asked to be canceled.
+enum class CancelResponse : int8_t
+{
+  /// The server will not try to cancel the goal.
+  REJECT = 1,
+  /// The server has agreed to try to cancel the goal.
+  ACCEPT = 2,
+};
+
 /// Base Action Server implementation
-/// It is responsible for interfacing with the C action server API.
+/// \internal
-class ServerBase
+/**
+ * This class should not be used directly by users writing an action server.
+ * Instead users should use `rclcpp_action::Server<>`.
+ *
+ * Internally, this class is responsible for interfacing with the `rcl_action` API.
+ */
+class ServerBase : public rclcpp::Waitable
 {
 public:
-  // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node
-  // TODO(sloretz) accept clock instance
-  RCLCPP_ACTION_PUBLIC
-  ServerBase(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
-    const std::string & name,
-    const rosidl_action_type_support_t * type_support);
-
   RCLCPP_ACTION_PUBLIC
   virtual ~ServerBase();
 
-  // TODO(sloretz) add a link between this class and callbacks in the server class
+  // -------------
+  // Waitables API
+
+  /// Return the number of subscriptions used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_subscriptions() override;
+
+  /// Return the number of timers used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_timers() override;
+
+  /// Return the number of service clients used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_clients() override;
+
+  /// Return the number of service servers used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_services() override;
+
+  /// Return the number of guard conditions used to implement an action server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  /// Add all entities to a wait set.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// Return true if any entity belonging to the action server is ready to be executed.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t *) override;
+
+  /// Act on entities in the wait set which are ready to be acted upon.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute() override;
+
+  // End Waitables API
+  // -----------------
+
+protected:
+  RCLCPP_ACTION_PUBLIC
+  ServerBase(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & name,
+    const rosidl_action_type_support_t * type_support,
+    const rcl_action_server_options_t & options);
+
+  // -----------------------------------------------------
+  // API for communication between ServerBase and Server<>
+
+  // ServerBase will call this function when a goal request is received.
+  // The subclass should convert to the real type and call a user's callback.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::pair<GoalResponse, std::shared_ptr<void>>
+  call_handle_goal_callback(GoalID &, std::shared_ptr<void> request) = 0;
+
+  // ServerBase will determine which goal ids are being cancelled, and then call this function for
+  // each goal id.
+  // The subclass should look up a goal handle and call the user's callback.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  CancelResponse
+  call_handle_cancel_callback(const GoalID & uuid) = 0;
+
+  /// Given a goal request message, return the UUID contained within.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  GoalID
+  get_goal_id_from_goal_request(void * message) = 0;
+
+  /// Create an empty goal request message so it can be taken from a lower layer.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  create_goal_request() = 0;
+
+  /// Call user callback to inform them a goal has been accepted.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  call_goal_accepted_callback(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
+    GoalID uuid, std::shared_ptr<void> goal_request_message) = 0;
+
+  /// Given a result request message, return the UUID contained within.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  GoalID
+  get_goal_id_from_result_request(void * message) = 0;
+
+  /// Create an empty goal request message so it can be taken from a lower layer.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  create_result_request() = 0;
+
+  /// Create an empty goal result message so it can be sent as a reply in a lower layer
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  std::shared_ptr<void>
+  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  publish_status();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  notify_goal_terminal_state();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg);
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  publish_feedback(std::shared_ptr<void> feedback_msg);
+
+  // End API for communication between ServerBase and Server<>
+  // ---------------------------------------------------------
 
 private:
+  /// Handle a request to add a new goal to the server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_goal_request_received();
+
+  /// Handle a request to cancel goals on the server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_cancel_request_received();
+
+  /// Handle a request to get the result of an action
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_result_request_received();
+
+  /// Handle a timeout indicating a completed goal should be forgotten by the server
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute_check_expired_goals();
+
+  /// Private implementation
+  /// \internal
   std::unique_ptr<ServerBaseImpl> pimpl_;
 };
 
-
+/// Action Server
-/// Templated Action Server class
+/**
-/// It is responsible for getting the C action type support struct from the C++ type, and
+ * This class creates an action server.
-/// calling user callbacks with goal handles of the appropriate type.
+ *
-template<typename ACTION>
+ * Create an instance of this server using `rclcpp_action::create_server()`.
-class Server : public ServerBase
+ *
+ * Internally, this class is responsible for:
+ *  - coverting between the C++ action type and generic types for `rclcpp_action::ServerBase`, and
+ *  - calling user callbacks.
+ */
+template<typename ActionT>
+class Server : public ServerBase, public std::enable_shared_from_this<Server<ActionT>>
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server)
 
-  using GoalCallback = std::function<void (rcl_action_goal_info_t &, typename ACTION::Goal *)>;
+  /// Signature of a callback that accepts or rejects goal requests.
-  using CancelCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ACTION>>)>;
+  using GoalCallback = std::function<GoalResponse(
+        const GoalID &, 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.
+  using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;
 
-  // TODO(sloretz) accept clock instance
+  /// Construct an action server.
+  /**
+   * This constructs an action server, but it will not work until it has been added to a node.
+   * Use `rclcpp_action::create_server()` to both construct and add to a node.
+   *
+   * Three callbacks must be provided:
+   *  - one to accept or reject goals sent to the server,
+   *  - one to accept or reject requests to cancel a goal,
+   *  - one to receive a goal handle after a goal has been accepted.
+   * All callbacks must be non-blocking.
+   * The result of a goal should be set using methods on `rclcpp_action::ServerGoalHandle<>`.
+   *
+   * \param[in] node_base a pointer to the base interface of a node.
+   * \param[in] node_clock a pointer to an interface that allows getting a node's clock.
+   * \param[in] node_logging a pointer to an interface that allows getting a node's logger.
+   * \param[in] name the name of an action.
+   *  The same name and type must be used by both the action client and action server to
+   *  communicate.
+   * \param[in] options options to pass to the underlying `rcl_action_server_t`.
+   * \param[in] handle_goal a callback that decides if a goal should be accepted or rejected.
+   * \param[in] handle_cancel a callback that decides if a goal should be attemted to be canceled.
+   *  The return from this callback only indicates if the server will try to cancel a goal.
+   *  It does not indicate if the goal was actually canceled.
+   * \param[in] handle_accepted a callback that is called to give the user a handle to the goal.
+   *  execution.
+   */
   Server(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
     const std::string & name,
+    const rcl_action_server_options_t & options,
     GoalCallback handle_goal,
-    CancelCallback handle_cancel
+    CancelCallback handle_cancel,
+    AcceptedCallback handle_accepted
   )
   : ServerBase(
       node_base,
+      node_clock,
+      node_logging,
       name,
-      rosidl_typesupport_cpp::get_action_type_support_handle<ACTION>()),
+      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
+      options),
     handle_goal_(handle_goal),
-    handle_cancel_(handle_cancel)
+    handle_cancel_(handle_cancel),
+    handle_accepted_(handle_accepted)
   {
-    // TODO(sloretz) what's the link that causes `handle_goal_` and `handle_cancel_` to be called?
   }
 
-  virtual ~Server()
+  virtual ~Server() = default;
+
+protected:
+  // -----------------------------------------------------
+  // API for communication between ServerBase and Server<>
+
+  /// \internal
+  std::pair<GoalResponse, std::shared_ptr<void>>
+  call_handle_goal_callback(GoalID & uuid, std::shared_ptr<void> message) override
   {
+    // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type
+    static_assert(
+      std::is_same<typename ActionT::Goal, typename ActionT::GoalRequestService::Request>::value,
+      "Assuming user fields were merged with goal request fields");
+    GoalResponse user_response = handle_goal_(
+      uuid, std::static_pointer_cast<typename ActionT::Goal>(message));
+
+    auto ros_response = std::make_shared<typename ActionT::GoalRequestService::Response>();
+    ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
+      GoalResponse::ACCEPT_AND_DEFER == user_response;
+    return std::make_pair(user_response, ros_response);
   }
 
+  /// \internal
+  CancelResponse
+  call_handle_cancel_callback(const GoalID & uuid) override
+  {
+    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    CancelResponse resp = CancelResponse::REJECT;
+    auto element = goal_handles_.find(uuid);
+    if (element != goal_handles_.end()) {
+      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
+      if (goal_handle) {
+        resp = handle_cancel_(goal_handle);
+        if (CancelResponse::ACCEPT == resp) {
+          goal_handle->_set_canceling();
+        }
+      }
+    }
+    return resp;
+  }
+
+  /// \internal
+  void
+  call_goal_accepted_callback(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
+    GoalID 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();
+
+    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state =
+      [weak_this](const GoalID & uuid, std::shared_ptr<void> result_message)
+      {
+        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
+        if (!shared_this) {
+          return;
+        }
+        // Send result message to anyone that asked
+        shared_this->publish_result(uuid, result_message);
+        // Publish a status message any time a goal handle changes state
+        shared_this->publish_status();
+        // notify base so it can recalculate the expired goal timer
+        shared_this->notify_goal_terminal_state();
+        // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires)
+        std::lock_guard<std::mutex> lock(shared_this->goal_handles_mutex_);
+        shared_this->goal_handles_.erase(uuid);
+      };
+
+    std::function<void(const GoalID &)> on_executing =
+      [weak_this](const GoalID & uuid)
+      {
+        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
+        if (!shared_this) {
+          return;
+        }
+        (void)uuid;
+        // Publish a status message any time a goal handle changes state
+        shared_this->publish_status();
+      };
+
+    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback =
+      [weak_this](std::shared_ptr<typename ActionT::Feedback> feedback_msg)
+      {
+        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
+        if (!shared_this) {
+          return;
+        }
+        shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
+      };
+
+    goal_handle.reset(
+      new ServerGoalHandle<ActionT>(
+        rcl_goal_handle, uuid,
+        std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
+        on_terminal_state, on_executing, publish_feedback));
+    {
+      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+      goal_handles_[uuid] = goal_handle;
+    }
+    handle_accepted_(goal_handle);
+  }
+
+  /// \internal
+  GoalID
+  get_goal_id_from_goal_request(void * message) override
+  {
+    return static_cast<typename ActionT::GoalRequestService::Request *>(message)->uuid;
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_goal_request() override
+  {
+    return std::shared_ptr<void>(new typename ActionT::GoalRequestService::Request());
+  }
+
+  /// \internal
+  GoalID
+  get_goal_id_from_result_request(void * message) override
+  {
+    return static_cast<typename ActionT::GoalResultService::Request *>(message)->uuid;
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_result_request() override
+  {
+    return std::shared_ptr<void>(new typename ActionT::GoalResultService::Request());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
+  {
+    auto result = std::make_shared<typename ActionT::GoalResultService::Response>();
+    result->status = status;
+    return std::static_pointer_cast<void>(result);
+  }
+
+  // End API for communication between ServerBase and Server<>
+  // ---------------------------------------------------------
+
 private:
   GoalCallback handle_goal_;
   CancelCallback handle_cancel_;
+  AcceptedCallback handle_accepted_;
+
+  using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
+  /// A map of goal id to goal handle weak pointers.
+  /// This is used to provide a goal handle to handle_cancel.
+  std::unordered_map<GoalID, GoalHandleWeakPtr> goal_handles_;
+  std::mutex goal_handles_mutex_;
 };
 }  // namespace rclcpp_action
 #endif  // RCLCPP_ACTION__SERVER_HPP_

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

@@ -15,41 +15,258 @@
 #ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
 #define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_
 
+#include <rcl_action/types.h>
 #include <rcl_action/goal_handle.h>
 
+#include <action_msgs/msg/goal_status.hpp>
+
 #include <functional>
 #include <memory>
+#include <mutex>
 
 #include "rclcpp_action/visibility_control.hpp"
+#include "rclcpp_action/types.hpp"
 
 namespace rclcpp_action
 {
-template<typename ACTION>
+
-class ServerGoalHandle
+/// Base class to interact with goals on a server.
+/// \internal
+/**
+ *
+ * This class in not be used directly by users writing an action server.
+ * Instead users will be given an instance of `rclcpp_action::ServerGoalHandle<>`.
+ *
+ * Internally, this class is responsible for interfacing with the `rcl_action` API.
+ */
+class ServerGoalHandleBase
 {
 public:
-  virtual ~ServerGoalHandle();
-
   /// Indicate if client has requested this goal be cancelled.
   /// \return true if a cancelation request has been accepted for this goal.
-  virtual bool
+  RCLCPP_ACTION_PUBLIC
-  is_cancel_request() const = 0;
+  bool
+  is_canceling() const;
 
-  /// Send an update about the progress of a goal.
+  /// Indicate if goal is pending or executing.
-  virtual void
+  /// \return false if goal has reached a terminal state.
-  publish_feedback(const typename ACTION::Feedback * feedback_msg) = 0;
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_active() const;
 
-  // TODO(sloretz) `set_cancelled`, `set_succeeded`, `set_aborted`
+  /// Indicate if goal is executing.
+  /// \return true only if the goal is in an executing state.
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_executing() const;
 
-  // TODO(sloretz) examples has this attribute as 'goal'
+  RCLCPP_ACTION_PUBLIC
-  /// The original request message describing the goal.
+  virtual
-  const typename ACTION::Goal goal_;
+  ~ServerGoalHandleBase();
 
 protected:
-  explicit ServerGoalHandle(const typename ACTION::Goal goal)
+  // -------------------------------------------------------------------------
-  : goal_(goal) {}
+  // API for communication between ServerGoalHandleBase and ServerGoalHandle<>
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  ServerGoalHandleBase(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle
+  )
+  : rcl_handle_(rcl_handle)
+  {
+  }
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_aborted();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_succeeded();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_canceling();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_canceled();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  _set_executing();
+
+  /// Transition the goal to canceled state if it never reached a terminal state.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  try_canceling() noexcept;
+
+  // End API for communication between ServerGoalHandleBase and ServerGoalHandle<>
+  // -----------------------------------------------------------------------------
+
+private:
+  std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;
+  mutable std::mutex rcl_handle_mutex_;
+};
+
+// Forward declare server
+template<typename ActionT>
+class Server;
+
+/// Class to interact with goals on a server.
+/**
+ * Use this class to check the status of a goal as well as set the result.
+ * This class is not meant to be created by a user, instead it is created when a goal has been
+ * accepted.
+ * The class `rclcpp_action::Server<>` will create an instance and give it to the user in their
+ * `handle_accepted` callback.
+ *
+ * Internally, this class is responsible for coverting between the C++ action type and generic
+ * types for `rclcpp_action::ServerGoalHandleBase`.
+ */
+template<typename ActionT>
+class ServerGoalHandle : public ServerGoalHandleBase
+{
+public:
+  /// Send an update about the progress of a goal.
+  /**
+   * This must only be called when the goal is executing.
+   * If execution of a goal is deferred then `ServerGoalHandle<>::set_executing()` must be called
+   * first.
+   * `std::runtime_error` is raised if the goal is in any state besides executing.
+   *
+   * \param[in] feedback_msg the message to publish to clients.
+   */
+  void
+  publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
+  {
+    feedback_msg->uuid = uuid_;
+    publish_feedback_(feedback_msg);
+  }
+
+  // TODO(sloretz) which exception is raised?
+  /// Indicate that a goal could not be reached and has been aborted.
+  /**
+   * Only call this if the goal was executing but cannot be completed.
+   * This is a terminal state, no more methods should be called on a goal handle after this is
+   * called.
+   * An exception is raised if the goal is in any state besides executing.
+   *
+   * \param[in] result_msg the final result to send to clients.
+   */
+  void
+  set_aborted(typename ActionT::Result::SharedPtr result_msg)
+  {
+    _set_aborted();
+    result_msg->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
+    on_terminal_state_(uuid_, result_msg);
+  }
+
+  /// Indicate that a goal has been reached.
+  /**
+   * Only call this if the goal is executing and has reached the desired final state.
+   * This is a terminal state, no more methods should be called on a goal handle after this is
+   * called.
+   * An exception is raised if the goal is in any state besides executing.
+   *
+   * \param[in] result_msg the final result to send to clients.
+   */
+  void
+  set_succeeded(typename ActionT::Result::SharedPtr result_msg)
+  {
+    _set_succeeded();
+    result_msg->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
+    on_terminal_state_(uuid_, result_msg);
+  }
+
+  /// Indicate that a goal has been canceled.
+  /**
+   * Only call this if the goal is executing or pending, but has been canceled.
+   * This is a terminal state, no more methods should be called on a goal handle after this is
+   * called.
+   * An exception is raised if the goal is in any state besides executing or pending.
+   *
+   * \param[in] result_msg the final result to send to clients.
+   */
+  void
+  set_canceled(typename ActionT::Result::SharedPtr result_msg)
+  {
+    _set_canceled();
+    result_msg->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
+    on_terminal_state_(uuid_, result_msg);
+  }
+
+  /// Indicate that the server is starting to execute a goal.
+  /**
+   * Only call this if the goal is pending.
+   * An exception is raised if the goal is in any state besides pending.
+   */
+  void
+  set_executing()
+  {
+    _set_executing();
+    on_executing_(uuid_);
+  }
+
+  /// Get the original request message describing the goal.
+  const std::shared_ptr<const typename ActionT::Goal>
+  get_goal() const
+  {
+    return goal_;
+  }
+
+  /// Get the unique identifier of the goal
+  const GoalID &
+  get_goal_id() const
+  {
+    return uuid_;
+  }
+
+  virtual ~ServerGoalHandle()
+  {
+    // Cancel goal if handle was allowed to destruct without reaching a terminal state
+    if (try_canceling()) {
+      auto null_result = std::make_shared<typename ActionT::Result>();
+      null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
+      on_terminal_state_(uuid_, null_result);
+    }
+  }
+
+protected:
+  /// \internal
+  ServerGoalHandle(
+    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
+    GoalID uuid,
+    std::shared_ptr<const typename ActionT::Goal> goal,
+    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state,
+    std::function<void(const GoalID &)> on_executing,
+    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback
+  )
+  : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
+    on_terminal_state_(on_terminal_state), on_executing_(on_executing),
+    publish_feedback_(publish_feedback)
+  {
+  }
+
+  /// The original request message describing the goal.
+  const std::shared_ptr<const typename ActionT::Goal> goal_;
+
+  /// A unique id for the goal request.
+  const GoalID uuid_;
+
+  friend Server<ActionT>;
+
+  std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state_;
+  std::function<void(const GoalID &)> on_executing_;
+  std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback_;
 };
 }  // namespace rclcpp_action
 
-#include <rclcpp_action/server_goal_handle_impl.hpp>  // NOLINT(build/include_order)
 #endif  // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_

rclcpp_action/include/rclcpp_action/server_goal_handle_impl.hpp

@@ -1,76 +0,0 @@
-// Copyright 2018 Open Source Robotics Foundation, Inc.
-//
-// 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.
-
-#ifndef RCLCPP_ACTION__SERVER_GOAL_HANDLE_IMPL_HPP_
-#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_IMPL_HPP_
-
-#include <rcl_action/goal_handle.h>
-#include <rcl_action/types.h>
-
-#include <memory>
-
-namespace rclcpp_action
-{
-template<typename ACTION>
-class Server;
-
-template<typename ACTION>
-class ServerGoalHandleImpl : public ServerGoalHandle<ACTION>
-{
-public:
-  virtual ~ServerGoalHandleImpl()
-  {
-  }
-
-  bool
-  is_cancel_request() const override
-  {
-    rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
-    if (RCL_RET_OK != rcl_action_goal_handle_get_status(rcl_handle_.get(), &state)) {
-      // TODO(sloretz) more specific exception
-      throw std::runtime_error("Failed to get goal handle state");
-    }
-    return GOAL_STATE_CANCELING == state;
-  }
-
-  void
-  publish_feedback(const typename ACTION::Feedback * feedback_msg) override
-  {
-    (void)feedback_msg;
-    // TODO(sloretz) what is ros_message and how does IntraProcessmessage come into play?
-    // if (RCL_RET_OK != rcl_action_publish_feedback(rcl_server_, ros_message) {
-    //   // TODO(sloretz) more specific exception
-    //   throw std::runtime_error("Failed to publish feedback");
-    // }
-    throw std::runtime_error("Failed to publish feedback");
-  }
-
-protected:
-  ServerGoalHandleImpl(
-    rcl_action_server_t * rcl_server,
-    const typename ACTION::Goal goal,
-    rcl_action_goal_handle_t * rcl_handle
-  )
-  : rcl_server_(rcl_server), rcl_handle_(rcl_handle), ServerGoalHandle<ACTION>(goal)
-  {
-  }
-
-private:
-  friend Server<ACTION>;
-  std::shared_ptr<rcl_action_server_t> rcl_server_;
-  std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;
-};
-}  // namespace rclcpp_action
-
-#endif  // RCLCPP_ACTION__SERVER_GOAL_HANDLE_IMPL_HPP_

rclcpp_action/include/rclcpp_action/types.hpp

@@ -20,8 +20,11 @@
 #include <action_msgs/msg/goal_status.hpp>
 #include <action_msgs/msg/goal_info.hpp>
 
+#include <climits>
 #include <functional>
+#include <string>
 
+#include "rclcpp_action/visibility_control.hpp"
 
 namespace rclcpp_action
 {
@@ -30,6 +33,20 @@ using GoalID = std::array<uint8_t, UUID_SIZE>;
 using GoalStatus = action_msgs::msg::GoalStatus;
 using GoalInfo = action_msgs::msg::GoalInfo;
 
+/// Convert a goal id to a human readable string.
+RCLCPP_ACTION_PUBLIC
+std::string
+to_string(const GoalID & goal_id);
+
+// Convert C++ GoalID to rcl_action_goal_info_t
+RCLCPP_ACTION_PUBLIC
+void
+convert(const GoalID & goal_id, rcl_action_goal_info_t * info);
+
+// Convert rcl_action_goal_info_t to C++ GoalID
+RCLCPP_ACTION_PUBLIC
+void
+convert(const rcl_action_goal_info_t & info, GoalID * goal_id);
 }  // namespace rclcpp_action
 
 namespace std
@@ -44,5 +61,24 @@ struct less<rclcpp_action::GoalID>
     return lhs < rhs;
   }
 };
+
+/// Hash a goal id so it can be used as a key in std::unordered_map
+template<>
+struct hash<rclcpp_action::GoalID>
+{
+  size_t operator()(const rclcpp_action::GoalID & uuid) const noexcept
+  {
+    // TODO(sloretz) Use someone else's hash function and cite it
+    size_t result = 0;
+    for (size_t i = 0; i < uuid.size(); ++i) {
+      for (size_t b = 0; b < sizeof(size_t); ++b) {
+        size_t part = uuid[i];
+        part <<= CHAR_BIT * b;
+        result ^= part;
+      }
+    }
+    return result;
+  }
+};
 }  // namespace std
 #endif  // RCLCPP_ACTION__TYPES_HPP_

rclcpp_action/src/server.cpp

@@ -12,30 +12,515 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <rcl_action/action_server.h>
+#include <rcl_action/wait.h>
+
+#include <action_msgs/msg/goal_status_array.hpp>
+#include <action_msgs/srv/cancel_goal.hpp>
+#include <rclcpp/exceptions.hpp>
+#include <rclcpp/scope_exit.hpp>
 #include <rclcpp_action/server.hpp>
 
+#include <memory>
+#include <mutex>
 #include <string>
+#include <unordered_map>
+#include <vector>
 
 using rclcpp_action::ServerBase;
+using rclcpp_action::GoalID;
 
 namespace rclcpp_action
 {
 class ServerBaseImpl
 {
-};
+public:
+  ServerBaseImpl(
+    rclcpp::Clock::SharedPtr clock,
+    rclcpp::Logger logger
+  )
+  : clock_(clock), logger_(logger)
+  {
   }
 
+  // Lock everything except user callbacks
+  std::recursive_mutex reentrant_mutex_;
+
+  std::shared_ptr<rcl_action_server_t> action_server_;
+
+  rclcpp::Clock::SharedPtr clock_;
+
+  size_t num_subscriptions_ = 0;
+  size_t num_timers_ = 0;
+  size_t num_clients_ = 0;
+  size_t num_services_ = 0;
+  size_t num_guard_conditions_ = 0;
+
+  bool goal_request_ready_ = false;
+  bool cancel_request_ready_ = false;
+  bool result_request_ready_ = false;
+  bool goal_expired_ = false;
+
+  // Results to be kept until the goal expires after reaching a terminal state
+  std::unordered_map<GoalID, std::shared_ptr<void>> goal_results_;
+  // Requests for results are kept until a result becomes available
+  std::unordered_map<GoalID, std::vector<rmw_request_id_t>> result_requests_;
+  // rcl goal handles are kept so api to send result doesn't try to access freed memory
+  std::unordered_map<GoalID, std::shared_ptr<rcl_action_goal_handle_t>> goal_handles_;
+
+  rclcpp::Logger logger_;
+};
+}  // namespace rclcpp_action
+
 ServerBase::ServerBase(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
   const std::string & name,
-  const rosidl_action_type_support_t * type_support)
+  const rosidl_action_type_support_t * type_support,
+  const rcl_action_server_options_t & options
+)
+: pimpl_(new ServerBaseImpl(
+      node_clock->get_clock(), node_logging->get_logger().get_child("rclcpp_action")))
 {
-  // TODO(sloretz) use rcl_action API when available
+  auto deleter = [node_base](rcl_action_server_t * ptr)
-  (void)node_base;
+    {
-  (void)name;
+      if (nullptr != ptr) {
-  (void)type_support;
+        rcl_node_t * rcl_node = node_base->get_rcl_node_handle();
+        rcl_ret_t ret = rcl_action_server_fini(ptr, rcl_node);
+        (void)ret;
+        RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"),
+          "failed to fini rcl_action_server_t in deleter");
+      }
+      delete ptr;
+    };
+
+  pimpl_->action_server_.reset(new rcl_action_server_t, deleter);
+  *(pimpl_->action_server_) = rcl_action_get_zero_initialized_server();
+
+  rcl_node_t * rcl_node = node_base->get_rcl_node_handle();
+  rcl_clock_t * rcl_clock = pimpl_->clock_->get_clock_handle();
+
+  rcl_ret_t ret = rcl_action_server_init(
+    pimpl_->action_server_.get(), rcl_node, rcl_clock, type_support, name.c_str(), &options);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  ret = rcl_action_server_wait_set_get_num_entities(
+    pimpl_->action_server_.get(),
+    &pimpl_->num_subscriptions_,
+    &pimpl_->num_guard_conditions_,
+    &pimpl_->num_timers_,
+    &pimpl_->num_clients_,
+    &pimpl_->num_services_);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
 }
 
 ServerBase::~ServerBase()
 {
 }
+
+size_t
+ServerBase::get_number_of_ready_subscriptions()
+{
+  return pimpl_->num_subscriptions_;
+}
+
+size_t
+ServerBase::get_number_of_ready_timers()
+{
+  return pimpl_->num_timers_;
+}
+
+size_t
+ServerBase::get_number_of_ready_clients()
+{
+  return pimpl_->num_clients_;
+}
+
+size_t
+ServerBase::get_number_of_ready_services()
+{
+  return pimpl_->num_services_;
+}
+
+size_t
+ServerBase::get_number_of_ready_guard_conditions()
+{
+  return pimpl_->num_guard_conditions_;
+}
+
+bool
+ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_wait_set_add_action_server(
+    wait_set, pimpl_->action_server_.get(), NULL);
+  return RCL_RET_OK == ret;
+}
+
+bool
+ServerBase::is_ready(rcl_wait_set_t * wait_set)
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
+    wait_set,
+    pimpl_->action_server_.get(),
+    &pimpl_->goal_request_ready_,
+    &pimpl_->cancel_request_ready_,
+    &pimpl_->result_request_ready_,
+    &pimpl_->goal_expired_);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  return pimpl_->goal_request_ready_ ||
+         pimpl_->cancel_request_ready_ ||
+         pimpl_->result_request_ready_ ||
+         pimpl_->goal_expired_;
+}
+
+void
+ServerBase::execute()
+{
+  if (pimpl_->goal_request_ready_) {
+    execute_goal_request_received();
+  } else if (pimpl_->cancel_request_ready_) {
+    execute_cancel_request_received();
+  } else if (pimpl_->result_request_ready_) {
+    execute_result_request_received();
+  } else if (pimpl_->goal_expired_) {
+    execute_check_expired_goals();
+  } else {
+    throw std::runtime_error("Executing action server but nothing is ready");
+  }
+}
+
+void
+ServerBase::execute_goal_request_received()
+{
+  rcl_ret_t ret;
+  rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
+  rmw_request_id_t request_header;
+
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+
+  std::shared_ptr<void> message = create_goal_request();
+  ret = rcl_action_take_goal_request(
+    pimpl_->action_server_.get(),
+    &request_header,
+    message.get());
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  pimpl_->goal_request_ready_ = false;
+  GoalID uuid = get_goal_id_from_goal_request(message.get());
+  convert(uuid, &goal_info);
+
+  // Call user's callback, getting the user's response and a ros message to send back
+  auto response_pair = call_handle_goal_callback(uuid, message);
+
+  ret = rcl_action_send_goal_response(
+    pimpl_->action_server_.get(),
+    &request_header,
+    response_pair.second.get());
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  const auto status = response_pair.first;
+
+  // if goal is accepted, create a goal handle, and store it
+  if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) {
+    RCLCPP_DEBUG(pimpl_->logger_, "Accepted goal %s", to_string(uuid).c_str());
+    // rcl_action will set time stamp
+    auto deleter = [](rcl_action_goal_handle_t * ptr)
+      {
+        if (nullptr != ptr) {
+          rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr);
+          (void)fail_ret;
+          RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"),
+            "failed to fini rcl_action_goal_handle_t in deleter");
+          delete ptr;
+        }
+      };
+    rcl_action_goal_handle_t * rcl_handle;
+    rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info);
+    if (!rcl_handle) {
+      throw std::runtime_error("Failed to accept new goal\n");
+    }
+
+    std::shared_ptr<rcl_action_goal_handle_t> handle(new rcl_action_goal_handle_t, deleter);
+    // Copy out goal handle since action server storage disappears when it is fini'd
+    *handle = *rcl_handle;
+
+    pimpl_->goal_handles_[uuid] = handle;
+
+    if (GoalResponse::ACCEPT_AND_EXECUTE == status) {
+      // Change status to executing
+      ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE);
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+    // publish status since a goal's state has changed (was accepted or has begun execution)
+    publish_status();
+
+    // Tell user to start executing action
+    call_goal_accepted_callback(handle, uuid, message);
+  }
+}
+
+void
+ServerBase::execute_cancel_request_received()
+{
+  rcl_ret_t ret;
+  rmw_request_id_t request_header;
+
+  // Initialize cancel request
+  auto request = std::make_shared<action_msgs::srv::CancelGoal::Request>();
+
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  ret = rcl_action_take_cancel_request(
+    pimpl_->action_server_.get(),
+    &request_header,
+    request.get());
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  // Convert c++ message to C message
+  rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
+  convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info);
+  cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec;
+  cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec;
+
+  // Get a list of goal info that should be attempted to be cancelled
+  rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response();
+
+  ret = rcl_action_process_cancel_request(
+    pimpl_->action_server_.get(),
+    &cancel_request,
+    &cancel_response);
+
+  RCLCPP_SCOPE_EXIT({
+    ret = rcl_action_cancel_response_fini(&cancel_response);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(pimpl_->logger_, "Failed to fini cancel response");
+    }
+  });
+
+  auto response = std::make_shared<action_msgs::srv::CancelGoal::Response>();
+
+  auto & goals = cancel_response.msg.goals_canceling;
+  // For each canceled goal, call cancel callback
+  for (size_t i = 0; i < goals.size; ++i) {
+    const rcl_action_goal_info_t & goal_info = goals.data[i];
+    GoalID uuid;
+    convert(goal_info, &uuid);
+    auto response_code = call_handle_cancel_callback(uuid);
+    if (CancelResponse::ACCEPT == response_code) {
+      action_msgs::msg::GoalInfo cpp_info;
+      cpp_info.goal_id.uuid = uuid;
+      cpp_info.stamp.sec = goal_info.stamp.sec;
+      cpp_info.stamp.nanosec = goal_info.stamp.nanosec;
+      response->goals_canceling.push_back(cpp_info);
+    }
+  }
+
+  if (!response->goals_canceling.empty()) {
+    // at least one goal state changed, publish a new status message
+    publish_status();
+  }
+
+  ret = rcl_action_send_cancel_response(
+    pimpl_->action_server_.get(), &request_header, response.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerBase::execute_result_request_received()
+{
+  rcl_ret_t ret;
+  // Get the result request message
+  rmw_request_id_t request_header;
+  std::shared_ptr<void> result_request = create_result_request();
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  ret = rcl_action_take_result_request(
+    pimpl_->action_server_.get(), &request_header, result_request.get());
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  std::shared_ptr<void> result_response;
+
+  // check if the goal exists
+  GoalID uuid = get_goal_id_from_result_request(result_request.get());
+  rcl_action_goal_info_t goal_info;
+  convert(uuid, &goal_info);
+  bool goal_exists;
+  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+  if (!goal_exists) {
+    // Goal does not exists
+    result_response = create_result_response(action_msgs::msg::GoalStatus::STATUS_UNKNOWN);
+  } else {
+    // Goal exists, check if a result is already available
+    auto iter = pimpl_->goal_results_.find(uuid);
+    if (iter != pimpl_->goal_results_.end()) {
+      result_response = iter->second;
+    }
+  }
+
+  if (result_response) {
+    // Send the result now
+    ret = rcl_action_send_result_response(
+      pimpl_->action_server_.get(), &request_header, result_response.get());
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  } else {
+    // Store the request so it can be responded to later
+    pimpl_->result_requests_[uuid].push_back(request_header);
+  }
+}
+
+void
+ServerBase::execute_check_expired_goals()
+{
+  // Allocate expecting only one goal to expire at a time
+  rcl_action_goal_info_t expired_goals[1];
+  size_t num_expired = 1;
+
+  // Loop in case more than 1 goal expired
+  while (num_expired > 0u) {
+    std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+    rcl_ret_t ret;
+    ret = rcl_action_expire_goals(pimpl_->action_server_.get(), expired_goals, 1, &num_expired);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    } else if (num_expired) {
+      // A goal expired!
+      GoalID uuid;
+      convert(expired_goals[0], &uuid);
+      RCLCPP_DEBUG(pimpl_->logger_, "Expired goal %s", to_string(uuid).c_str());
+      pimpl_->goal_results_.erase(uuid);
+      pimpl_->result_requests_.erase(uuid);
+      pimpl_->goal_handles_.erase(uuid);
+    }
+  }
+}
+
+void
+ServerBase::publish_status()
+{
+  rcl_ret_t ret;
+
+  // Get all goal handles known to C action server
+  rcl_action_goal_handle_t ** goal_handles = NULL;
+  size_t num_goals = 0;
+  ret = rcl_action_server_get_goal_handles(
+    pimpl_->action_server_.get(), &goal_handles, &num_goals);
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  auto status_msg = std::make_shared<action_msgs::msg::GoalStatusArray>();
+  status_msg->status_list.reserve(num_goals);
+  // Populate a c++ status message with the goals and their statuses
+  rcl_action_goal_status_array_t c_status_array =
+    rcl_action_get_zero_initialized_goal_status_array();
+  ret = rcl_action_get_goal_status_array(pimpl_->action_server_.get(), &c_status_array);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+
+  RCLCPP_SCOPE_EXIT({
+    ret = rcl_action_goal_status_array_fini(&c_status_array);
+    if (RCL_RET_OK != ret) {
+      RCLCPP_ERROR(pimpl_->logger_, "Failed to fini status array message");
+    }
+  });
+
+  for (size_t i = 0; i < c_status_array.msg.status_list.size; ++i) {
+    auto & c_status_msg = c_status_array.msg.status_list.data[i];
+
+    action_msgs::msg::GoalStatus msg;
+    msg.status = c_status_msg.status;
+    // Convert C goal info to C++ goal info
+    convert(c_status_msg.goal_info, &msg.goal_info.goal_id.uuid);
+    msg.goal_info.stamp.sec = c_status_msg.goal_info.stamp.sec;
+    msg.goal_info.stamp.nanosec = c_status_msg.goal_info.stamp.nanosec;
+
+    status_msg->status_list.push_back(msg);
+  }
+
+  // Publish the message through the status publisher
+  ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get());
+
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerBase::publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg)
+{
+  // Check that the goal exists
+  rcl_action_goal_info_t goal_info;
+  convert(uuid, &goal_info);
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  bool goal_exists;
+  goal_exists = rcl_action_server_goal_exists(pimpl_->action_server_.get(), &goal_info);
+
+  if (!goal_exists) {
+    throw std::runtime_error("Asked to publish result for goal that does not exist");
+  }
+
+  pimpl_->goal_results_[uuid] = result_msg;
+
+  // if there are clients who already asked for the result, send it to them
+  auto iter = pimpl_->result_requests_.find(uuid);
+  if (iter != pimpl_->result_requests_.end()) {
+    for (auto & request_header : iter->second) {
+      rcl_ret_t ret = rcl_action_send_result_response(
+        pimpl_->action_server_.get(), &request_header, result_msg.get());
+      if (RCL_RET_OK != ret) {
+        rclcpp::exceptions::throw_from_rcl_error(ret);
+      }
+    }
+  }
+}
+
+void
+ServerBase::notify_goal_terminal_state()
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_notify_goal_done(pimpl_->action_server_.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerBase::publish_feedback(std::shared_ptr<void> feedback_msg)
+{
+  std::lock_guard<std::recursive_mutex> lock(pimpl_->reentrant_mutex_);
+  rcl_ret_t ret = rcl_action_publish_feedback(pimpl_->action_server_.get(), feedback_msg.get());
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback");
+  }
+}

rclcpp_action/src/server_goal_handle.cpp

@@ -12,4 +12,140 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <rcl_action/action_server.h>
+#include <rcl_action/goal_handle.h>
+
 #include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/exceptions.hpp>
+
+#include <memory>
+
+namespace rclcpp_action
+{
+ServerGoalHandleBase::~ServerGoalHandleBase()
+{
+}
+
+bool
+ServerGoalHandleBase::is_canceling() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
+  }
+  return GOAL_STATE_CANCELING == state;
+}
+
+bool
+ServerGoalHandleBase::is_active() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  return rcl_action_goal_handle_is_active(rcl_handle_.get());
+}
+
+bool
+ServerGoalHandleBase::is_executing() const
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
+  }
+  return GOAL_STATE_EXECUTING == state;
+}
+
+void
+ServerGoalHandleBase::_set_aborted()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_ABORTED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_succeeded()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_SUCCEEDED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_canceling()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_canceled()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+void
+ServerGoalHandleBase::_set_executing()
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_EXECUTE);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret);
+  }
+}
+
+bool
+ServerGoalHandleBase::try_canceling() noexcept
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  // Check if the goal reached a terminal state already
+  const bool active = rcl_action_goal_handle_is_active(rcl_handle_.get());
+  if (!active) {
+    return false;
+  }
+
+  rcl_ret_t ret;
+
+  // Get the current state
+  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
+  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    return false;
+  }
+
+  // If it's not already canceling then transition to that state
+  if (GOAL_STATE_CANCELING != state) {
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL);
+    if (RCL_RET_OK != ret) {
+      return false;
+    }
+  }
+
+  // Get the state again
+  ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
+  if (RCL_RET_OK != ret) {
+    return false;
+  }
+
+  // If it's canceling, cancel it
+  if (GOAL_STATE_CANCELING == state) {
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SET_CANCELED);
+    return RCL_RET_OK == ret;
+  }
+
+  return false;
+}
+}  // namespace rclcpp_action

rclcpp_action/src/types.cpp

@@ -0,0 +1,48 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#include "rclcpp_action/types.hpp"
+
+#include <string>
+#include <sstream>
+
+namespace rclcpp_action
+{
+std::string
+to_string(const GoalID & goal_id)
+{
+  std::stringstream stream;
+  stream << std::hex;
+  for (const auto & element : goal_id) {
+    stream << static_cast<int>(element);
+  }
+  return stream.str();
+}
+
+void
+convert(const GoalID & goal_id, rcl_action_goal_info_t * info)
+{
+  for (size_t i = 0; i < 16; ++i) {
+    info->goal_id.uuid[i] = goal_id[i];
+  }
+}
+
+void
+convert(const rcl_action_goal_info_t & info, GoalID * goal_id)
+{
+  for (size_t i = 0; i < 16; ++i) {
+    (*goal_id)[i] = info.goal_id.uuid[i];
+  }
+}
+}  // namespace rclcpp_action

rclcpp_action/test/test_server.cpp

@@ -20,10 +20,13 @@
 #include <gtest/gtest.h>
 
 #include <memory>
+#include <vector>
 
 #include "rclcpp_action/create_server.hpp"
 #include "rclcpp_action/server.hpp"
 
+using Fibonacci = test_msgs::action::Fibonacci;
+using GoalID = rclcpp_action::GoalID;
 
 class TestServer : public ::testing::Test
 {
@@ -32,15 +35,609 @@ protected:
   {
     rclcpp::init(0, nullptr);
   }
+
+  std::shared_ptr<Fibonacci::GoalRequestService::Request>
+  send_goal_request(rclcpp::Node::SharedPtr node, GoalID uuid)
+  {
+    auto client = node->create_client<Fibonacci::GoalRequestService>(
+      "fibonacci/_action/send_goal");
+    if (!client->wait_for_service(std::chrono::seconds(20))) {
+      throw std::runtime_error("send goal service didn't become available");
+    }
+    auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
+    request->uuid = uuid;
+    auto future = client->async_send_request(request);
+    if (rclcpp::executor::FutureReturnCode::SUCCESS !=
+      rclcpp::spin_until_future_complete(node, future))
+    {
+      throw std::runtime_error("send goal future didn't complete succesfully");
+    }
+    return request;
+  }
+
+  void
+  send_cancel_request(rclcpp::Node::SharedPtr node, GoalID uuid)
+  {
+    auto cancel_client = node->create_client<Fibonacci::CancelGoalService>(
+      "fibonacci/_action/cancel_goal");
+    if (!cancel_client->wait_for_service(std::chrono::seconds(20))) {
+      throw std::runtime_error("cancel goal service didn't become available");
+    }
+    auto request = std::make_shared<Fibonacci::CancelGoalService::Request>();
+    request->goal_info.goal_id.uuid = uuid;
+    auto future = cancel_client->async_send_request(request);
+    if (rclcpp::executor::FutureReturnCode::SUCCESS !=
+      rclcpp::spin_until_future_complete(node, future))
+    {
+      throw std::runtime_error("cancel goal future didn't complete succesfully");
+    }
+  }
 };
 
 TEST_F(TestServer, construction_and_destruction)
 {
-  auto node = std::make_shared<rclcpp::Node>("my_node", "/rclcpp_action/test/server");
+  auto node = std::make_shared<rclcpp::Node>("construct_node", "/rclcpp_action/construct");
 
-  using GoalHandle = rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>;
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
-  auto as = rclcpp_action::create_server<test_msgs::action::Fibonacci>(node.get(), "fibonacci",
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
-      [](rcl_action_goal_info_t &, test_msgs::action::Fibonacci::Goal *) {},
+      [](const GoalID &, std::shared_ptr<const Fibonacci::Goal>) {
+        return rclcpp_action::GoalResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
       [](std::shared_ptr<GoalHandle>) {});
   (void)as;
 }
+
+TEST_F(TestServer, handle_goal_called)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_goal_node", "/rclcpp_action/handle_goal");
+  GoalID received_uuid;
+
+  auto handle_goal = [&received_uuid](
+    const GoalID & uuid, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      received_uuid = uuid;
+      return rclcpp_action::GoalResponse::REJECT;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      [](std::shared_ptr<GoalHandle>) {});
+  (void)as;
+
+  // Create a client that calls the goal request service
+  // Make sure the UUID received is the same as the one sent
+
+  auto client = node->create_client<Fibonacci::GoalRequestService>(
+    "fibonacci/_action/send_goal");
+
+  ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(20)));
+
+  auto request = std::make_shared<Fibonacci::GoalRequestService::Request>();
+
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+  request->uuid = uuid;
+
+  auto future = client->async_send_request(request);
+  ASSERT_EQ(
+    rclcpp::executor::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future));
+
+  ASSERT_EQ(uuid, received_uuid);
+}
+
+TEST_F(TestServer, handle_accepted_called)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_exec_node", "/rclcpp_action/handle_accepted");
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      [](std::shared_ptr<GoalHandle>) {
+        return rclcpp_action::CancelResponse::REJECT;
+      },
+      handle_accepted);
+  (void)as;
+
+  auto request = send_goal_request(node, uuid);
+
+  ASSERT_TRUE(received_handle);
+  ASSERT_TRUE(received_handle->is_active());
+  EXPECT_EQ(uuid, received_handle->get_goal_id());
+  EXPECT_EQ(*request, *(received_handle->get_goal()));
+}
+
+TEST_F(TestServer, handle_cancel_called)
+{
+  auto node = std::make_shared<rclcpp::Node>("handle_cancel_node", "/rclcpp_action/handle_cancel");
+  const GoalID uuid{{10, 20, 30, 40, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  ASSERT_TRUE(received_handle);
+  EXPECT_EQ(uuid, received_handle->get_goal_id());
+  EXPECT_FALSE(received_handle->is_canceling());
+
+  send_cancel_request(node, uuid);
+  EXPECT_TRUE(received_handle->is_canceling());
+}
+
+TEST_F(TestServer, publish_status_accepted)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_accept_node", "/rclcpp_action/status_accept");
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 8, 9, 100, 110, 120, 13, 14, 15, 16}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() != 1u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  // Not sure whether accepted will come through because not sure when subscriber will match
+  for (auto & msg : received_msgs) {
+    ASSERT_EQ(1u, msg->status_list.size());
+    EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+    auto status = msg->status_list.at(0).status;
+    if (action_msgs::msg::GoalStatus::STATUS_ACCEPTED == status) {
+      EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ACCEPTED, status);
+    } else {
+      EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_EXECUTING, status);
+    }
+  }
+}
+
+TEST_F(TestServer, publish_status_canceling)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_cancel_node", "/rclcpp_action/status_cancel");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 7, 80, 9, 10, 11, 120, 13, 14, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  send_cancel_request(node, uuid);
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELING, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_status_canceled)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_canceled", "/rclcpp_action/status_canceled");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::ACCEPT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  send_cancel_request(node, uuid);
+
+  received_handle->set_canceled(std::make_shared<Fibonacci::Result>());
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 3u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_CANCELED, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_status_succeeded)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_succeeded", "/rclcpp_action/status_succeeded");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  received_handle->set_succeeded(std::make_shared<Fibonacci::Result>());
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_status_aborted)
+{
+  auto node = std::make_shared<rclcpp::Node>("status_aborted", "/rclcpp_action/status_aborted");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to status messages
+  std::vector<action_msgs::msg::GoalStatusArray::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<action_msgs::msg::GoalStatusArray>(
+    "fibonacci/_action/status", [&received_msgs](action_msgs::msg::GoalStatusArray::SharedPtr list)
+    {
+      received_msgs.push_back(list);
+    });
+
+  send_goal_request(node, uuid);
+  received_handle->set_aborted(std::make_shared<Fibonacci::Result>());
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 2u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_LT(0u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(1u, msg->status_list.size());
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_ABORTED, msg->status_list.at(0).status);
+  EXPECT_EQ(uuid, msg->status_list.at(0).goal_info.goal_id.uuid);
+}
+
+TEST_F(TestServer, publish_feedback)
+{
+  auto node = std::make_shared<rclcpp::Node>("pub_feedback", "/rclcpp_action/pub_feedback");
+  const GoalID uuid{{1, 20, 30, 4, 5, 6, 70, 8, 9, 1, 11, 120, 13, 14, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  // Subscribe to feedback messages
+  using FeedbackT = Fibonacci::Feedback;
+  std::vector<FeedbackT::SharedPtr> received_msgs;
+  auto subscriber = node->create_subscription<FeedbackT>(
+    "fibonacci/_action/feedback", [&received_msgs](FeedbackT::SharedPtr msg)
+    {
+      received_msgs.push_back(msg);
+    });
+
+  send_goal_request(node, uuid);
+
+  auto sent_message = std::make_shared<FeedbackT>();
+  sent_message->sequence = {1, 1, 2, 3, 5};
+  received_handle->publish_feedback(sent_message);
+
+  // 10 seconds
+  const size_t max_tries = 10 * 1000 / 100;
+  for (size_t retry = 0; retry < max_tries && received_msgs.size() < 1u; ++retry) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    rclcpp::spin_some(node);
+  }
+
+  ASSERT_EQ(1u, received_msgs.size());
+  auto & msg = received_msgs.back();
+  ASSERT_EQ(sent_message->sequence, msg->sequence);
+  ASSERT_EQ(uuid, msg->uuid);
+}
+
+TEST_F(TestServer, get_result)
+{
+  auto node = std::make_shared<rclcpp::Node>("get_result", "/rclcpp_action/get_result");
+  const GoalID uuid{{1, 2, 3, 4, 5, 6, 7, 80, 90, 10, 11, 12, 13, 14, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  // Send result request
+  auto result_client = node->create_client<Fibonacci::GoalResultService>(
+    "fibonacci/_action/get_result");
+  if (!result_client->wait_for_service(std::chrono::seconds(20))) {
+    throw std::runtime_error("get result service didn't become available");
+  }
+  auto request = std::make_shared<Fibonacci::GoalResultService::Request>();
+  request->uuid = uuid;
+  auto future = result_client->async_send_request(request);
+
+  // Send a result
+  auto result = std::make_shared<Fibonacci::Result>();
+  result->sequence = {5, 8, 13, 21};
+  received_handle->set_succeeded(result);
+
+  // Wait for the result request to be received
+  ASSERT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS,
+    rclcpp::spin_until_future_complete(node, future));
+
+  auto response = future.get();
+  EXPECT_EQ(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED, response->status);
+  EXPECT_EQ(result->sequence, response->sequence);
+}
+
+TEST_F(TestServer, deferred_execution)
+{
+  auto node = std::make_shared<rclcpp::Node>("defer_exec", "/rclcpp_action/defer_exec");
+  const GoalID uuid{{1, 2, 3, 40, 5, 6, 70, 8, 9, 1, 11, 120, 13, 140, 15, 160}};
+
+  auto handle_goal = [](
+    const GoalID &, std::shared_ptr<const Fibonacci::Goal>)
+    {
+      return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+    };
+
+  using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  auto handle_cancel = [](std::shared_ptr<GoalHandle>)
+    {
+      return rclcpp_action::CancelResponse::REJECT;
+    };
+
+  std::shared_ptr<GoalHandle> received_handle;
+  auto handle_accepted = [&received_handle](std::shared_ptr<GoalHandle> handle)
+    {
+      received_handle = handle;
+    };
+
+  auto as = rclcpp_action::create_server<test_msgs::action::Fibonacci>(node, "fibonacci",
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  (void)as;
+
+  send_goal_request(node, uuid);
+
+  EXPECT_TRUE(received_handle->is_active());
+  EXPECT_FALSE(received_handle->is_executing());
+  received_handle->set_executing();
+  EXPECT_TRUE(received_handle->is_executing());
+}