[rclcpp_action] Action client implementation

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -17,13 +17,29 @@
 
 #include <rcl_action/action_client.h>
 
+#include <action_msgs/msg/goal_status.hpp>
+#include <rclcpp/macros.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>
 class Client;
@@ -32,24 +48,72 @@ template<typename ACTION>
 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 ACTION::Result::SharedPtr response;
+  } Result;
+
+  using Feedback = typename ACTION::Feedback;
+  using FeedbackCallback = std::function<void (ClientGoalHandle::SharedPtr, 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>
+  std::shared_future<Result>
   async_result();
 
+  FeedbackCallback
+  get_feedback_callback();
+
+  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>;
 
-  ClientGoalHandle(rcl_action_client_t * rcl_client, const rcl_action_goal_info_t rcl_info);
+  ClientGoalHandle(const GoalInfo & info, FeedbackCallback callback);
+
+  void
+  set_feedback_callback(FeedbackCallback callback);
+
+  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};
 
-  // 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,14 +17,15 @@
 
 #include <rcl_action/types.h>
 
+#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)
+  const GoalInfo & info, FeedbackCallback callback)
+: info_(info), result_future_(result_promise_.get_future()), feedback_callback_(callback)
 {
 }
 
@@ -34,18 +35,99 @@ ClientGoalHandle<ACTION>::~ClientGoalHandle()
 }
 
 template<typename ACTION>
-std::future<bool>
-ClientGoalHandle<ACTION>::async_cancel()
+const GoalID &
+ClientGoalHandle<ACTION>::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>
+std::shared_future<typename ClientGoalHandle<ACTION>::Result>
 ClientGoalHandle<ACTION>::async_result()
 {
-  throw std::runtime_error("Failed to get result future");
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  if (!is_result_aware_) {
+    throw exceptions::UnawareGoalHandleError();
+  }
+  return result_future_;
+}
+
+template<typename ACTION>
+void
+ClientGoalHandle<ACTION>::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 ACTION>
+typename ClientGoalHandle<ACTION>::FeedbackCallback
+ClientGoalHandle<ACTION>::get_feedback_callback()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return feedback_callback_;
 }
+
+template<typename ACTION>
+void
+ClientGoalHandle<ACTION>::set_feedback_callback(FeedbackCallback callback)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  feedback_callback_ = callback;
+}
+
+template<typename ACTION>
+int8_t
+ClientGoalHandle<ACTION>::get_status()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return status_;
+}
+
+template<typename ACTION>
+void
+ClientGoalHandle<ACTION>::set_status(int8_t status)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = status;
+}
+
+template<typename ACTION>
+bool
+ClientGoalHandle<ACTION>::is_feedback_aware()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return feedback_callback_ != nullptr;
+}
+
+template<typename ACTION>
+bool
+ClientGoalHandle<ACTION>::is_result_aware()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  return is_result_aware_;
+}
+
+template<typename ACTION>
+void
+ClientGoalHandle<ACTION>::set_result_awareness(bool awareness)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  is_result_aware_ = awareness;
+}
+
+template<typename ACTION>
+void
+ClientGoalHandle<ACTION>::invalidate()
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  status_ = GoalStatus::STATUS_UNKNOWN;
+  result_promise_.set_exception(std::make_exception_ptr(
+      exceptions::UnawareGoalHandleError()));
+}
+
 }  // namespace rclcpp_action
 
 #endif  // RCLCPP_ACTION__CLIENT_GOAL_HANDLE_IMPL_HPP_

rclcpp_action/include/rclcpp_action/create_client.hpp

@@ -28,13 +28,46 @@ namespace rclcpp_action
 template<typename ACTION>
 typename Client<ACTION>::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<ACTION> * 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<ACTION>> fake_shared_ptr(ptr, [](Client<ACTION> *) {});
+
+      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<ACTION>> action_client(
+    new Client<ACTION>(node->get_node_base_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,56 @@
+// 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 RejectedGoalError : public std::runtime_error
+{
+public:
+  RejectedGoalError()
+  : std::runtime_error("")
+  {
+  }
+};
+
+class UnknownGoalHandleError : public std::invalid_argument
+{
+public:
+  UnknownGoalHandleError()
+  : std::invalid_argument("")
+  {
+  }
+};
+
+class UnawareGoalHandleError : public std::runtime_error
+{
+public:
+  UnawareGoalHandleError()
+  : std::runtime_error("")
+  {
+  }
+};
+
+}  // namespace exceptions
+
+}  // namespace rclcpp_action
+
+#endif  // RCLCPP_ACTION__EXCEPTIONS_HPP_

rclcpp_action/include/rclcpp_action/types.hpp

@@ -0,0 +1,49 @@
+// 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 = unique_identifier_msgs::msg::UUID;
+using GoalID = std::array<uint8_t, 16>;
+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 & id0,
+    const rclcpp_action::GoalID & id1) const
+  {
+    // return (id0.uuid < id1.uuid);
+    return id0 < id1;
+  }
+};
+}  // namespace std
+#endif  // RCLCPP_ACTION__TYPES_HPP_