avoid using dds common public mutex directly (#725)

* call method to update graph instead of using mutex directly

* initialize publish_callback once

* use make_scope_exit to make it destroyed all error paths

Signed-off-by: Chen Lihui <[email protected]>

rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp

@@ -159,6 +159,13 @@ init_context_impl(
   common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
     eprosima_fastrtps_identifier, participant_info->participant_->guid());
   common_context->pub = publisher.get();
+  common_context->publish_callback = [](const rmw_publisher_t * pub, const void * msg) {
+      return rmw_fastrtps_shared_cpp::__rmw_publish(
+        eprosima_fastrtps_identifier,
+        pub,
+        msg,
+        nullptr);
+    };
   common_context->sub = subscription.get();
   common_context->graph_guard_condition = graph_guard_condition.get();
 

rmw_fastrtps_cpp/src/rmw_client.cpp

@@ -442,43 +442,16 @@ rmw_create_client(
   }
   memcpy(const_cast<char *>(rmw_client->service_name), service_name, strlen(service_name) + 1);
 
+  // Update graph
+  rmw_gid_t request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+    eprosima_fastrtps_identifier, info->request_writer_->guid());
+  rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+    eprosima_fastrtps_identifier, info->response_reader_->guid());
+  if (RMW_RET_OK != common_context->add_client_graph(
+      request_publisher_gid, response_subscriber_gid,
+      node->name, node->namespace_))
   {
-    // Update graph
-    std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
-    rmw_gid_t request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
-      eprosima_fastrtps_identifier, info->request_writer_->guid());
-    common_context->graph_cache.associate_writer(
-      request_publisher_gid,
-      common_context->gid,
-      node->name,
-      node->namespace_);
-
-    rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
-      eprosima_fastrtps_identifier, info->response_reader_->guid());
-    rmw_dds_common::msg::ParticipantEntitiesInfo msg =
-      common_context->graph_cache.associate_reader(
-      response_subscriber_gid,
-      common_context->gid,
-      node->name,
-      node->namespace_);
-    rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_publish(
-      eprosima_fastrtps_identifier,
-      common_context->pub,
-      static_cast<void *>(&msg),
-      nullptr);
-    if (RMW_RET_OK != ret) {
-      common_context->graph_cache.dissociate_reader(
-        response_subscriber_gid,
-        common_context->gid,
-        node->name,
-        node->namespace_);
-      common_context->graph_cache.dissociate_writer(
-        request_publisher_gid,
-        common_context->gid,
-        node->name,
-        node->namespace_);
-      return nullptr;
-    }
+    return nullptr;
   }
 
   cleanup_rmw_client.cancel();

rmw_fastrtps_cpp/src/rmw_publisher.cpp

@@ -20,6 +20,8 @@
 #include "rmw/get_topic_endpoint_info.h"
 #include "rmw/rmw.h"
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "rmw/impl/cpp/macros.hpp"
 
 #include "rmw_dds_common/qos.hpp"
@@ -99,37 +101,32 @@ rmw_create_publisher(
   if (!publisher) {
     return nullptr;
   }
-
-  auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
-
-  auto info = static_cast<const CustomPublisherInfo *>(publisher->data);
-  {
-    // Update graph
-    std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
-    rmw_dds_common::msg::ParticipantEntitiesInfo msg =
-      common_context->graph_cache.associate_writer(
-      info->publisher_gid, common_context->gid, node->name, node->namespace_);
-    rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish(
-      eprosima_fastrtps_identifier,
-      common_context->pub,
-      static_cast<void *>(&msg),
-      nullptr);
-    if (RMW_RET_OK != rmw_ret) {
+  auto cleanup_publisher = rcpputils::make_scope_exit(
+    [participant_info, publisher]() {
       rmw_error_state_t error_state = *rmw_get_error_state();
       rmw_reset_error();
-      static_cast<void>(common_context->graph_cache.dissociate_writer(
-        info->publisher_gid, common_context->gid, node->name, node->namespace_));
-      rmw_ret = rmw_fastrtps_shared_cpp::destroy_publisher(
-        eprosima_fastrtps_identifier, participant_info, publisher);
-      if (RMW_RET_OK != rmw_ret) {
+      if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher(
+        eprosima_fastrtps_identifier, participant_info, publisher))
+      {
         RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str);
         RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n");
         rmw_reset_error();
       }
       rmw_set_error_state(error_state.message, error_state.file, error_state.line_number);
-      return nullptr;
-    }
+    });
+
+  auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
+  auto info = static_cast<const CustomPublisherInfo *>(publisher->data);
+
+  // Update graph
+  if (RMW_RET_OK != common_context->add_publisher_graph(
+      info->publisher_gid,
+      node->name, node->namespace_))
+  {
+    return nullptr;
   }
+
+  cleanup_publisher.cancel();
   return publisher;
 }
 

