[rclcpp_action] Action client implementation (#594)

* WIP

* Removed async_cancel from action ClintGoalHandle API

* Added status handler to action client goal handler

* Added result handler to action client goal handler

* Identation fix

* Added get/set for action client goal handler

* Changed action client goal handler attrs from rcl to cpp versions

* Added check methods to action client goal handler

* Removed rcl_client pointer from action client goal handler

* Added basic waitable interface to action client

* Updated waitable execute from action client

* Added throw for rcl calls in action client

* Removed duplicated ready flags from action client

* Minor fix

* Added header to action ClientBaseImpl execute

* Mich's update to action client interface

* Added trailing suffix to client pimpl attrs

* Towards a consistent action client

* Misc fixes for the action client

* Yet more misc fixes for the action client

* Few more fixes and shortcuts to deal with missing type support.

* Fixed lint errors in action headers and client

* Fixes to action client internal workflow.

* Misc fixes to get client example to build

* More misck client fixes

* Remove debug print

* replace logging with throw_from_rcl_error

* Wrap result object given by client to user

* Fix a couple bugs trying to cancel goals

* Use unique_indentifier_msgs

* create_client accepts group and removes waitable

* Uncrustify fixes

* [rclcpp_action] Adds tests for action client.

* [WIP] Failing action client tests.

* [rclcpp_action] Action client tests passing.

* Spin both executors to make tests pass on my machine

* Feedback callback uses shared pointer

* comment about why make_result_aware is called

* Client documentation

* Execute one thing at a time

* Return nullptr instead of throwing RejectedGoalError

* ClientGoalHandle worries about feedback awareness

* cpplint + uncrustify

* Use node logging interface

* ACTION -> ActionT

* Make ClientBase constructor protected

* Return types on different line

* Avoid passing const reference to temporary

* Child logger rclcpp_action

* Child logger rclcpp_action

* possible windows fixes

* remove excess space

* swap argument order

* Misc test additions

* Windows independent_bits_engine can't do uint8_t

* Windows link issues

rclcpp_action/include/rclcpp_action/client.hpp

@@ -15,17 +15,26 @@
 #ifndef RCLCPP_ACTION__CLIENT_HPP_
 #define RCLCPP_ACTION__CLIENT_HPP_
 
+#include <rclcpp/macros.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/waitable.hpp>
 
 #include <rosidl_generator_c/action_type_support_struct.h>
-#include <rclcpp/node_interfaces/node_base_interface.hpp>
-#include <rclcpp/time.hpp>
 #include <rosidl_typesupport_cpp/action_type_support.hpp>
 
+#include <algorithm>
 #include <functional>
+#include <future>
+#include <map>
 #include <memory>
+#include <mutex>
 #include <string>
+#include <utility>
 
 #include "rclcpp_action/client_goal_handle.hpp"
+#include "rclcpp_action/types.hpp"
 #include "rclcpp_action/visibility_control.hpp"
 
 
@@ -35,65 +44,475 @@ namespace rclcpp_action
 class ClientBaseImpl;
 
 /// Base Action Client implementation
-/// It is responsible for interfacing with the C action client API.
+/// \internal
-class ClientBase
+/**
+ * This class should not be used directly by users wanting to create an aciton client.
+ * Instead users should use `rclcpp_action::Client<>`.
+ *
+ * Internally, this class is responsible for interfacing with the `rcl_action` API.
+ */
+class ClientBase : public rclcpp::Waitable
 {
 public:
-  // TODO(sloretz) NodeLoggingInterface when it can be gotten off a node
-  RCLCPP_ACTION_PUBLIC
-  ClientBase(
-    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
-    const std::string & name,
-    const rosidl_action_type_support_t * type_support);
-
   RCLCPP_ACTION_PUBLIC
   virtual ~ClientBase();
 
-  // TODO(sloretz) add a link between this class and callbacks in the templated
+  // -------------
+  // Waitables API
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_subscriptions() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_timers() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_clients() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_services() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  size_t
+  get_number_of_ready_guard_conditions() override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  add_to_wait_set(rcl_wait_set_t * wait_set) override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  is_ready(rcl_wait_set_t * wait_set) override;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  void
+  execute() override;
+
+  // End Waitables API
+  // -----------------
+
+protected:
+  RCLCPP_ACTION_PUBLIC
+  ClientBase(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & action_name,
+    const rosidl_action_type_support_t * type_support,
+    const rcl_action_client_options_t & options);
+
+  // -----------------------------------------------------
+  // API for communication between ClientBase and Client<>
+  using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  rclcpp::Logger get_logger();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  GoalID
+  generate_goal_id();
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  send_goal_request(
+    std::shared_ptr<void> request,
+    ResponseCallback callback);
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  send_result_request(
+    std::shared_ptr<void> request,
+    ResponseCallback callback);
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  send_cancel_request(
+    std::shared_ptr<void> request,
+    ResponseCallback callback);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_goal_response() const = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  handle_goal_response(
+    const rmw_request_id_t & response_header,
+    std::shared_ptr<void> goal_response);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_result_response() const = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  handle_result_response(
+    const rmw_request_id_t & response_header,
+    std::shared_ptr<void> result_response);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_cancel_response() const = 0;
+
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  virtual
+  void
+  handle_cancel_response(
+    const rmw_request_id_t & response_header,
+    std::shared_ptr<void> cancel_response);
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_feedback_message() const = 0;
+
+  /// \internal
+  virtual
+  void
+  handle_feedback_message(std::shared_ptr<void> message) = 0;
+
+  /// \internal
+  virtual
+  std::shared_ptr<void>
+  create_status_message() const = 0;
+
+  /// \internal
+  virtual
+  void
+  handle_status_message(std::shared_ptr<void> message) = 0;
+
+  // End API for communication between ClientBase and Client<>
+  // ---------------------------------------------------------
 
 private:
   std::unique_ptr<ClientBaseImpl> pimpl_;
 };
 
-
+/// Action Client
-/// Templated Action Client class
+/**
-/// It is responsible for getting the C action type support struct from the C++ type, and
+ * This class creates an action client.
-/// calling user callbacks with goal handles of the appropriate type.
+ *
-template<typename ACTION>
+ * Create an instance of this server using `rclcpp_action::create_client()`.
+ *
+ * Internally, this class is responsible for:
+ *  - coverting between the C++ action type and generic types for `rclcpp_action::ClientBase`, and
+ *  - calling user callbacks.
+ */
+template<typename ActionT>
 class Client : public ClientBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client)
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Client<ActionT>)
 
