[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.
-class ClientBase
+/// \internal
+/**
+ * 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_;
 };
 
-
-/// Templated Action Client class
-/// It is responsible for getting the C action type support struct from the C++ type, and
-/// calling user callbacks with goal handles of the appropriate type.
-template<typename ACTION>
+/// Action Client
+/**
+ * This class creates an action client.
+ *
+ * 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 (
-        std::shared_ptr<ClientGoalHandle<ACTION>>, const typename ACTION::Feedback)>;
+  using Goal = typename ActionT::Goal;
+  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,
-      name,
-      rosidl_typesupport_cpp::get_action_type_support_handle<ACTION>())
-  {
-    // 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()
+      node_base, node_logging, action_name,
+      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
+      client_options)
   {
   }
+
+  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 <memory>
+#include <action_msgs/msg/goal_status.hpp>
+#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)`
-  std::future<bool>
-  async_cancel();
+  const GoalID &
+  get_goal_id() const;
 
-  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
-  friend Client<ACTION>;
+  // The templated Client creates goal handles
+  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
-  rcl_action_client_t * rcl_client_;
-  rcl_action_goal_info_t rcl_info_;
+  void
+  set_feedback_callback(FeedbackCallback callback);
+
+  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(
-  rcl_action_client_t * rcl_client,
-  const rcl_action_goal_info_t rcl_info
-)
-: rcl_client_(rcl_client), rcl_info_(rcl_info)
+
+template<typename ActionT>
+ClientGoalHandle<ActionT>::ClientGoalHandle(
+  const GoalInfo & info, FeedbackCallback callback)
+: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback)
 {
 }
 
-template<typename ACTION>
-ClientGoalHandle<ACTION>::~ClientGoalHandle()
+template<typename ActionT>
+ClientGoalHandle<ActionT>::~ClientGoalHandle()
 {
 }
 
-template<typename ACTION>
-std::future<bool>
-ClientGoalHandle<ACTION>::async_cancel()
+template<typename ActionT>
+const GoalID &
+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>
-std::future<typename ACTION::Result>
-ClientGoalHandle<ACTION>::async_result()
+template<typename ActionT>
+rclcpp::Time
+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>
-typename Client<ACTION>::SharedPtr
+template<typename ActionT>
+typename Client<ActionT>::SharedPtr
 create_client(
-  rclcpp::Node * node,
-  const std::string & name)
+  rclcpp::Node::SharedPtr node,
+  const std::string & name,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
 {
-  return Client<ACTION>::make_shared(
-    node->get_node_base_interface(),
-    name);
+  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](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_