rmw_fastrtps_cpp/src/rmw_service.cpp

@@ -442,42 +442,16 @@ rmw_create_service(
   }
   memcpy(const_cast<char *>(rmw_service->service_name), service_name, strlen(service_name) + 1);
 
+  // Update graph
+  rmw_gid_t request_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+    eprosima_fastrtps_identifier, info->request_reader_->guid());
+  rmw_gid_t response_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+    eprosima_fastrtps_identifier, info->response_writer_->guid());
+  if (RMW_RET_OK != common_context->add_service_graph(
+      request_subscriber_gid, response_publisher_gid,
+      node->name, node->namespace_))
   {
-    // Update graph
-    std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
-    rmw_gid_t request_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
-      eprosima_fastrtps_identifier, info->request_reader_->guid());
-    common_context->graph_cache.associate_reader(
-      request_subscriber_gid,
-      common_context->gid,
-      node->name,
-      node->namespace_);
-    rmw_gid_t response_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
-      eprosima_fastrtps_identifier, info->response_writer_->guid());
-    rmw_dds_common::msg::ParticipantEntitiesInfo msg =
-      common_context->graph_cache.associate_writer(
-      response_publisher_gid,
-      common_context->gid,
-      node->name,
-      node->namespace_);
-    rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_publish(
-      eprosima_fastrtps_identifier,
-      common_context->pub,
-      static_cast<void *>(&msg),
-      nullptr);
-    if (RMW_RET_OK != ret) {
-      common_context->graph_cache.dissociate_writer(
-        response_publisher_gid,
-        common_context->gid,
-        node->name,
-        node->namespace_);
-      common_context->graph_cache.dissociate_reader(
-        request_subscriber_gid,
-        common_context->gid,
-        node->name,
-        node->namespace_);
-      return nullptr;
-    }
+    return nullptr;
   }
 
   cleanup_rmw_service.cancel();

rmw_fastrtps_cpp/src/rmw_subscription.cpp

@@ -21,6 +21,8 @@
 #include "rmw/get_topic_endpoint_info.h"
 #include "rmw/rmw.h"
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "rmw_dds_common/qos.hpp"
 
 #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp"
@@ -93,40 +95,35 @@ rmw_create_subscription(
   if (!subscription) {
     return nullptr;
   }
-
-  auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
-  auto info = static_cast<CustomSubscriberInfo *>(subscription->data);
-
-  {
-    // Update graph
-    std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
-    rmw_dds_common::msg::ParticipantEntitiesInfo msg =
-      common_context->graph_cache.associate_reader(
-      info->subscription_gid_, common_context->gid, node->name, node->namespace_);
-    rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish(
-      eprosima_fastrtps_identifier,
-      common_context->pub,
-      static_cast<void *>(&msg),
-      nullptr);
-    if (RMW_RET_OK != rmw_ret) {
+  auto cleanup_subscription = rcpputils::make_scope_exit(
+    [participant_info, subscription]() {
       rmw_error_state_t error_state = *rmw_get_error_state();
       rmw_reset_error();
-      static_cast<void>(common_context->graph_cache.dissociate_reader(
-        info->subscription_gid_, common_context->gid, node->name, node->namespace_));
-      rmw_ret = rmw_fastrtps_shared_cpp::destroy_subscription(
-        eprosima_fastrtps_identifier, participant_info, subscription);
-      if (RMW_RET_OK != rmw_ret) {
+      if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription(
+        eprosima_fastrtps_identifier, participant_info, subscription))
+      {
         RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str);
         RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n");
         rmw_reset_error();
       }
       rmw_set_error_state(error_state.message, error_state.file, error_state.line_number);
-      return nullptr;
-    }
+    });
+
+  auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
+  auto info = static_cast<CustomSubscriberInfo *>(subscription->data);
+
+  // Update graph
+  if (RMW_RET_OK != common_context->add_subscriber_graph(
+      info->subscription_gid_,
+      node->name, node->namespace_))
+  {
+    return nullptr;
   }
+
   info->node_ = node;
   info->common_context_ = common_context;
 
+  cleanup_subscription.cancel();
   return subscription;
 }
 

rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp

@@ -159,6 +159,13 @@ init_context_impl(
   common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
     eprosima_fastrtps_identifier, participant_info->participant_->guid());
   common_context->pub = publisher.get();
