[rclcpp_action] Action client implementation

rclcpp_action/include/rclcpp_action/client_goal_handle.hpp

@@ -19,6 +19,7 @@
 
 #include <action_msgs/msg/goal_status.hpp>
 #include <rclcpp/macros.hpp>
+#include <rclcpp/time.hpp>
 
 #include <functional>
 #include <future>
@@ -41,10 +42,10 @@ enum class ResultCode : int8_t
 
 
 // Forward declarations
-template<typename ACTION>
+template<typename ActionT>
 class Client;
 
-template<typename ACTION>
+template<typename ActionT>
 class ClientGoalHandle
 {
 public:
@@ -58,22 +59,24 @@ class ClientGoalHandle
     /// 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;
+    typename ActionT::Result::SharedPtr response;
   } Result;
 
-  using Feedback = typename ACTION::Feedback;
-  using FeedbackCallback = std::function<void (ClientGoalHandle::SharedPtr, const Feedback &)>;
+  using Feedback = typename ActionT::Feedback;
+  using FeedbackCallback =
+    std::function<void (ClientGoalHandle::SharedPtr, const std::shared_ptr<const Feedback>)>;
 
   virtual ~ClientGoalHandle();
 
-  const GoalID & get_goal_id() const;
+  const GoalID &
+  get_goal_id() const;
+
+  rclcpp::Time
+  get_goal_stamp() const;
 
   std::shared_future<Result>
   async_result();
 
-  FeedbackCallback
-  get_feedback_callback();
-
   int8_t
   get_status();
 
@@ -85,13 +88,18 @@ class ClientGoalHandle
 
 private:
   // The templated Client creates goal handles
-  friend Client<ACTION>;
+  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);
 

rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp

@@ -17,34 +17,44 @@
 
 #include <rcl_action/types.h>
 
+#include <memory>
+
 #include "rclcpp_action/exceptions.hpp"
 
 namespace rclcpp_action
 {
 
-template<typename ACTION>
-ClientGoalHandle<ACTION>::ClientGoalHandle(
+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>
+template<typename ActionT>
 const GoalID &
-ClientGoalHandle<ACTION>::get_goal_id() const
+ClientGoalHandle<ActionT>::get_goal_id() const
 {
   // return info_.goal_id;
   return info_.goal_id.uuid;
 }
 
-template<typename ACTION>
-std::shared_future<typename ClientGoalHandle<ACTION>::Result>
-ClientGoalHandle<ACTION>::async_result()
+template<typename ActionT>
+rclcpp::Time
+ClientGoalHandle<ActionT>::get_goal_stamp() const
+{
+  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_) {
@@ -53,81 +63,92 @@ ClientGoalHandle<ACTION>::async_result()
   return result_future_;
 }
 
-template<typename ACTION>
+template<typename ActionT>
 void
-ClientGoalHandle<ACTION>::set_result(const Result & result)
+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 ACTION>
-typename ClientGoalHandle<ACTION>::FeedbackCallback
-ClientGoalHandle<ACTION>::get_feedback_callback()
-{
-  std::lock_guard<std::mutex> guard(handle_mutex_);
-  return feedback_callback_;
-}
-
-template<typename ACTION>
+template<typename ActionT>
 void
-ClientGoalHandle<ACTION>::set_feedback_callback(FeedbackCallback callback)
+ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
   feedback_callback_ = callback;
 }
 
-template<typename ACTION>
+template<typename ActionT>
 int8_t
-ClientGoalHandle<ACTION>::get_status()
+ClientGoalHandle<ActionT>::get_status()
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
   return status_;
 }
 
-template<typename ACTION>
+template<typename ActionT>
 void
-ClientGoalHandle<ACTION>::set_status(int8_t status)
+ClientGoalHandle<ActionT>::set_status(int8_t status)
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
   status_ = status;
 }
 
-template<typename ACTION>
+template<typename ActionT>
 bool
-ClientGoalHandle<ACTION>::is_feedback_aware()
+ClientGoalHandle<ActionT>::is_feedback_aware()
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
   return feedback_callback_ != nullptr;
 }
 
-template<typename ACTION>
+template<typename ActionT>
 bool
-ClientGoalHandle<ACTION>::is_result_aware()
+ClientGoalHandle<ActionT>::is_result_aware()
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
   return is_result_aware_;
 }
 
-template<typename ACTION>
+template<typename ActionT>
 void
-ClientGoalHandle<ACTION>::set_result_awareness(bool awareness)
+ClientGoalHandle<ActionT>::set_result_awareness(bool awareness)
 {
   std::lock_guard<std::mutex> guard(handle_mutex_);
   is_result_aware_ = awareness;
 }
 
-template<typename ACTION>
+template<typename ActionT>
 void
-ClientGoalHandle<ACTION>::invalidate()
+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,8 +25,8 @@
 
 namespace rclcpp_action
 {
-template<typename ACTION>
-typename Client<ACTION>::SharedPtr
+template<typename ActionT>
+typename Client<ActionT>::SharedPtr
 create_client(
   rclcpp::Node::SharedPtr node,
   const std::string & name,
@@ -37,7 +37,7 @@ create_client(
   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)
+  auto deleter = [weak_node, weak_group, group_is_null](Client<ActionT> * ptr)
     {
       if (nullptr == ptr) {
         return;
@@ -47,7 +47,7 @@ create_client(
         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> *) {});
+      std::shared_ptr<Client<ActionT>> fake_shared_ptr(ptr, [](Client<ActionT> *) {});
 
       if (group_is_null) {
         // Was added to default group
@@ -62,8 +62,9 @@ create_client(
       delete ptr;
     };
 
-  std::shared_ptr<Client<ACTION>> action_client(
-    new Client<ACTION>(node->get_node_base_interface(), name), deleter);
+  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;

rclcpp_action/include/rclcpp_action/exceptions.hpp

@@ -21,21 +21,11 @@ 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("")
+  : std::invalid_argument("Goal handle is not know to this client.")
   {
   }
 };
@@ -44,7 +34,7 @@ class UnawareGoalHandleError : public std::runtime_error
 {
 public:
   UnawareGoalHandleError()
-  : std::runtime_error("")
+  : std::runtime_error("Goal handle is not tracking the goal result.")
   {
   }
 };

rclcpp_action/include/rclcpp_action/types.hpp

@@ -25,8 +25,8 @@
 
 namespace rclcpp_action
 {
-// using GoalID = unique_identifier_msgs::msg::UUID;
-using GoalID = std::array<uint8_t, 16>;
+
+using GoalID = std::array<uint8_t, UUID_SIZE>;
 using GoalStatus = action_msgs::msg::GoalStatus;
 using GoalInfo = action_msgs::msg::GoalInfo;
 
@@ -38,11 +38,10 @@ template<>
 struct less<rclcpp_action::GoalID>
 {
   bool operator()(
-    const rclcpp_action::GoalID & id0,
-    const rclcpp_action::GoalID & id1) const
+    const rclcpp_action::GoalID & lhs,
+    const rclcpp_action::GoalID & rhs) const
   {
-    // return (id0.uuid < id1.uuid);
-    return id0 < id1;
+    return lhs < rhs;
   }
 };
 }  // namespace std