Allow deferred service replies. (#29)

Author: StefanFabian

ros_babel_fish/include/ros_babel_fish/detail/any_service_callback.hpp

@@ -4,12 +4,13 @@
 #ifndef ROS_BABEL_FISH_BABEL_FISH_ANY_SERVICE_CALLBACK_HPP
 #define ROS_BABEL_FISH_BABEL_FISH_ANY_SERVICE_CALLBACK_HPP
 
+#include "ros_babel_fish/messages/compound_message.hpp"
+
 #include <rclcpp/any_service_callback.hpp>
 
 namespace ros_babel_fish
 {
 class BabelFishService;
-class CompoundMessage;
 
 /**
  * Designed to be compatible with rclcpp::AnyServiceCallback compatible callbacks with equivalent tracing support.
@@ -53,26 +54,38 @@ class AnyServiceCallback
     }
   }
 
-  void dispatch( const std::shared_ptr<BabelFishService> &service_handle,
-                 const std::shared_ptr<rmw_request_id_t> &request_header,
-                 std::shared_ptr<CompoundMessage> request, std::shared_ptr<CompoundMessage> response )
+  //! Dispatches the request to the registered callback.
+  //! @return The filled response which the caller should send, or nullptr if the callback deferred
+  //!   the response (a defer variant), in which case the caller must not send.
+  std::shared_ptr<CompoundMessage> dispatch( const std::shared_ptr<BabelFishService> &service_handle,
+                                             const std::shared_ptr<rmw_request_id_t> &request_header,
+                                             std::shared_ptr<CompoundMessage> request,
+                                             const MessageMembersIntrospection &response_type )
   {
     TRACEPOINT( callback_start, static_cast<const void *>( this ), false );
+    if ( std::holds_alternative<SharedPtrDeferResponseCallback>( callback_ ) ) {
+      const auto &cb = std::get<SharedPtrDeferResponseCallback>( callback_ );
+      cb( request_header, std::move( request ) );
+      TRACEPOINT( callback_end, static_cast<const void *>( this ) );
+      return nullptr;
+    }
+    if ( std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>( callback_ ) ) {
+      const auto &cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>( callback_ );
+      cb( service_handle, request_header, std::move( request ) );
+      TRACEPOINT( callback_end, static_cast<const void *>( this ) );
+      return nullptr;
+    }
+    auto response = CompoundMessage::make_shared( response_type );
     if ( std::holds_alternative<SharedPtrCallback>( callback_ ) ) {
       (void)request_header;
       const auto &cb = std::get<SharedPtrCallback>( callback_ );
       cb( std::move( request ), response );
     } 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<SharedPtrDeferResponseCallback>( callback_ ) ) {
-      const auto &cb = std::get<SharedPtrDeferResponseCallback>( callback_ );
-      cb( request_header, std::move( request ) );
-    } else if ( std::holds_alternative<SharedPtrDeferResponseCallbackWithServiceHandle>( callback_ ) ) {
-      const auto &cb = std::get<SharedPtrDeferResponseCallbackWithServiceHandle>( callback_ );
-      cb( service_handle, request_header, std::move( request ) );
     }
     TRACEPOINT( callback_end, static_cast<const void *>( this ) );
+    return response;
   }
 
   void register_callback_for_tracing()

ros_babel_fish/src/detail/babel_fish_service.cpp

@@ -82,8 +82,11 @@ void BabelFishService::handle_request( std::shared_ptr<rmw_request_id_t> request
                                        std::shared_ptr<void> request )
 {
   auto typed_request = CompoundMessage::make_shared( type_support_->request(), request );
-  auto response = CompoundMessage::make_shared( type_support_->response() );
-  callback_.dispatch( this->shared_from_this(), request_header, typed_request, response );
-  send_response( *request_header, *response );
+  // Defer variants do not fill a response and return nullptr, in which case the response is sent
+  // later by the user via send_response. Immediate variants return the filled response to send.
+  auto response = callback_.dispatch( this->shared_from_this(), request_header, typed_request,
+                                      type_support_->response() );
+  if ( response )
+    send_response( *request_header, *response );
 }
 } // namespace ros_babel_fish

ros_babel_fish/test/service.cpp

@@ -65,6 +65,46 @@ TEST( ServiceTest, server )
   EXPECT_EQ( result->sum, 512 + 314 + 1337 );
 }
 
+TEST( ServiceTest, deferredResponse )
+{
+  BabelFish fish;
+  auto type_support = fish.get_service_type_support( "example_interfaces/srv/AddTwoInts" );
+  std::shared_ptr<rmw_request_id_t> stored_header;
+  CompoundMessage::SharedPtr stored_request;
+  rclcpp::TimerBase::SharedPtr send_timer;
+  BabelFishService::SharedPtr service;
+  // Register the defer variant (header, request) which must NOT auto-send. The response is sent
+  // later, back on the spinner thread, via a one-shot wall timer so take_request and send_response
+  // stay on the same thread.
+  service = fish.create_service(
+      *node, "/test_service_server/deferred_two_ints", "example_interfaces/srv/AddTwoInts",
+      [&]( std::shared_ptr<rmw_request_id_t> header, CompoundMessage::SharedPtr request ) {
+        stored_header = std::move( header );
+        stored_request = std::move( request );
+        send_timer = node->create_wall_timer( 250ms, [&]() {
+          send_timer->cancel();
+          auto response = CompoundMessage::make_shared( type_support->response() );
+          response->set( "sum", stored_request->get<int64_t>( "a" ) +
+                                    stored_request->get<int64_t>( "b" ) + 100 );
+          service->send_response( *stored_header, *response );
+        } );
+      } );
+  auto req = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
+  req->a = 7;
+  req->b = 5;
+  auto client = node->create_client<example_interfaces::srv::AddTwoInts>(
+      "test_service_server/deferred_two_ints" );
+  ASSERT_TRUE( client->wait_for_service( 5s ) );
+  auto response_future = client->async_send_request( req );
+  // Regression guard: with the auto-send disabled for defer variants, the client must not receive
+  // an (empty) response before the deferred send actually fires.
+  EXPECT_EQ( response_future.wait_for( 100ms ), std::future_status::timeout );
+  ASSERT_EQ( response_future.wait_for( 5s ), std::future_status::ready );
+  auto result = response_future.get();
+  ASSERT_NE( result, nullptr );
+  EXPECT_EQ( result->sum, 7 + 5 + 100 );
+}
+
 int main( int argc, char **argv )
 {
   testing::InitGoogleTest( &argc, argv );