Goal response callback compatibility shim with deprecation of old sig…

…nature (#1495) Signed-off-by: Ivan Santiago Paunovic <ivanpauno@ekumenlabs.com>
ros2 · Dec 21, 2020 · 6257103 · 6257103
1 parent 543a3c3
commit 6257103
Show file tree

Hide file tree

Showing 2 changed files with 155 additions and 3 deletions.
diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp
@@ -268,13 +268,107 @@ class Client : public ClientBase
   using Feedback = typename ActionT::Feedback;
   using GoalHandle = ClientGoalHandle<ActionT>;
   using WrappedResult = typename GoalHandle::WrappedResult;
-  using GoalResponseCallback = std::function<void (typename GoalHandle::SharedPtr)>;
   using FeedbackCallback = typename GoalHandle::FeedbackCallback;
   using ResultCallback = typename GoalHandle::ResultCallback;
   using CancelRequest = typename ActionT::Impl::CancelGoalService::Request;
   using CancelResponse = typename ActionT::Impl::CancelGoalService::Response;
   using CancelCallback = std::function<void (typename CancelResponse::SharedPtr)>;
 
+  /// Compatibility wrapper for `goal_response_callback`.
+  class GoalResponseCallback
+  {
+public:
+    using NewSignature = std::function<void (typename GoalHandle::SharedPtr)>;
+    using OldSignature = std::function<void (std::shared_future<typename GoalHandle::SharedPtr>)>;
+
+    GoalResponseCallback() = default;
+
+    GoalResponseCallback(std::nullptr_t) {}  // NOLINT, intentionally implicit.
+
+    // implicit constructor
+    [[deprecated(
+      "Use new goal response callback signature "
+      "`std::function<void (Client<ActionT>::GoalHandle::SharedPtr)>` "
+      "instead of the old "
+      "`std::function<void (std::shared_future<Client<ActionT>::GoalHandle::SharedPtr>)>`.\n"
+      "e.g.:\n"
+      "```cpp\n"
+      "Client<ActionT>::SendGoalOptions options;\n"
+      "options.goal_response_callback = [](Client<ActionT>::GoalHandle::SharedPtr goal) {\n"
+      "  // do something with `goal` here\n"
+      "};")]]
+    GoalResponseCallback(OldSignature old_callback)  // NOLINT, intentionally implicit.
+    : old_callback_(std::move(old_callback)) {}
+
+    GoalResponseCallback(NewSignature new_callback)  // NOLINT, intentionally implicit.
+    : new_callback_(std::move(new_callback)) {}
+
+    GoalResponseCallback &
+    operator=(OldSignature old_callback) {old_callback_ = std::move(old_callback); return *this;}
+
+    GoalResponseCallback &
+    operator=(NewSignature new_callback) {new_callback_ = std::move(new_callback); return *this;}
+
+    void
+    operator()(typename GoalHandle::SharedPtr goal_handle) const
+    {
+      if (new_callback_) {
+        new_callback_(std::move(goal_handle));
+        return;
+      }
+      if (old_callback_) {
+        throw std::runtime_error{
+                "Cannot call GoalResponseCallback(GoalHandle::SharedPtr) "
+                "if using the old goal response callback signature."};
+      }
+      throw std::bad_function_call{};
+    }
+
+    [[deprecated(
+      "Calling "
+      "`void goal_response_callback("
+      "   std::shared_future<Client<ActionT>::GoalHandle::SharedPtr> goal_handle_shared_future)`"
+      " is deprecated.")]]
+    void
+    operator()(std::shared_future<typename GoalHandle::SharedPtr> goal_handle_future) const
+    {
+      if (old_callback_) {
+        old_callback_(std::move(goal_handle_future));
+        return;
+      }
+      if (new_callback_) {
+        new_callback_(std::move(goal_handle_future).get_future().share());
+        return;
+      }
+      throw std::bad_function_call{};
+    }
+
+    explicit operator bool() const noexcept {
+      return new_callback_ || old_callback_;
+    }
+
+private:
+    friend class Client;
+    void
+    operator()(
+      typename GoalHandle::SharedPtr goal_handle,
+      std::shared_future<typename GoalHandle::SharedPtr> goal_handle_future) const
+    {
+      if (new_callback_) {
+        new_callback_(std::move(goal_handle));
+        return;
+      }
+      if (old_callback_) {
+        old_callback_(std::move(goal_handle_future));
+        return;
+      }
+      throw std::bad_function_call{};
+    }
+
+    NewSignature new_callback_;
+    OldSignature old_callback_;
+  };
+
   /// Options for sending a goal.
   /**
    * This struct is used to pass parameters to the function `async_send_goal`.
@@ -363,7 +457,7 @@ class Client : public ClientBase
         if (!goal_response->accepted) {
           promise->set_value(nullptr);
           if (options.goal_response_callback) {
-            options.goal_response_callback(nullptr);
+            options.goal_response_callback(nullptr, future);
           }
           return;
         }
@@ -379,7 +473,7 @@ class Client : public ClientBase
         }
         promise->set_value(goal_handle);
         if (options.goal_response_callback) {
-          options.goal_response_callback(goal_handle);
+          options.goal_response_callback(goal_handle, future);
         }
 
         if (options.result_callback) {

diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp
@@ -536,6 +536,64 @@ TEST_F(TestClientAgainstServer, async_send_goal_with_goal_response_callback_wait
   }
 }
 
+TEST_F(TestClientAgainstServer, async_send_goal_with_deprecated_goal_response_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));
+
+  bool goal_response_received = false;
+  auto send_goal_ops = rclcpp_action::Client<ActionType>::SendGoalOptions();
+
+#if !defined(_WIN32)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#else
+# pragma warning(push)
+# pragma warning(disable: 4996)
+#endif
+  send_goal_ops.goal_response_callback =
+    [&goal_response_received]
+      (std::shared_future<typename ActionGoalHandle::SharedPtr> goal_future)
+    {
+      if (goal_future.get()) {
+        goal_response_received = true;
+      }
+    };
+#if !defined(_WIN32)
+# pragma GCC diagnostic pop
+#else
+# pragma warning(pop)
+#endif
+
+  {
+    ActionGoal bad_goal;
+    bad_goal.order = -1;
+    auto future_goal_handle = action_client->async_send_goal(bad_goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    EXPECT_FALSE(goal_response_received);
+    EXPECT_EQ(nullptr, goal_handle);
+  }
+
+  {
+    ActionGoal goal;
+    goal.order = 4;
+    auto future_goal_handle = action_client->async_send_goal(goal, send_goal_ops);
+    dual_spin_until_future_complete(future_goal_handle);
+    auto goal_handle = future_goal_handle.get();
+    EXPECT_TRUE(goal_response_received);
+    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());
+    auto future_result = action_client->async_get_result(goal_handle);
+    EXPECT_TRUE(goal_handle->is_result_aware());
+    dual_spin_until_future_complete(future_result);
+    auto wrapped_result = future_result.get();
+    ASSERT_EQ(5u, wrapped_result.result->sequence.size());
+    EXPECT_EQ(3, wrapped_result.result->sequence.back());
+  }
+}
+
 TEST_F(TestClientAgainstServer, async_send_goal_with_feedback_callback_wait_for_result)
 {
   auto action_client = rclcpp_action::create_client<ActionType>(client_node, action_name);