-  using FeedbackCallback = std::function<void (
+  using Goal = typename ActionT::Goal;
-        std::shared_ptr<ClientGoalHandle<ACTION>>, const typename ACTION::Feedback)>;
+  using Feedback = typename ActionT::Feedback;
+  using GoalHandle = ClientGoalHandle<ActionT>;
+  using Result = typename GoalHandle::Result;
+  using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
+  using CancelRequest = typename ActionT::CancelGoalService::Request;
+  using CancelResponse = typename ActionT::CancelGoalService::Response;
 
   Client(
     rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
-    const std::string & name
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & action_name,
+    const rcl_action_client_options_t client_options = rcl_action_client_get_default_options()
   )
   : ClientBase(
-      node_base,
+      node_base, node_logging, action_name,
-      name,
+      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
-      rosidl_typesupport_cpp::get_action_type_support_handle<ACTION>())
+      client_options)
-  {
-    // TODO(sloretz) what's the link that causes a feedback topic to be called ?
-  }
-
-  ClientGoalHandle<ACTION>
-  async_send_goal(typename ACTION::Goal * goal, FeedbackCallback callback = nullptr);
-
-  RCLCPP_ACTION_PUBLIC
-  std::future<typename ACTION::CancelGoalService::Response>
-  async_cancel_all_goals();
-
-  RCLCPP_ACTION_PUBLIC
-  std::future<typename ACTION::CancelGoalService::Response>
-  async_cancel_goals_before(rclcpp::Time);
-
-  virtual ~Client()
   {
   }
