[rclcpp_action] Action client implementation

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -17,39 +17,111 @@
 
 #include <rcl_action/action_client.h>
 
+#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 (ClientGoalHandle::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;
+
+  rclcpp::Time
+  get_goal_stamp() const;
 
-  std::future<typename ACTION::Result>
+  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(const GoalInfo & info, FeedbackCallback callback);
+
+  void
+  set_feedback_callback(FeedbackCallback callback);
+
+  void
+  call_feedback_callback(
+    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_;
 
-  ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info);
+  FeedbackCallback feedback_callback_{nullptr};
+  int8_t status_{GoalStatus::STATUS_ACCEPTED};
 
-  // 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_;
+  std::mutex handle_mutex_;
 };
 }  // namespace rclcpp_action
 

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -17,35 +17,138 @@
 
 #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 ActionT>
+ClientGoalHandle<ActionT>::~ClientGoalHandle()
+{
+}
+
+template<typename ActionT>
+const GoalID &
+ClientGoalHandle<ActionT>::get_goal_id() const
+{
+  // return info_.goal_id;
+  return info_.goal_id.uuid;
+}
+
+template<typename ActionT>
+rclcpp::Time
+ClientGoalHandle<ActionT>::get_goal_stamp() const
 {
+  return info_.stamp;
 }
 
-template<typename ACTION>
-ClientGoalHandle<ACTION>::~ClientGoalHandle()
+
+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 ACTION>
-std::future<bool>
-ClientGoalHandle<ACTION>::async_cancel()
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_result(const Result & result)
 {
-  throw std::runtime_error("Failed to cancel goal");
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = static_cast<int8_t>(result.code);
+  result_promise_.set_value(result);
 }
 
-template<typename ACTION>
-std::future<typename ACTION::Result>
-ClientGoalHandle<ACTION>::async_result()
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
 {
-  throw std::runtime_error("Failed to get result future");
+  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(
+  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 (feedback_callback_ == nullptr) {
+    // 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_