+  common_context->publish_callback = [](const rmw_publisher_t * pub, const void * msg) {
+      return rmw_fastrtps_shared_cpp::__rmw_publish(
+        eprosima_fastrtps_identifier,
+        pub,
+        msg,
+        nullptr);
+    };
   common_context->sub = subscription.get();
   common_context->graph_guard_condition = graph_guard_condition.get();
 

rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp

@@ -471,40 +471,16 @@ rmw_create_client(
   }
   memcpy(const_cast<char *>(rmw_client->service_name), service_name, strlen(service_name) + 1);
 
+  // Update graph
+  rmw_gid_t request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+    eprosima_fastrtps_identifier, info->request_writer_->guid());
+  rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
+    eprosima_fastrtps_identifier, info->response_reader_->guid());
+  if (RMW_RET_OK != common_context->add_client_graph(
+      request_publisher_gid, response_subscriber_gid,
+      node->name, node->namespace_))
   {
-    // Update graph
-    std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
-    rmw_gid_t request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
-      eprosima_fastrtps_identifier, info->request_writer_->guid());
-    common_context->graph_cache.associate_writer(
-      request_publisher_gid,
-      common_context->gid,
-      node->name,
-      node->namespace_);
-
-    rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid(
-      eprosima_fastrtps_identifier, info->response_reader_->guid());
-    rmw_dds_common::msg::ParticipantEntitiesInfo msg =
-      common_context->graph_cache.associate_reader(
-      response_subscriber_gid, common_context->gid, node->name, node->namespace_);
-    rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish(
-      eprosima_fastrtps_identifier,
-      common_context->pub,
-      static_cast<void *>(&msg),
-      nullptr);
-    if (RMW_RET_OK != rmw_ret) {
-      common_context->graph_cache.dissociate_reader(
-        response_subscriber_gid,
-        common_context->gid,
-        node->name,
-        node->namespace_);
-      common_context->graph_cache.dissociate_writer(
-        request_publisher_gid,
-        common_context->gid,
-        node->name,
-        node->namespace_);
-      return nullptr;
-    }
+    return nullptr;
   }
 
   cleanup_rmw_client.cancel();

rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp

@@ -19,6 +19,8 @@
 #include "rmw/get_topic_endpoint_info.h"
 #include "rmw/rmw.h"
 
+#include "rcpputils/scope_exit.hpp"
+
 #include "rmw/impl/cpp/macros.hpp"
 
 #include "rmw_dds_common/qos.hpp"
@@ -99,37 +101,32 @@ rmw_create_publisher(
   if (!publisher) {
     return nullptr;
   }
-
-  auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
-
-  auto info = static_cast<const CustomPublisherInfo *>(publisher->data);
-  {
-    // Update graph
-    std::lock_guard<std::mutex> guard(common_context->node_update_mutex);
-    rmw_dds_common::msg::ParticipantEntitiesInfo msg =
-      common_context->graph_cache.associate_writer(
-      info->publisher_gid, common_context->gid, node->name, node->namespace_);
-    rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish(
-      eprosima_fastrtps_identifier,
-      common_context->pub,
-      static_cast<void *>(&msg),
-      nullptr);
-    if (RMW_RET_OK != rmw_ret) {
+  auto cleanup_publisher = rcpputils::make_scope_exit(
+    [participant_info, publisher]() {
       rmw_error_state_t error_state = *rmw_get_error_state();
       rmw_reset_error();
-      static_cast<void>(common_context->graph_cache.dissociate_writer(
-        info->publisher_gid, common_context->gid, node->name, node->namespace_));
-      rmw_ret = rmw_fastrtps_shared_cpp::destroy_publisher(
-        eprosima_fastrtps_identifier, participant_info, publisher);
-      if (RMW_RET_OK != rmw_ret) {
+      if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher(
+        eprosima_fastrtps_identifier, participant_info, publisher))
+      {
         RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str);
         RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n");
         rmw_reset_error();
       }
       rmw_set_error_state(error_state.message, error_state.file, error_state.line_number);
-      return nullptr;
-    }
+    });
+
+  auto common_context = static_cast<rmw_dds_common::Context *>(node->context->impl->common);
+  auto info = static_cast<const CustomPublisherInfo *>(publisher->data);
+
+  // Update graph
+  if (RMW_RET_OK != common_context->add_publisher_graph(
+      info->publisher_gid,
+      node->name, node->namespace_))
+  {
+    return nullptr;
   }
+
+  cleanup_publisher.cancel();
   return publisher;
 }