Make service wait for response reader

rmw_fastrtps_cpp/src/rmw_client.cpp

@@ -192,6 +192,7 @@ rmw_create_client(
   }
 
   info->writer_guid_ = info->request_publisher_->getGuid();
+  info->reader_guid_ = info->response_subscriber_->getGuid();
 
   rmw_client = rmw_client_allocate();
   if (!rmw_client) {

rmw_fastrtps_cpp/src/rmw_service.cpp

@@ -192,8 +192,9 @@ rmw_create_service(
     RMW_SET_ERROR_MSG("failed to get datawriter qos");
     goto fail;
   }
+  info->pub_listener_ = new ServicePubListener();
   info->response_publisher_ =
-    Domain::createPublisher(participant, publisherParam, nullptr);
+    Domain::createPublisher(participant, publisherParam, info->pub_listener_);
   if (!info->response_publisher_) {
     RMW_SET_ERROR_MSG("create_service() could not create publisher");
     goto fail;
@@ -255,6 +256,10 @@ rmw_create_service(
       Domain::removePublisher(info->response_publisher_);
     }
 
+    if (info->pub_listener_) {
+      delete info->pub_listener_;
+    }
+
     if (info->request_subscriber_) {
       rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
         eprosima_fastrtps_identifier, info->request_subscriber_->getGuid());

rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp

@@ -217,11 +217,12 @@ rmw_create_client(
   info->request_publisher_ =
     Domain::createPublisher(participant, publisherParam, info->pub_listener_);
   if (!info->request_publisher_) {
-    RMW_SET_ERROR_MSG("create_publisher() could not create publisher");
+    RMW_SET_ERROR_MSG("create_client() could not create publisher");
     goto fail;
   }
 
   info->writer_guid_ = info->request_publisher_->getGuid();
+  info->reader_guid_ = info->response_subscriber_->getGuid();
 
   rmw_client = rmw_client_allocate();
   if (!rmw_client) {

rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp

@@ -222,8 +222,9 @@ rmw_create_service(
     RMW_SET_ERROR_MSG("failed to get datawriter qos");
     goto fail;
   }
+  info->pub_listener_ = new ServicePubListener();
   info->response_publisher_ =
-    Domain::createPublisher(participant, publisherParam, nullptr);
+    Domain::createPublisher(participant, publisherParam, info->pub_listener_);
   if (!info->response_publisher_) {
     RMW_SET_ERROR_MSG("create_publisher() could not create publisher");
     goto fail;
@@ -285,6 +286,10 @@ rmw_create_service(
       Domain::removePublisher(info->response_publisher_);
     }
 
+    if (info->pub_listener_) {
+      delete info->pub_listener_;
+    }
+
     if (info->request_subscriber_) {
       rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
         eprosima_fastrtps_identifier, info->request_subscriber_->getGuid());

rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp

@@ -49,6 +49,7 @@ typedef struct CustomClientInfo
   eprosima::fastrtps::Publisher * request_publisher_;
   ClientListener * listener_;
   eprosima::fastrtps::rtps::GUID_t writer_guid_;
+  eprosima::fastrtps::rtps::GUID_t reader_guid_;
   eprosima::fastrtps::Participant * participant_;
   const char * typesupport_identifier_;
   ClientPubListener * pub_listener_;
@@ -88,7 +89,8 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener
       if (eprosima::fastrtps::rtps::ALIVE == response.sample_info_.sampleKind) {
         response.sample_identity_ = response.sample_info_.related_sample_identity;
 
-        if (response.sample_identity_.writer_guid() == info_->writer_guid_) {
+        if (response.sample_identity_.writer_guid() == info_->reader_guid_ ||
+            response.sample_identity_.writer_guid() == info_->writer_guid_) {
           std::lock_guard<std::mutex> lock(internalMutex_);
 
           if (conditionMutex_ != nullptr) {

rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp

@@ -34,6 +34,7 @@
 #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp"
 
 class ServiceListener;
+class ServicePubListener;
 
 typedef struct CustomServiceInfo
 {
@@ -44,6 +45,7 @@ typedef struct CustomServiceInfo
   eprosima::fastrtps::Subscriber * request_subscriber_;
   eprosima::fastrtps::Publisher * response_publisher_;
   ServiceListener * listener_;
+  ServicePubListener * pub_listener_;
   eprosima::fastrtps::Participant * participant_;
   const char * typesupport_identifier_;
 } CustomServiceInfo;
@@ -84,6 +86,12 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener
     if (sub->takeNextData(&data, &request.sample_info_)) {
       if (eprosima::fastrtps::rtps::ALIVE == request.sample_info_.sampleKind) {
         request.sample_identity_ = request.sample_info_.sample_identity;
+        // Use response subscriber guid (on related_sample_identity) when present.
+        const eprosima::fastrtps::rtps::GUID_t& reader_guid =
+          request.sample_info_.related_sample_identity.writer_guid();
+        if (reader_guid != eprosima::fastrtps::rtps::GUID_t::unknown() ) {
+          request.sample_identity_.writer_guid() = reader_guid;
+        }
 
         std::lock_guard<std::mutex> lock(internalMutex_);
 
@@ -159,4 +167,45 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener
   std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_);
 };
 
+class ServicePubListener : public eprosima::fastrtps::PublisherListener
+{
+public:
+  explicit ServicePubListener() = default;
+
+  template< class Rep, class Period >
+  bool wait_for_subscription(
+    const eprosima::fastrtps::rtps::GUID_t & guid,
+    const std::chrono::duration<Rep, Period> & rel_time)
+  {
+    auto guid_is_present = [this, guid]() -> bool
+    {
+      return subscriptions_.find(guid) != subscriptions_.end();
+    };
+
+    std::unique_lock<std::mutex> lock(mutex_);
+    return cv_.wait_for(lock, rel_time, guid_is_present);
+  }
+
+  void onPublicationMatched(
+    eprosima::fastrtps::Publisher * pub,
+    eprosima::fastrtps::rtps::MatchingInfo & matchingInfo)
+  {
+    (void) pub;
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (eprosima::fastrtps::rtps::MATCHED_MATCHING == matchingInfo.status) {
+      subscriptions_.insert(matchingInfo.remoteEndpointGuid);
+    } else if (eprosima::fastrtps::rtps::REMOVED_MATCHING == matchingInfo.status) {
+      subscriptions_.erase(matchingInfo.remoteEndpointGuid);
+    } else {
+      return;
+    }
+    cv_.notify_all();
+  }
+
+private:
+  std::mutex mutex_;
+  std::set<eprosima::fastrtps::rtps::GUID_t> subscriptions_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
+  std::condition_variable cv_;
+};
+
 #endif  // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_

rmw_fastrtps_shared_cpp/src/rmw_request.cpp

@@ -57,6 +57,7 @@ __rmw_send_request(
   data.is_cdr_buffer = false;
   data.data = const_cast<void *>(ros_request);
   data.impl = info->request_type_support_impl_;
+  wparams.related_sample_identity().writer_guid() = info->reader_guid_;
   if (info->request_publisher_->write(&data, wparams)) {
     returnedValue = RMW_RET_OK;
     *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 |

rmw_fastrtps_shared_cpp/src/rmw_response.cpp

@@ -104,6 +104,22 @@ __rmw_send_response(
   wparams.related_sample_identity().sequence_number().low =
     (int32_t)(request_header->sequence_number & 0xFFFFFFFF);
 
+  // According to the list of possible entity kinds in section 9.3.1.2 of RTPS
+  // readers will have this bit on, while writers will not. We use this to know
+  // if the related guid is the request writer or the response reader.
+  constexpr uint8_t entity_id_is_reader_bit = 0x04;
+  const eprosima::fastrtps::rtps::GUID_t & related_guid =
+    wparams.related_sample_identity().writer_guid();
+  if((related_guid.entityId.value[3] & entity_id_is_reader_bit) != 0)
+  {
+    // Related guid is a reader, so it is the response subscription guid.
+    // Wait for the response writer to be matched with it.
+    if(!info->pub_listener_->wait_for_subscription(related_guid, std::chrono::milliseconds(100))) {
+      RMW_SET_ERROR_MSG("client will not receive response");
+      return RMW_RET_ERROR;
+    }
+  }
+
   rmw_fastrtps_shared_cpp::SerializedData data;
   data.is_cdr_buffer = false;
   data.data = const_cast<void *>(ros_response);

rmw_fastrtps_shared_cpp/src/rmw_service.cpp

@@ -97,6 +97,9 @@ __rmw_destroy_service(
     if (info->response_publisher_ != nullptr) {
       Domain::removePublisher(info->response_publisher_);
     }
+    if (info->pub_listener_ != nullptr) {
+      delete info->pub_listener_;
+    }
     if (info->listener_ != nullptr) {
       delete info->listener_;
     }