+
+  std::shared_future<typename GoalHandle::SharedPtr>
+  async_send_goal(
+    const Goal & goal, FeedbackCallback callback = nullptr, bool ignore_result = false)
+  {
+    // Put promise in the heap to move it around.
+    auto promise = std::make_shared<std::promise<typename GoalHandle::SharedPtr>>();
+    std::shared_future<typename GoalHandle::SharedPtr> future(promise->get_future());
+    using GoalRequest = typename ActionT::GoalRequestService::Request;
+    // auto goal_request = std::make_shared<GoalRequest>();
+    // goal_request->goal_id = this->generate_goal_id();
+    // goal_request->goal = goal;
+    auto goal_request = std::make_shared<GoalRequest>(goal);
+    goal_request->uuid = this->generate_goal_id();
+    this->send_goal_request(
+      std::static_pointer_cast<void>(goal_request),
+      [this, goal_request, callback, ignore_result, promise](
+        std::shared_ptr<void> response) mutable
+      {
+        using GoalResponse = typename ActionT::GoalRequestService::Response;
+        auto goal_response = std::static_pointer_cast<GoalResponse>(response);
+        if (!goal_response->accepted) {
+          promise->set_value(nullptr);
+          return;
+        }
+        GoalInfo goal_info;
+        // goal_info.goal_id = goal_request->goal_id;
+        goal_info.goal_id.uuid = goal_request->uuid;
+        goal_info.stamp = goal_response->stamp;
+        // Do not use std::make_shared as friendship cannot be forwarded.
+        std::shared_ptr<GoalHandle> goal_handle(new GoalHandle(goal_info, callback));
+        if (!ignore_result) {
+          try {
+            this->make_result_aware(goal_handle);
+          } catch (...) {
+            promise->set_exception(std::current_exception());
+            return;
+          }
+        }
+        std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+        goal_handles_[goal_handle->get_goal_id()] = goal_handle;
+        promise->set_value(goal_handle);
+      });
+    return future;
+  }
+
+  std::shared_future<Result>
+  async_get_result(typename GoalHandle::SharedPtr goal_handle)
+  {
+    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
+      throw exceptions::UnknownGoalHandleError();
+    }
+    // If the user chose to ignore the result before, then ask the server for the result now.
+    if (!goal_handle->is_result_aware()) {
+      this->make_result_aware(goal_handle);
+    }
+    return goal_handle->async_result();
+  }
+
+  std::shared_future<bool>
+  async_cancel_goal(typename GoalHandle::SharedPtr goal_handle)
+  {
+    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+    if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
+      throw exceptions::UnknownGoalHandleError();
+    }
+    // Put promise in the heap to move it around.
+    auto promise = std::make_shared<std::promise<bool>>();
+    std::shared_future<bool> future(promise->get_future());
+    auto cancel_request = std::make_shared<CancelRequest>();
+    // cancel_request->goal_info.goal_id = goal_handle->get_goal_id();
+    cancel_request->goal_info.goal_id.uuid = goal_handle->get_goal_id();
+    this->send_cancel_request(
+      std::static_pointer_cast<void>(cancel_request),
+      [goal_handle, promise](std::shared_ptr<void> response) mutable
+      {
+        auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
+        bool goal_canceled = false;
+        if (!cancel_response->goals_canceling.empty()) {
+          const GoalInfo & canceled_goal_info = cancel_response->goals_canceling[0];
+          // goal_canceled = (canceled_goal_info.goal_id == goal_handle->get_goal_id());
+          goal_canceled = (canceled_goal_info.goal_id.uuid == goal_handle->get_goal_id());
+        }
+        promise->set_value(goal_canceled);
+      });
+    return future;
+  }
+
+  std::shared_future<typename CancelResponse::SharedPtr>
+  async_cancel_all_goals()
+  {
+    auto cancel_request = std::make_shared<CancelRequest>();
+    // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
+    std::fill(
+      cancel_request->goal_info.goal_id.uuid.begin(),
+      cancel_request->goal_info.goal_id.uuid.end(), 0u);
+    return async_cancel(cancel_request);
+  }
+
+  std::shared_future<typename CancelResponse::SharedPtr>
+  async_cancel_goals_before(const rclcpp::Time & stamp)
+  {
+    auto cancel_request = std::make_shared<CancelRequest>();
+    // std::fill(cancel_request->goal_info.goal_id.uuid, 0u);
+    std::fill(
+      cancel_request->goal_info.goal_id.uuid.begin(),
+      cancel_request->goal_info.goal_id.uuid.end(), 0u);
+    cancel_request->goal_info.stamp = stamp;
+    return async_cancel(cancel_request);
+  }
+
+  virtual
+  ~Client()
+  {
+    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    auto it = goal_handles_.begin();
+    while (it != goal_handles_.end()) {
+      it->second->invalidate();
+      it = goal_handles_.erase(it);
+    }
+  }
+
+private:
+  /// \internal
+  std::shared_ptr<void>
+  create_goal_response() const override
+  {
+    using GoalResponse = typename ActionT::GoalRequestService::Response;
+    return std::shared_ptr<void>(new GoalResponse());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_result_response() const override
+  {
+    using GoalResultResponse = typename ActionT::GoalResultService::Response;
+    return std::shared_ptr<void>(new GoalResultResponse());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_cancel_response() const override
+  {
+    return std::shared_ptr<void>(new CancelResponse());
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_feedback_message() const override
+  {
+    // using FeedbackMessage = typename ActionT::FeedbackMessage;
+    // return std::shared_ptr<void>(new FeedbackMessage());
+    return std::shared_ptr<void>(new Feedback());
+  }
+
+  /// \internal
+  void
+  handle_feedback_message(std::shared_ptr<void> message) override
+  {
+    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    // using FeedbackMessage = typename ActionT::FeedbackMessage;
+    // typename FeedbackMessage::SharedPtr feedback_message =
+    //   std::static_pointer_cast<FeedbackMessage>(message);
+    typename Feedback::SharedPtr feedback_message =
+      std::static_pointer_cast<Feedback>(message);
+    // const GoalID & goal_id = feedback_message->goal_id;
+    const GoalID & goal_id = feedback_message->uuid;
+    if (goal_handles_.count(goal_id) == 0) {
+      RCLCPP_DEBUG(
+        this->get_logger(),
+        "Received feedback for unknown goal. Ignoring...");
+      return;
+    }
+    typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
+    // goal_handle->call_feedback_callback(goal_handle, feedback_message->feedback);
+    goal_handle->call_feedback_callback(goal_handle, feedback_message);
+  }
+
+  /// \internal
+  std::shared_ptr<void>
+  create_status_message() const override
+  {
+    using GoalStatusMessage = typename ActionT::GoalStatusMessage;
+    return std::shared_ptr<void>(new GoalStatusMessage());
+  }
+
+  /// \internal
+  void
+  handle_status_message(std::shared_ptr<void> message) override
+  {
+    std::lock_guard<std::mutex> guard(goal_handles_mutex_);
+    using GoalStatusMessage = typename ActionT::GoalStatusMessage;
+    auto status_message = std::static_pointer_cast<GoalStatusMessage>(message);
+    for (const GoalStatus & status : status_message->status_list) {
+      // const GoalID & goal_id = status.goal_info.goal_id;
+      const GoalID & goal_id = status.goal_info.goal_id.uuid;
+      if (goal_handles_.count(goal_id) == 0) {
+        RCLCPP_DEBUG(
+          this->get_logger(),
+          "Received status for unknown goal. Ignoring...");
+        continue;
+      }
+      typename GoalHandle::SharedPtr goal_handle = goal_handles_[goal_id];
+      goal_handle->set_status(status.status);
+      const int8_t goal_status = goal_handle->get_status();
+      if (
+        goal_status == GoalStatus::STATUS_SUCCEEDED ||
+        goal_status == GoalStatus::STATUS_CANCELED ||
+        goal_status == GoalStatus::STATUS_ABORTED)
+      {
+        goal_handles_.erase(goal_id);
+      }
+    }
+  }
+
+  /// \internal
+  void
+  make_result_aware(typename GoalHandle::SharedPtr goal_handle)
+  {
+    using GoalResultRequest = typename ActionT::GoalResultService::Request;
+    auto goal_result_request = std::make_shared<GoalResultRequest>();
+    // goal_result_request.goal_id = goal_handle->get_goal_id();
+    goal_result_request->uuid = goal_handle->get_goal_id();
+    this->send_result_request(
+      std::static_pointer_cast<void>(goal_result_request),
+      [goal_handle, this](std::shared_ptr<void> response) mutable
+      {
+        // Wrap the response in a struct with the fields a user cares about
+        Result result;
+        using GoalResultResponse = typename ActionT::GoalResultService::Response;
+        result.response = std::static_pointer_cast<GoalResultResponse>(response);
+        result.goal_id = goal_handle->get_goal_id();
+        result.code = static_cast<ResultCode>(result.response->status);
+        goal_handle->set_result(result);
+        std::lock_guard<std::mutex> lock(goal_handles_mutex_);
+        goal_handles_.erase(goal_handle->get_goal_id());
+      });
+    goal_handle->set_result_awareness(true);
+  }
+
+  /// \internal
+  std::shared_future<typename CancelResponse::SharedPtr>
+  async_cancel(typename CancelRequest::SharedPtr cancel_request)
+  {
+    // Put promise in the heap to move it around.
+    auto promise = std::make_shared<std::promise<typename CancelResponse::SharedPtr>>();
+    std::shared_future<typename CancelResponse::SharedPtr> future(promise->get_future());
+    this->send_cancel_request(
+      std::static_pointer_cast<void>(cancel_request),
+      [promise](std::shared_ptr<void> response) mutable
+      {
+        auto cancel_response = std::static_pointer_cast<CancelResponse>(response);
+        promise->set_value(cancel_response);
+      });
+    return future;
+  }
+
+  std::map<GoalID, typename GoalHandle::SharedPtr> goal_handles_;
+  std::mutex goal_handles_mutex_;
 };
 }  // namespace rclcpp_action
+
 #endif  // RCLCPP_ACTION__CLIENT_HPP_

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -17,39 +17,112 @@
 
 #include <rcl_action/action_client.h>
 
-#include <functional>
+#include <action_msgs/msg/goal_status.hpp>
-#include <memory>
+#include <rclcpp/macros.hpp>
+#include <rclcpp/time.hpp>
 
+#include <functional>
+#include <future>
+#include <memory>
+#include <mutex>
+
+#include "rclcpp_action/types.hpp"
 #include "rclcpp_action/visibility_control.hpp"
 
 namespace rclcpp_action
 {
+/// The possible statuses that an action goal can finish with.
+enum class ResultCode : int8_t
+{
+  UNKNOWN = action_msgs::msg::GoalStatus::STATUS_UNKNOWN,
+  SUCCEEDED = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED,
+  CANCELED = action_msgs::msg::GoalStatus::STATUS_CANCELED,
+  ABORTED = action_msgs::msg::GoalStatus::STATUS_ABORTED
+};
+
+
 // Forward declarations
-template<typename ACTION>
+template<typename ActionT>
 class Client;
 
-template<typename ACTION>
+template<typename ActionT>
 class ClientGoalHandle
 {
 public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientGoalHandle)
+
+  // A wrapper that defines the result of an action
+  typedef struct Result
+  {
+    /// The unique identifier of the goal
+    GoalID goal_id;
+    /// A status to indicate if the goal was canceled, aborted, or suceeded
+    ResultCode code;
+    /// User defined fields sent back with an action
+    typename ActionT::Result::SharedPtr response;
+  } Result;
+
+  using Feedback = typename ActionT::Feedback;
+  using FeedbackCallback =
+    std::function<void (
+        typename ClientGoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const Feedback>)>;
+
   virtual ~ClientGoalHandle();
 
-  /// TODO(sloretz) examples have this on client as `async_cancel_goal(goal_handle)`
+  const GoalID &
-  std::future<bool>
+  get_goal_id() const;
-  async_cancel();
 
-  std::future<typename ACTION::Result>
+  rclcpp::Time
+  get_goal_stamp() const;
+
+  std::shared_future<Result>
   async_result();
 
+  int8_t
+  get_status();
+
+  bool
+  is_feedback_aware();
+
+  bool
+  is_result_aware();
+
 private:
-  // The templated Server creates goal handles
+  // The templated Client creates goal handles
-  friend Client<ACTION>;
+  friend Client<ActionT>;
 
-  ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info);
+  ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback);
 
