Add const reference service callback signatures

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -96,6 +96,20 @@ class AnyServiceCallback
       >::value)
     {
       callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        ConstRefCallback
+      >::value)
+    {
+      callback_.template emplace<ConstRefCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        ConstRefWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<ConstRefWithRequestHeaderCallback>(callback);
     } else {
       // the else clause is not needed, but anyways we should only be doing this instead
       // of all the above workaround ...
@@ -141,6 +155,20 @@ class AnyServiceCallback
       >::value)
     {
       callback_.template emplace<SharedPtrDeferResponseCallbackWithServiceHandle>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        ConstRefCallback
+      >::value)
+    {
+      callback_.template emplace<ConstRefCallback>(callback);
+    } else if constexpr (  // NOLINT
+      rclcpp::function_traits::same_arguments<
+        CallbackT,
+        ConstRefWithRequestHeaderCallback
+      >::value)
+    {
+      callback_.template emplace<ConstRefWithRequestHeaderCallback>(callback);
     } else {
       // the else clause is not needed, but anyways we should only be doing this instead
       // of all the above workaround ...
@@ -182,6 +210,18 @@ class AnyServiceCallback
     } else if (std::holds_alternative<SharedPtrWithRequestHeaderCallback>(callback_)) {
       const auto & cb = std::get<SharedPtrWithRequestHeaderCallback>(callback_);
       cb(request_header, std::move(request), response);
+    } else if (std::holds_alternative<ConstRefCallback>(callback_)) {
+      if (nullptr == request) {
+        throw std::runtime_error("dispatch called with a null request");
+      }
+      const auto & cb = std::get<ConstRefCallback>(callback_);
+      cb(*request, *response);
+    } else if (std::holds_alternative<ConstRefWithRequestHeaderCallback>(callback_)) {
+      if (nullptr == request_header || nullptr == request) {
+        throw std::runtime_error("dispatch called with a null request header or request");
+      }
+      const auto & cb = std::get<ConstRefWithRequestHeaderCallback>(callback_);
+      cb(*request_header, *request, *response);
     }
     TRACETOOLS_TRACEPOINT(callback_end, static_cast<const void *>(this));
     return response;
@@ -227,13 +267,28 @@ class AnyServiceCallback
       std::shared_ptr<rmw_request_id_t>,
       std::shared_ptr<typename ServiceT::Request>
     )>;
+  // The request and response are received as shared pointers and dereferenced before
+  // calling the user callback, like AnySubscriptionCallback does for `const MessageT &`.
+  using ConstRefCallback = std::function<
+    void (
+      const typename ServiceT::Request &,
+      typename ServiceT::Response &
+    )>;
+  using ConstRefWithRequestHeaderCallback = std::function<
+    void (
+      const rmw_request_id_t &,
+      const typename ServiceT::Request &,
+      typename ServiceT::Response &
+    )>;
 
   std::variant<
     std::monostate,
     SharedPtrCallback,
     SharedPtrWithRequestHeaderCallback,
     SharedPtrDeferResponseCallback,
-    SharedPtrDeferResponseCallbackWithServiceHandle> callback_;
+    SharedPtrDeferResponseCallbackWithServiceHandle,
+    ConstRefCallback,
+    ConstRefWithRequestHeaderCallback> callback_;
 };
 
 }  // namespace rclcpp

rclcpp/test/rclcpp/test_any_service_callback.cpp

@@ -23,6 +23,7 @@
 
 #include "rclcpp/any_service_callback.hpp"
 #include "rclcpp/service.hpp"
+#include "test_msgs/srv/basic_types.hpp"
 #include "test_msgs/srv/empty.hpp"
 
 class TestAnyServiceCallback : public ::testing::Test
