Sending response subscriber guid with request.

Signed-off-by: Miguel Company <[email protected]>
ros2 · May 24, 2020 · fce9022 · fce9022
1 parent 2a3c63b
commit fce9022
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 2 deletions.
diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/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) {

diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/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) {

diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/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) {

diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp
@@ -84,6 +84,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_);
 

diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/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 |