-  // TODO(sloretz) shared pointer to keep rcl_client_ alive while goal handles are alive
+  void
-  rcl_action_client_t * rcl_client_;
+  set_feedback_callback(FeedbackCallback callback);
-  rcl_action_goal_info_t rcl_info_;
+
+  void
+  call_feedback_callback(
+    typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
+    typename std::shared_ptr<const Feedback> feedback_message);
+
+  void
+  set_result_awareness(bool awareness);
+
+  void
+  set_status(int8_t status);
+
+  void
+  set_result(const Result & result);
+
+  void
+  invalidate();
+
+  GoalInfo info_;
+
+  bool is_result_aware_{false};
+  std::promise<Result> result_promise_;
+  std::shared_future<Result> result_future_;
+
+  FeedbackCallback feedback_callback_{nullptr};
+  int8_t status_{GoalStatus::STATUS_ACCEPTED};
+
+  std::mutex handle_mutex_;
 };
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -17,35 +17,137 @@
 
 #include <rcl_action/types.h>
 
+#include <memory>
+
+#include "rclcpp_action/exceptions.hpp"
+
 namespace rclcpp_action
 {
-template<typename ACTION>
+
-ClientGoalHandle<ACTION>::ClientGoalHandle(
+template<typename ActionT>
-  rcl_action_client_t * rcl_client,
+ClientGoalHandle<ActionT>::ClientGoalHandle(
-  const rcl_action_goal_info_t rcl_info
+  const GoalInfo & info, FeedbackCallback callback)
-)
+: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback)
-: rcl_client_(rcl_client), rcl_info_(rcl_info)
 {
 }
 
-template<typename ACTION>
+template<typename ActionT>
-ClientGoalHandle<ACTION>::~ClientGoalHandle()
+ClientGoalHandle<ActionT>::~ClientGoalHandle()
 {
 }
 
-template<typename ACTION>
+template<typename ActionT>
-std::future<bool>
+const GoalID &
-ClientGoalHandle<ACTION>::async_cancel()
+ClientGoalHandle<ActionT>::get_goal_id() const
 {
-  throw std::runtime_error("Failed to cancel goal");
+  // return info_.goal_id;
+  return info_.goal_id.uuid;
 }
 
-template<typename ACTION>
+template<typename ActionT>
-std::future<typename ACTION::Result>
+rclcpp::Time
-ClientGoalHandle<ACTION>::async_result()
+ClientGoalHandle<ActionT>::get_goal_stamp() const
 {
-  throw std::runtime_error("Failed to get result future");
+  return info_.stamp;
 }
+
+template<typename ActionT>
+std::shared_future<typename ClientGoalHandle<ActionT>::Result>
+ClientGoalHandle<ActionT>::async_result()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  if (!is_result_aware_) {
+    throw exceptions::UnawareGoalHandleError();
+  }
+  return result_future_;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_result(const Result & result)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = static_cast<int8_t>(result.code);
+  result_promise_.set_value(result);
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  feedback_callback_ = callback;
+}
+
+template<typename ActionT>
+int8_t
+ClientGoalHandle<ActionT>::get_status()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return status_;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_status(int8_t status)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = status;
+}
+
+template<typename ActionT>
+bool
+ClientGoalHandle<ActionT>::is_feedback_aware()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return feedback_callback_ != nullptr;
+}
+
+template<typename ActionT>
+bool
+ClientGoalHandle<ActionT>::is_result_aware()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return is_result_aware_;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  is_result_aware_ = awareness;
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::invalidate()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = GoalStatus::STATUS_UNKNOWN;
+  result_promise_.set_exception(std::make_exception_ptr(
+      exceptions::UnawareGoalHandleError()));
+}
+
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::call_feedback_callback(
+  typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
+  typename std::shared_ptr<const Feedback> feedback_message)
+{
+  if (shared_this.get() != this) {
+    RCLCPP_ERROR(rclcpp::get_logger("rclcpp_action"), "Sent feedback to wrong goal handle.");
+    return;
+  }
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  if (nullptr == feedback_callback_) {
+    // Normal, some feedback messages may arrive after the goal result.
+    RCLCPP_DEBUG(rclcpp::get_logger("rclcpp_action"), "Received feedback but goal ignores it.");
+    return;
+  }
+  feedback_callback_(shared_this, feedback_message);
+}
+
 }  // namespace rclcpp_action
 
 #endif  // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_

rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -25,16 +25,50 @@
 
 namespace rclcpp_action
 {
-template<typename ACTION>
+template<typename ActionT>
-typename Client<ACTION>::SharedPtr
+typename Client<ActionT>::SharedPtr
 create_client(
-  rclcpp::Node * node,
+  rclcpp::Node::SharedPtr node,
-  const std::string & name)
+  const std::string & name,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
 {
-  return Client<ACTION>::make_shared(
+  std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
-    node->get_node_base_interface(),
+    node->get_node_waitables_interface();
-    name);
+  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](Client<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<Client<ActionT>> fake_shared_ptr(ptr, [](Client<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<Client<ActionT>> action_client(
+    new Client<ActionT>(node->get_node_base_interface(), node->get_node_logging_interface(), name),
+    deleter);
+
+  node->get_node_waitables_interface()->add_waitable(action_client, group);
+  return action_client;
 }
 }  // namespace rclcpp_action
+
 #endif  // RCLCPP_ACTION__CREATE_CLIENT_HPP_
-// TODO(sloretz) rclcpp_action::create_client<>(node, action_name);

rclcpp_action/include/rclcpp_action/exceptions.hpp

@@ -0,0 +1,46 @@
+// 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__EXCEPTIONS_HPP_
+#define RCLCPP_ACTION__EXCEPTIONS_HPP_
+
+#include <stdexcept>
+
+namespace rclcpp_action
+{
+namespace exceptions
+{
+class UnknownGoalHandleError : public std::invalid_argument
+{
+public:
+  UnknownGoalHandleError()
+  : std::invalid_argument("Goal handle is not know to this client.")
+  {
+  }
+};
+
+class UnawareGoalHandleError : public std::runtime_error
+{
+public:
+  UnawareGoalHandleError()
+  : std::runtime_error("Goal handle is not tracking the goal result.")
+  {
+  }
+};
+
+}  // namespace exceptions
+
+}  // namespace rclcpp_action
+
+#endif  // RCLCPP_ACTION__EXCEPTIONS_HPP_

rclcpp_action/include/rclcpp_action/types.hpp

@@ -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.
+
+#ifndef RCLCPP_ACTION__TYPES_HPP_
+#define RCLCPP_ACTION__TYPES_HPP_
+
+#include <rcl_action/types.h>
+
+#include <action_msgs/msg/goal_status.hpp>
+#include <action_msgs/msg/goal_info.hpp>
+
+#include <functional>
+
+
+namespace rclcpp_action
+{
+
+using GoalID = std::array<uint8_t, UUID_SIZE>;
+using GoalStatus = action_msgs::msg::GoalStatus;
+using GoalInfo = action_msgs::msg::GoalInfo;
+
+}  // namespace rclcpp_action
+
+namespace std
+{
+template<>
+struct less<rclcpp_action::GoalID>
+{
+  bool operator()(
+    const rclcpp_action::GoalID & lhs,
+    const rclcpp_action::GoalID & rhs) const
+  {
+    return lhs < rhs;
+  }
+};
+}  // namespace std
+#endif  // RCLCPP_ACTION__TYPES_HPP_

rclcpp_action/src/client.cpp

@@ -12,30 +12,349 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <rclcpp_action/client.hpp>
+#include <rcl_action/action_client.h>
+#include <rcl_action/wait.h>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
 
+#include <algorithm>
+#include <map>
+#include <memory>
+#include <random>
 #include <string>
 
-using rclcpp_action::ClientBase;
+#include "rclcpp_action/client.hpp"
+#include "rclcpp_action/exceptions.hpp"
 
 namespace rclcpp_action
 {
+
 class ClientBaseImpl
 {
+public:
+  ClientBaseImpl(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    const std::string & action_name,
+    const rosidl_action_type_support_t * type_support,
+    const rcl_action_client_options_t & client_options)
+  : node_handle(node_base->get_shared_rcl_node_handle()),
+    logger(node_logging->get_logger().get_child("rclcpp_acton")),
+    random_bytes_generator(std::random_device{} ())
+  {
+    std::weak_ptr<rcl_node_t> weak_node_handle(node_handle);
+    client_handle = std::shared_ptr<rcl_action_client_t>(
+      new rcl_action_client_t, [weak_node_handle](rcl_action_client_t * client)
+      {
+        auto handle = weak_node_handle.lock();
+        if (handle) {
+          if (RCL_RET_OK != rcl_action_client_fini(client, handle.get())) {
+            RCLCPP_ERROR(
+              rclcpp::get_logger(rcl_node_get_logger_name(handle.get())).get_child("rclcpp_action"),
+              "Error in destruction of rcl action client handle: %s", rcl_get_error_string().str);
+            rcl_reset_error();
+          }
+        } else {
+          RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp_action"),
+            "Error in destruction of rcl action client handle: "
+            "the Node Handle was destructed too early. You will leak memory");
+        }
+        delete client;
+      });
+    *client_handle = rcl_action_get_zero_initialized_client();
+    rcl_ret_t ret = rcl_action_client_init(
+      client_handle.get(), node_handle.get(), type_support,
+      action_name.c_str(), &client_options);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "could not initialize rcl action client");
+    }
+
+    ret = rcl_action_client_wait_set_get_num_entities(
+      client_handle.get(),
+      &num_subscriptions,
+      &num_guard_conditions,
+      &num_timers,
+      &num_clients,
+      &num_services);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(
+        ret, "could not retrieve rcl action client details");
+    }
+  }
+
+  size_t num_subscriptions{0u};
+  size_t num_guard_conditions{0u};
+  size_t num_timers{0u};
+  size_t num_clients{0u};
+  size_t num_services{0u};
+
+  bool is_feedback_ready{false};
+  bool is_status_ready{false};
+  bool is_goal_response_ready{false};
+  bool is_cancel_response_ready{false};
+  bool is_result_response_ready{false};
+
+  std::shared_ptr<rcl_action_client_t> client_handle{nullptr};
+  std::shared_ptr<rcl_node_t> node_handle{nullptr};
+  rclcpp::Logger logger;
+
+  using ResponseCallback = std::function<void (std::shared_ptr<void> response)>;
+
+  std::map<int64_t, ResponseCallback> pending_goal_responses;
+  std::mutex goal_requests_mutex;
+
+  std::map<int64_t, ResponseCallback> pending_result_responses;
+  std::mutex result_requests_mutex;
+
+  std::map<int64_t, ResponseCallback> pending_cancel_responses;
+  std::mutex cancel_requests_mutex;
+
+  std::independent_bits_engine<
+    std::default_random_engine, 8, unsigned int> random_bytes_generator;
 };
-}
 
 ClientBase::ClientBase(
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
-  const std::string & name,
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
-  const rosidl_action_type_support_t * type_support)
+  const std::string & action_name,
+  const rosidl_action_type_support_t * type_support,
+  const rcl_action_client_options_t & client_options)
+: pimpl_(new ClientBaseImpl(node_base, node_logging, action_name, type_support, client_options))
 {
-  // TODO(sloretz) use rcl_action API when available
-  (void)node_base;
-  (void)name;
-  (void)type_support;
 }
 
 ClientBase::~ClientBase()
 {
 }