@@ -109,3 +110,112 @@ TEST_F(TestAnyServiceCallback, set_and_dispatch_defered_with_service_handle) {
     EXPECT_EQ(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
   EXPECT_EQ(callback_with_header_calls, 1);
 }
+
+TEST_F(TestAnyServiceCallback, set_and_dispatch_const_ref_no_header) {
+  int callback_calls = 0;
+  auto callback = [&callback_calls](
+    const test_msgs::srv::Empty::Request &,
+    test_msgs::srv::Empty::Response &)
+    {
+      callback_calls++;
+    };
+
+  any_service_callback_.set(callback);
+  EXPECT_NO_THROW(
+    EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
+  EXPECT_EQ(callback_calls, 1);
+}
+
+TEST_F(TestAnyServiceCallback, set_and_dispatch_const_ref_header) {
+  int callback_calls = 0;
+  int64_t seen_sequence_number = 0;
+  request_header_->sequence_number = 42;
+  auto callback = [&callback_calls, &seen_sequence_number](
+    const rmw_request_id_t & request_header,
+    const test_msgs::srv::Empty::Request &,
+    test_msgs::srv::Empty::Response &)
+    {
+      callback_calls++;
+      seen_sequence_number = request_header.sequence_number;
+    };
+
+  any_service_callback_.set(callback);
+  EXPECT_NO_THROW(
+    EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_)));
+  EXPECT_EQ(callback_calls, 1);
+  EXPECT_EQ(seen_sequence_number, 42);
+}
+
+TEST_F(TestAnyServiceCallback, const_ref_response_is_returned) {
+  rclcpp::AnyServiceCallback<test_msgs::srv::BasicTypes> any_service_callback;
+  auto request = std::make_shared<test_msgs::srv::BasicTypes::Request>();
+  request->int64_value = 7;
+  request->string_value = "ping";
+
+  auto callback = [](
+    const test_msgs::srv::BasicTypes::Request & req,
+    test_msgs::srv::BasicTypes::Response & res)
+    {
+      res.int64_value = req.int64_value * 2;
+      res.string_value = req.string_value + "-pong";
+    };
+
+  any_service_callback.set(callback);
+  auto response = any_service_callback.dispatch(nullptr, request_header_, request);
+  ASSERT_NE(nullptr, response);
+  EXPECT_EQ(response->int64_value, 14);
+  EXPECT_EQ(response->string_value, "ping-pong");
+}
+
+TEST_F(TestAnyServiceCallback, set_and_dispatch_const_ref_std_bind) {
+  struct Handler
+  {
+    int calls = 0;
+    void no_header(const test_msgs::srv::Empty::Request &, test_msgs::srv::Empty::Response &)
+    {
+      calls++;
+    }
+    void header(
+      const rmw_request_id_t &,
+      const test_msgs::srv::Empty::Request &,
+      test_msgs::srv::Empty::Response &)
+    {
+      calls++;
+    }
+  };
+  Handler handler;
+
+  any_service_callback_.set(
+    std::bind(&Handler::no_header, &handler, std::placeholders::_1, std::placeholders::_2));
+  EXPECT_NE(nullptr, any_service_callback_.dispatch(nullptr, request_header_, request_));
+
+  rclcpp::AnyServiceCallback<test_msgs::srv::Empty> any_service_callback_with_header;
+  any_service_callback_with_header.set(
+    std::bind(
+      &Handler::header, &handler,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+  EXPECT_NE(
+    nullptr, any_service_callback_with_header.dispatch(nullptr, request_header_, request_));
+  EXPECT_EQ(handler.calls, 2);
+}
+
+TEST_F(TestAnyServiceCallback, const_ref_dispatch_null_arguments_throw) {
+  any_service_callback_.set(
+    [](const test_msgs::srv::Empty::Request &, test_msgs::srv::Empty::Response &) {});
+  EXPECT_THROW(
+    any_service_callback_.dispatch(nullptr, request_header_, nullptr),
+    std::runtime_error);
+
+  rclcpp::AnyServiceCallback<test_msgs::srv::Empty> any_service_callback_with_header;
+  any_service_callback_with_header.set(
+    [](
+      const rmw_request_id_t &,
+      const test_msgs::srv::Empty::Request &,
+      test_msgs::srv::Empty::Response &) {});
+  EXPECT_THROW(
+    any_service_callback_with_header.dispatch(nullptr, nullptr, request_),
+    std::runtime_error);
+  EXPECT_THROW(
+    any_service_callback_with_header.dispatch(nullptr, request_header_, nullptr),
+    std::runtime_error);
+}

rclcpp/test/rclcpp/test_service.cpp

@@ -414,3 +414,26 @@ TEST_F(TestService, server_qos_depth) {
 
   EXPECT_EQ(server_cb_count_, server_qos_profile.depth());
 }
+
+TEST_F(TestService, const_ref_callback) {
+  uint64_t server_cb_count = 0;
+  auto server_callback = [&server_cb_count](
+    const test_msgs::srv::Empty::Request &,
+    test_msgs::srv::Empty::Response &) {server_cb_count++;};
+
+  auto server = node->create_service<test_msgs::srv::Empty>(
+    "test_const_ref_callback", std::move(server_callback));
+  auto client = node->create_client<test_msgs::srv::Empty>("test_const_ref_callback");
+
+  auto request = std::make_shared<test_msgs::srv::Empty::Request>();
+  auto future = client->async_send_request(request);
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  EXPECT_EQ(
+    executor.spin_until_future_complete(future, 10s),
+    rclcpp::FutureReturnCode::SUCCESS);
+
+  EXPECT_EQ(server_cb_count, 1u);
+  EXPECT_NE(nullptr, future.get());
+}