From 9a26a24e3b38bd0dfbda1c5babb5633306ed8f30 Mon Sep 17 00:00:00 2001
From: Jacob Perron <jacob@openrobotics.org>
Date: Tue, 23 Apr 2019 15:38:33 -0700
Subject: [PATCH] Add optional result callback to async_get_result

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
---
 .../include/rclcpp_action/client.hpp          | 13 +++++--
 .../rclcpp_action/client_goal_handle.hpp      |  3 ++
 .../rclcpp_action/client_goal_handle_impl.hpp |  8 +++++
 rclcpp_action/test/test_client.cpp            | 35 +++++++++++++++++++
 4 files changed, 56 insertions(+), 3 deletions(-)

diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp
index a9cb8e6ec2..36e01933a7 100644
--- a/rclcpp_action/include/rclcpp_action/client.hpp
+++ b/rclcpp_action/include/rclcpp_action/client.hpp
@@ -263,8 +263,8 @@ class Client : public ClientBase
   using WrappedResult = typename GoalHandle::WrappedResult;
   using GoalResponseCallback =
     std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
-  using FeedbackCallback = typename ClientGoalHandle<ActionT>::FeedbackCallback;
-  using ResultCallback = typename ClientGoalHandle<ActionT>::ResultCallback;
+  using FeedbackCallback = typename GoalHandle::FeedbackCallback;
+  using ResultCallback = typename GoalHandle::ResultCallback;
   using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
   using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
 
@@ -391,15 +391,22 @@ class Client : public ClientBase
    * \throws exceptions::UnknownGoalHandleError If the goal unknown or already reached a terminal
    *   state.
    * \param[in] goal_handle The goal handle for which to get the result.
+   * \param[in] result_callback Optional callback that is called when the result is received.
    * \return A future that is set to the goal result when the goal is finished.
    */
   std::shared_future<WrappedResult>
-  async_get_result(typename GoalHandle::SharedPtr goal_handle)
+  async_get_result(
+    typename GoalHandle::SharedPtr goal_handle,
+    ResultCallback result_callback = nullptr)
   {
     std::lock_guard<std::mutex> lock(goal_handles_mutex_);
     if (goal_handles_.count(goal_handle->get_goal_id()) == 0) {
       throw exceptions::UnknownGoalHandleError();
     }
+    if (result_callback) {
+      // This will override any previously registered callback
+      goal_handle->set_result_callback(result_callback);
+    }
     // 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);
diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
index f367fefbcd..28210fad5b 100644
--- a/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
+++ b/rclcpp_action/include/rclcpp_action/client_goal_handle.hpp
@@ -126,6 +126,9 @@ class ClientGoalHandle
   void
   set_feedback_callback(FeedbackCallback callback);
 
+  void
+  set_result_callback(ResultCallback callback);
+
   void
   call_feedback_callback(
     typename ClientGoalHandle<ActionT>::SharedPtr shared_this,
diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
index 23fb200ebd..2a491014f1 100644
--- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
+++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp
@@ -86,6 +86,14 @@ ClientGoalHandle<ActionT>::set_feedback_callback(FeedbackCallback callback)
   feedback_callback_ = callback;
 }
 
+template<typename ActionT>
+void
+ClientGoalHandle<ActionT>::set_result_callback(ResultCallback callback)
+{
+  std::lock_guard<std::mutex> guard(handle_mutex_);
+  result_callback_ = callback;
+}
+
 template<typename ActionT>
 int8_t
 ClientGoalHandle<ActionT>::get_status()
diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp
index 61d07dac54..e17ba6a73b 100644
--- a/rclcpp_action/test/test_client.cpp
+++ b/rclcpp_action/test/test_client.cpp
@@ -415,6 +415,41 @@ TEST_F(TestClient, async_send_goal_with_result_callback_wait_for_result)
   EXPECT_EQ(3, wrapped_result.result->sequence.back());
 }
 
+TEST_F(TestClient, async_get_result_with_callback)
+{
+  auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);
+  ASSERT_TRUE(action_client->wait_for_action_server(WAIT_FOR_SERVER_TIMEOUT));
+
+  ActionGoal goal;
+  goal.order = 4;
+  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_NE(goal_handle, nullptr);
+  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());
+  bool result_callback_received = false;
+  auto future_result = action_client->async_get_result(
+    goal_handle,
+    [&result_callback_received](
+      const typename ActionGoalHandle::WrappedResult & result) mutable
+    {
+      if (
+        rclcpp_action::ResultCode::SUCCEEDED == result.code &&
+        result.result->sequence.size() == 5u)
+      {
+        result_callback_received = true;
+      }
+    });
+  dual_spin_until_future_complete(future_result);
+  auto wrapped_result = future_result.get();
+
+  EXPECT_TRUE(result_callback_received);
+  ASSERT_EQ(5u, wrapped_result.result->sequence.size());
+  EXPECT_EQ(3, wrapped_result.result->sequence.back());
+}
+
 TEST_F(TestClient, async_cancel_one_goal)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);