+
+rclcpp::Logger
+ClientBase::get_logger()
+{
+  return pimpl_->logger;
+}
+
+size_t
+ClientBase::get_number_of_ready_subscriptions()
+{
+  return pimpl_->num_subscriptions;
+}
+
+size_t
+ClientBase::get_number_of_ready_guard_conditions()
+{
+  return pimpl_->num_guard_conditions;
+}
+
+size_t
+ClientBase::get_number_of_ready_timers()
+{
+  return pimpl_->num_timers;
+}
+
+size_t
+ClientBase::get_number_of_ready_clients()
+{
+  return pimpl_->num_clients;
+}
+
+size_t
+ClientBase::get_number_of_ready_services()
+{
+  return pimpl_->num_services;
+}
+
+bool
+ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set)
+{
+  rcl_ret_t ret = rcl_action_wait_set_add_action_client(
+    wait_set, pimpl_->client_handle.get(), nullptr, nullptr);
+  return RCL_RET_OK == ret;
+}
+
+bool
+ClientBase::is_ready(rcl_wait_set_t * wait_set)
+{
+  rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
+    wait_set, pimpl_->client_handle.get(),
+    &pimpl_->is_feedback_ready,
+    &pimpl_->is_status_ready,
+    &pimpl_->is_goal_response_ready,
+    &pimpl_->is_cancel_response_ready,
+    &pimpl_->is_result_response_ready);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "failed to check for any ready entities");
+  }
+  return
+    pimpl_->is_feedback_ready ||
+    pimpl_->is_status_ready ||
+    pimpl_->is_goal_response_ready ||
+    pimpl_->is_cancel_response_ready ||
+    pimpl_->is_result_response_ready;
+}
+
+void
+ClientBase::handle_goal_response(
+  const rmw_request_id_t & response_header,
+  std::shared_ptr<void> response)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->goal_requests_mutex);
+  const int64_t & sequence_number = response_header.sequence_number;
+  if (pimpl_->pending_goal_responses.count(sequence_number) == 0) {
+    RCLCPP_ERROR(pimpl_->logger, "unknown goal response, ignoring...");
+    return;
+  }
+  pimpl_->pending_goal_responses[sequence_number](response);
+  pimpl_->pending_goal_responses.erase(sequence_number);
+}
+
+void
+ClientBase::send_goal_request(std::shared_ptr<void> request, ResponseCallback callback)
+{
+  std::unique_lock<std::mutex> guard(pimpl_->goal_requests_mutex);
+  int64_t sequence_number;
+  rcl_ret_t ret = rcl_action_send_goal_request(
+    pimpl_->client_handle.get(), request.get(), &sequence_number);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send goal request");
+  }
+  assert(pimpl_->pending_goal_responses.count(sequence_number) == 0);
+  pimpl_->pending_goal_responses[sequence_number] = callback;
+}
+
+void
+ClientBase::handle_result_response(
+  const rmw_request_id_t & response_header,
+  std::shared_ptr<void> response)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
+  const int64_t & sequence_number = response_header.sequence_number;
+  if (pimpl_->pending_result_responses.count(sequence_number) == 0) {
+    RCLCPP_ERROR(pimpl_->logger, "unknown result response, ignoring...");
+    return;
+  }
+  pimpl_->pending_result_responses[sequence_number](response);
+  pimpl_->pending_result_responses.erase(sequence_number);
+}
+
+void
+ClientBase::send_result_request(std::shared_ptr<void> request, ResponseCallback callback)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->result_requests_mutex);
+  int64_t sequence_number;
+  rcl_ret_t ret = rcl_action_send_result_request(
+    pimpl_->client_handle.get(), request.get(), &sequence_number);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send result request");
+  }
+  assert(pimpl_->pending_result_responses.count(sequence_number) == 0);
+  pimpl_->pending_result_responses[sequence_number] = callback;
+}
+
+void
+ClientBase::handle_cancel_response(
+  const rmw_request_id_t & response_header,
+  std::shared_ptr<void> response)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->cancel_requests_mutex);
+  const int64_t & sequence_number = response_header.sequence_number;
+  if (pimpl_->pending_cancel_responses.count(sequence_number) == 0) {
+    RCLCPP_ERROR(pimpl_->logger, "unknown cancel response, ignoring...");
+    return;
+  }
+  pimpl_->pending_cancel_responses[sequence_number](response);
+  pimpl_->pending_cancel_responses.erase(sequence_number);
+}
+
+void
+ClientBase::send_cancel_request(std::shared_ptr<void> request, ResponseCallback callback)
+{
+  std::lock_guard<std::mutex> guard(pimpl_->cancel_requests_mutex);
+  int64_t sequence_number;
+  rcl_ret_t ret = rcl_action_send_cancel_request(
+    pimpl_->client_handle.get(), request.get(), &sequence_number);
+  if (RCL_RET_OK != ret) {
+    rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send cancel request");
+  }
+  assert(pimpl_->pending_cancel_responses.count(sequence_number) == 0);
+  pimpl_->pending_cancel_responses[sequence_number] = callback;
+}
+
+GoalID
+ClientBase::generate_goal_id()
+{
+  GoalID goal_id;
+  // TODO(hidmic): Do something better than this for UUID generation.
+  // std::generate(
+  //   goal_id.uuid.begin(), goal_id.uuid.end(),
+  //   std::ref(pimpl_->random_bytes_generator));
+  std::generate(
+    goal_id.begin(), goal_id.end(),
+    std::ref(pimpl_->random_bytes_generator));
+  return goal_id;
+}
+
+void
+ClientBase::execute()
+{
+  if (pimpl_->is_feedback_ready) {
+    std::shared_ptr<void> feedback_message = this->create_feedback_message();
+    rcl_ret_t ret = rcl_action_take_feedback(
+      pimpl_->client_handle.get(), feedback_message.get());
+    pimpl_->is_feedback_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking feedback");
+    } else {
+      this->handle_feedback_message(feedback_message);
+    }
+  } else if (pimpl_->is_status_ready) {
+    std::shared_ptr<void> status_message = this->create_status_message();
+    rcl_ret_t ret = rcl_action_take_status(
+      pimpl_->client_handle.get(), status_message.get());
+    pimpl_->is_status_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking status");
+    } else {
+      this->handle_status_message(status_message);
+    }
+  } else if (pimpl_->is_goal_response_ready) {
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> goal_response = this->create_goal_response();
+    rcl_ret_t ret = rcl_action_take_goal_response(
+      pimpl_->client_handle.get(), &response_header, goal_response.get());
+    pimpl_->is_goal_response_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking goal response");
+    } else {
+      this->handle_goal_response(response_header, goal_response);
+    }
+  } else if (pimpl_->is_result_response_ready) {
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> result_response = this->create_result_response();
+    rcl_ret_t ret = rcl_action_take_result_response(
+      pimpl_->client_handle.get(), &response_header, result_response.get());
+    pimpl_->is_result_response_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking result response");
+    } else {
+      this->handle_result_response(response_header, result_response);
+    }
+  } else if (pimpl_->is_cancel_response_ready) {
+    rmw_request_id_t response_header;
+    std::shared_ptr<void> cancel_response = this->create_cancel_response();
+    rcl_ret_t ret = rcl_action_take_cancel_response(
+      pimpl_->client_handle.get(), &response_header, cancel_response.get());
+    pimpl_->is_cancel_response_ready = false;
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "error taking cancel response");
+    } else {
+      this->handle_cancel_response(response_header, cancel_response);
+    }
+  } else {
+    throw std::runtime_error("Executing action client but nothing is ready");
+  }
+}
+
+}  // namespace rclcpp_action

rclcpp_action/test/test_client.cpp

@@ -14,29 +14,407 @@
 
 #include <gtest/gtest.h>
 
+#include <rcl/allocator.h>
+#include <rcl/time.h>
+#include <rcl/types.h>
+
+#include <rcl_action/names.h>
+#include <rcl_action/default_qos.h>
+
+#include <rclcpp/clock.hpp>
 #include <rclcpp/exceptions.hpp>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/executors/multi_threaded_executor.hpp>
 #include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/service.hpp>
+#include <rclcpp/time.hpp>
+
 #include <test_msgs/action/fibonacci.hpp>
 
+#include <map>
 #include <memory>
+#include <string>
+#include <utility>
+#include <thread>
+#include <chrono>
 
+#include "rclcpp_action/exceptions.hpp"
 #include "rclcpp_action/create_client.hpp"
 #include "rclcpp_action/client.hpp"
+#include "rclcpp_action/types.hpp"
+
+using namespace std::chrono_literals;
 
 class TestClient : public ::testing::Test
 {
 protected:
+  using ActionType = test_msgs::action::Fibonacci;
+  using ActionGoal = ActionType::Goal;
+  using ActionGoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
+  using ActionGoalRequestService = ActionType::GoalRequestService;
+  using ActionGoalRequest = ActionGoalRequestService::Request;
+  using ActionGoalResponse = ActionGoalRequestService::Response;
+  using ActionGoalResultService = ActionType::GoalResultService;
+  using ActionGoalResultRequest = ActionGoalResultService::Request;
+  using ActionGoalResultResponse = ActionGoalResultService::Response;
+  using ActionCancelGoalService = ActionType::CancelGoalService;
+  using ActionCancelGoalRequest = ActionType::CancelGoalService::Request;
+  using ActionCancelGoalResponse = ActionType::CancelGoalService::Response;
+  using ActionStatusMessage = ActionType::GoalStatusMessage;
+  using ActionFeedbackMessage = ActionType::Feedback;
+  using ActionFeedback = ActionType::Feedback;
+
   static void SetUpTestCase()
   {
     rclcpp::init(0, nullptr);
   }
+
+  void SetUp()
+  {
+    rcl_allocator_t allocator = rcl_get_default_allocator();
+
+    server_node = std::make_shared<rclcpp::Node>(server_node_name, namespace_name);
+
+    char * goal_service_name = nullptr;
+    rcl_ret_t ret = rcl_action_get_goal_service_name(
+      action_name, allocator, &goal_service_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    goal_service = server_node->create_service<ActionGoalRequestService>(
+      goal_service_name,
+      [this](
+        const ActionGoalRequest::SharedPtr request,
+        ActionGoalResponse::SharedPtr response)
+      {
+        response->stamp = clock.now();
+        response->accepted = (request->order >= 0);
+        if (response->accepted) {
+          goals[request->uuid] = {request, response};
+        }
+      });
+    ASSERT_TRUE(goal_service != nullptr);
+    allocator.deallocate(goal_service_name, allocator.state);
+
+    char * result_service_name = nullptr;
+    ret = rcl_action_get_result_service_name(
+      action_name, allocator, &result_service_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    result_service = server_node->create_service<ActionGoalResultService>(
+      result_service_name,
+      [this](
+        const ActionGoalResultRequest::SharedPtr request,
+        ActionGoalResultResponse::SharedPtr response)
+      {
+        if (goals.count(request->uuid) == 1) {
+          auto goal_request = goals[request->uuid].first;
+          auto goal_response = goals[request->uuid].second;
+          ActionStatusMessage status_message;
+          rclcpp_action::GoalStatus goal_status;
+          goal_status.goal_info.goal_id.uuid = goal_request->uuid;
+          goal_status.goal_info.stamp = goal_response->stamp;
+          goal_status.status = rclcpp_action::GoalStatus::STATUS_EXECUTING;
+          status_message.status_list.push_back(goal_status);
+          status_publisher->publish(status_message);
+          client_executor.spin_once();
+          ActionFeedbackMessage feedback_message;
+          feedback_message.uuid = goal_request->uuid;
+          feedback_message.sequence.push_back(0);
+          feedback_publisher->publish(feedback_message);
+          client_executor.spin_once();
+          if (goal_request->order > 0) {
+            feedback_message.sequence.push_back(1);
+            feedback_publisher->publish(feedback_message);
+            client_executor.spin_once();
+            for (int i = 1; i < goal_request->order; ++i) {
+              feedback_message.sequence.push_back(
+                feedback_message.sequence[i] + feedback_message.sequence[i - 1]);
+              feedback_publisher->publish(feedback_message);
+              client_executor.spin_once();
+            }
+          }
+          goal_status.status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
+          status_message.status_list[0] = goal_status;
+          status_publisher->publish(status_message);
+          client_executor.spin_once();
+          response->sequence = feedback_message.sequence;
+          response->status = rclcpp_action::GoalStatus::STATUS_SUCCEEDED;
+          goals.erase(request->uuid);
+        } else {
+          response->status = rclcpp_action::GoalStatus::STATUS_UNKNOWN;
+        }
+      });
+    ASSERT_TRUE(result_service != nullptr);
+    allocator.deallocate(result_service_name, allocator.state);
+
+    char * cancel_service_name = nullptr;
+    ret = rcl_action_get_cancel_service_name(
+      action_name, allocator, &cancel_service_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    cancel_service = server_node->create_service<ActionCancelGoalService>(
+      cancel_service_name,
+      [this](
+        const ActionCancelGoalRequest::SharedPtr request,
+        ActionCancelGoalResponse::SharedPtr response)
+      {
+        rclcpp_action::GoalID zero_uuid;
+        std::fill(zero_uuid.begin(), zero_uuid.end(), 0u);
+        const rclcpp::Time cancel_stamp = request->goal_info.stamp;
+        bool cancel_all = (
+          request->goal_info.goal_id.uuid == zero_uuid &&
+          cancel_stamp == zero_stamp);
+        ActionStatusMessage status_message;
+        auto it = goals.begin();
+        while (it != goals.end()) {
+          auto goal_request = it->second.first;
+          auto goal_response = it->second.second;
+          const rclcpp::Time goal_stamp = goal_response->stamp;
+          bool cancel_this = (
+            request->goal_info.goal_id.uuid == goal_request->uuid ||
+            cancel_stamp > goal_stamp);
+          if (cancel_all || cancel_this) {
+            rclcpp_action::GoalStatus goal_status;
+            goal_status.goal_info.goal_id.uuid = goal_request->uuid;
+            goal_status.goal_info.stamp = goal_response->stamp;
+            goal_status.status = rclcpp_action::GoalStatus::STATUS_CANCELED;
+            status_message.status_list.push_back(goal_status);
+            response->goals_canceling.push_back(goal_status.goal_info);
+            it = goals.erase(it);
+          } else {
+            ++it;
+          }
+        }
+        status_publisher->publish(status_message);
+        client_executor.spin_once();
+      });
+    ASSERT_TRUE(cancel_service != nullptr);
+    allocator.deallocate(cancel_service_name, allocator.state);
+
+    char * feedback_topic_name = nullptr;
+    ret = rcl_action_get_feedback_topic_name(
+      action_name, allocator, &feedback_topic_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    feedback_publisher = server_node->create_publisher<ActionFeedbackMessage>(feedback_topic_name);
+    ASSERT_TRUE(feedback_publisher != nullptr);
+    allocator.deallocate(feedback_topic_name, allocator.state);
+
+    char * status_topic_name = nullptr;
+    ret = rcl_action_get_status_topic_name(
+      action_name, allocator, &status_topic_name);
+    ASSERT_EQ(RCL_RET_OK, ret);
+    status_publisher = server_node->create_publisher<ActionStatusMessage>(
+      status_topic_name, rcl_action_qos_profile_status_default);
+    ASSERT_TRUE(status_publisher != nullptr);
+    allocator.deallocate(status_topic_name, allocator.state);
+    server_executor.add_node(server_node);
+
+    client_node = std::make_shared<rclcpp::Node>(client_node_name, namespace_name);
+    client_executor.add_node(client_node);
+
+    ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock.get_clock_handle()));
+    ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(1)));
+  }
+
+  void Teardown()
+  {
+    status_publisher.reset();
+    feedback_publisher.reset();
+    cancel_service.reset();
+    result_service.reset();
+    goal_service.reset();
+    server_node.reset();
+    client_node.reset();
+  }
+
+  template<typename FutureT>
+  void dual_spin_until_future_complete(std::shared_future<FutureT> & future)
+  {
+    std::future_status status;
+    do {
+      server_executor.spin_some();
+      client_executor.spin_some();
+      status = future.wait_for(std::chrono::seconds(0));
+    } while (std::future_status::ready != status);
+  }
+
+  rclcpp::Clock clock{RCL_ROS_TIME};
+  const rclcpp::Time zero_stamp{0, 0, RCL_ROS_TIME};
+
+  rclcpp::Node::SharedPtr server_node;
+  rclcpp::executors::SingleThreadedExecutor server_executor;
+  rclcpp::Node::SharedPtr client_node;
+  rclcpp::executors::SingleThreadedExecutor client_executor;
+  const char * const server_node_name{"fibonacci_action_test_server"};
+  const char * const client_node_name{"fibonacci_action_test_client"};
+  const char * const namespace_name{"/rclcpp_action/test/client"};
+  const char * const action_name{"fibonacci_test"};
+
+  std::map<
+    rclcpp_action::GoalID,
+    std::pair<
+      typename ActionGoalRequest::SharedPtr,
+      typename ActionGoalResponse::SharedPtr>> goals;
+  typename rclcpp::Service<ActionGoalRequestService>::SharedPtr goal_service;
+  typename rclcpp::Service<ActionGoalResultService>::SharedPtr result_service;
+  typename rclcpp::Service<ActionCancelGoalService>::SharedPtr cancel_service;
+  typename rclcpp::Publisher<ActionFeedbackMessage>::SharedPtr feedback_publisher;
+  typename rclcpp::Publisher<ActionStatusMessage>::SharedPtr status_publisher;
 };
 
 TEST_F(TestClient, construction_and_destruction)
 {
-  auto node = std::make_shared<rclcpp::Node>("my_node", "/rclcpp_action/test/client");
+  ASSERT_NO_THROW(rclcpp_action::create_client<ActionType>(client_node, action_name).reset());
-
+}
-  auto ac = rclcpp_action::create_client<test_msgs::action::Fibonacci>(node.get(), "fibonacci");
+
-  (void)ac;
+TEST_F(TestClient, async_send_goal_but_ignore_feedback_and_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal bad_goal;
+  bad_goal.order = -5;
+  auto future_goal_handle = action_client->async_send_goal(bad_goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle);
+  EXPECT_EQ(nullptr, future_goal_handle.get().get());
+
+  ActionGoal good_goal;
+  good_goal.order = 5;
+  future_goal_handle = action_client->async_send_goal(good_goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_FALSE(goal_handle->is_result_aware());
+  EXPECT_THROW(goal_handle->async_result(), rclcpp_action::exceptions::UnawareGoalHandleError);
+}
+
+TEST_F(TestClient, async_send_goal_and_ignore_feedback_but_wait_for_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 5;
+  auto future_goal_handle = action_client->async_send_goal(goal);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_FALSE(goal_handle->is_feedback_aware());
+  EXPECT_TRUE(goal_handle->is_result_aware());
+  auto future_result = goal_handle->async_result();
+  dual_spin_until_future_complete(future_result);
+  auto result = future_result.get();
+  ASSERT_EQ(6ul, result.response->sequence.size());
+  EXPECT_EQ(0, result.response->sequence[0]);
+  EXPECT_EQ(1, result.response->sequence[1]);
+  EXPECT_EQ(5, result.response->sequence[5]);
+}
+
+TEST_F(TestClient, async_send_goal_with_feedback_and_result)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 4;
+  int feedback_count = 0;
+  auto future_goal_handle = action_client->async_send_goal(
+    goal,
+    [&feedback_count](
+      typename ActionGoalHandle::SharedPtr goal_handle,
+      const std::shared_ptr<const ActionFeedback> feedback) mutable
+    {
+      (void)goal_handle;
+      (void)feedback;
+      feedback_count++;
+    });
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+  EXPECT_TRUE(goal_handle->is_feedback_aware());
+  EXPECT_TRUE(goal_handle->is_result_aware());
+  auto future_result = goal_handle->async_result();
+  dual_spin_until_future_complete(future_result);
+  auto result = future_result.get();
+
+  ASSERT_EQ(5u, result.response->sequence.size());
+  EXPECT_EQ(3, result.response->sequence.back());
+  EXPECT_EQ(5, feedback_count);
+}
+
+TEST_F(TestClient, async_cancel_one_goal)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 5;
+  auto future_goal_handle = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle);
+  auto goal_handle = future_goal_handle.get();
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_ACCEPTED, goal_handle->get_status());
+
+  auto future_cancel = action_client->async_cancel_goal(goal_handle);
+  dual_spin_until_future_complete(future_cancel);
+  bool goal_canceled = future_cancel.get();
+  EXPECT_TRUE(goal_canceled);
+}
+
+TEST_F(TestClient, async_cancel_all_goals)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 6;
+  auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle0);
+  auto goal_handle0 = future_goal_handle0.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
+
+  goal.order = 8;
+  auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  if (goal_handle1->get_goal_id() < goal_handle0->get_goal_id()) {
+    goal_handle0.swap(goal_handle1);
+  }
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  auto future_cancel_all = action_client->async_cancel_all_goals();
+  dual_spin_until_future_complete(future_cancel_all);
+  auto cancel_response = future_cancel_all.get();
+
+  ASSERT_EQ(2ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(goal_handle1->get_goal_id(), cancel_response->goals_canceling[1].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle1->get_status());
+}
+
+TEST_F(TestClient, async_cancel_some_goals)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+
+  ActionGoal goal;
+  goal.order = 6;
+  auto future_goal_handle0 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle0);
+  auto goal_handle0 = future_goal_handle0.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(2)));
+
+  goal.order = 8;
+  auto future_goal_handle1 = action_client->async_send_goal(goal, nullptr, true);
+  dual_spin_until_future_complete(future_goal_handle1);
+  auto goal_handle1 = future_goal_handle1.get();
+
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock.get_clock_handle(), RCL_S_TO_NS(3)));
+
+  auto future_cancel_some =
+    action_client->async_cancel_goals_before(goal_handle1->get_goal_stamp());
+  dual_spin_until_future_complete(future_cancel_some);
+  auto cancel_response = future_cancel_some.get();
+
+  ASSERT_EQ(1ul, cancel_response->goals_canceling.size());
+  EXPECT_EQ(goal_handle0->get_goal_id(), cancel_response->goals_canceling[0].goal_id.uuid);
+  EXPECT_EQ(rclcpp_action::GoalStatus::STATUS_CANCELED, goal_handle0->get_status());
 }