Fix deadlock between wait_set condition_mutex and entity-specific mutexes (backport #992)

rmw_zenoh_cpp/src/detail/guard_condition.cpp

@@ -30,17 +30,21 @@ GuardCondition::GuardCondition()
 ///=============================================================================
 void GuardCondition::trigger()
 {
-  std::lock_guard<std::mutex> lock(internal_mutex_);
+  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
+  {
+    std::lock_guard<std::mutex> lock(internal_mutex_);
 
-  // the change to hasTriggered_ needs to be mutually exclusive with
-  // rmw_wait() which checks hasTriggered() and decides if wait() needs to
-  // be called
-  has_triggered_ = true;
+    // the change to hasTriggered_ needs to be mutually exclusive with
+    // rmw_wait() which checks hasTriggered() and decides if wait() needs to
+    // be called
+    has_triggered_ = true;
+    wait_set_data_to_trigger = wait_set_data_;
+  }
 
-  if (wait_set_data_ != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
-    wait_set_data_->triggered = true;
-    wait_set_data_->condition_variable.notify_one();
+  if (wait_set_data_to_trigger != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
+    wait_set_data_to_trigger->triggered = true;
+    wait_set_data_to_trigger->condition_variable.notify_one();
   }
 }
 

rmw_zenoh_cpp/src/detail/rmw_client_data.cpp

@@ -229,29 +229,34 @@ std::array<uint8_t, RMW_GID_STORAGE_SIZE> ClientData::copy_gid() const
 ///=============================================================================
 void ClientData::add_new_reply(std::unique_ptr<ZenohReply> reply)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
-  const rmw_qos_profile_t adapted_qos_profile =
-    entity_->topic_info().value().qos_;
-  if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
-    reply_queue_.size() >= adapted_qos_profile.depth)
+  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
   {
-    // Log warning if message is discarded due to hitting the queue depth
-    RMW_ZENOH_LOG_ERROR_NAMED(
-      "rmw_zenoh_cpp",
-      "Query queue depth of %ld reached, discarding oldest Query "
-      "for client for %s",
-      adapted_qos_profile.depth,
-      this->entity_->topic_info().value().topic_keyexpr_.c_str());
-    reply_queue_.pop_front();
+    std::lock_guard<std::mutex> lock(mutex_);
+    const rmw_qos_profile_t adapted_qos_profile =
+      entity_->topic_info().value().qos_;
+    if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
+      reply_queue_.size() >= adapted_qos_profile.depth)
+    {
+      // Log warning if message is discarded due to hitting the queue depth
+      RMW_ZENOH_LOG_ERROR_NAMED(
+        "rmw_zenoh_cpp",
+        "Query queue depth of %ld reached, discarding oldest Query "
+        "for client for %s",
+        adapted_qos_profile.depth,
+        this->entity_->topic_info().value().topic_keyexpr_.c_str());
+      reply_queue_.pop_front();
+    }
+    reply_queue_.emplace_back(std::move(reply));
+
+    // Since we added new data, trigger user callback and guard condition if they are available
+    data_callback_mgr_.trigger_callback();
+    wait_set_data_to_trigger = wait_set_data_;
   }
-  reply_queue_.emplace_back(std::move(reply));
-
-  // Since we added new data, trigger user callback and guard condition if they are available
-  data_callback_mgr_.trigger_callback();
-  if (wait_set_data_ != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
-    wait_set_data_->triggered = true;
-    wait_set_data_->condition_variable.notify_one();
+
+  if (wait_set_data_to_trigger != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
+    wait_set_data_to_trigger->triggered = true;
+    wait_set_data_to_trigger->condition_variable.notify_one();
   }
 }
 

rmw_zenoh_cpp/src/detail/rmw_service_data.cpp

@@ -235,36 +235,41 @@ bool ServiceData::liveliness_is_valid() const
 ///=============================================================================
 void ServiceData::add_new_query(std::unique_ptr<ZenohQuery> query)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
-  if (is_shutdown_.load(std::memory_order_acquire)) {
-    RMW_ZENOH_LOG_DEBUG_NAMED(
-      "rmw_zenoh_cpp",
-      "Request from client will be ignored since the service is shutdown."
-    );
-    return;
-  }
-  const rmw_qos_profile_t adapted_qos_profile =
-    entity_->topic_info().value().qos_;
-  if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
-    query_queue_.size() >= adapted_qos_profile.depth)
+  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
   {
-    // Log warning if message is discarded due to hitting the queue depth
-    RMW_ZENOH_LOG_ERROR_NAMED(
-      "rmw_zenoh_cpp",
-      "Query queue depth of %ld reached, discarding oldest Query "
-      "for service '%s'",
-      adapted_qos_profile.depth,
-      entity_->topic_info().value().name_.c_str());
-    query_queue_.pop_front();
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (is_shutdown_.load(std::memory_order_acquire)) {
+      RMW_ZENOH_LOG_DEBUG_NAMED(
+        "rmw_zenoh_cpp",
+        "Request from client will be ignored since the service is shutdown."
+      );
+      return;
+    }
+    const rmw_qos_profile_t adapted_qos_profile =
+      entity_->topic_info().value().qos_;
+    if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
+      query_queue_.size() >= adapted_qos_profile.depth)
+    {
+      // Log warning if message is discarded due to hitting the queue depth
+      RMW_ZENOH_LOG_ERROR_NAMED(
+        "rmw_zenoh_cpp",
+        "Query queue depth of %ld reached, discarding oldest Query "
+        "for service '%s'",
+        adapted_qos_profile.depth,
+        entity_->topic_info().value().name_.c_str());
+      query_queue_.pop_front();
+    }
+    query_queue_.emplace_back(std::move(query));
+
+    // Since we added new data, trigger user callback and guard condition if they are available
+    data_callback_mgr_.trigger_callback();
+    wait_set_data_to_trigger = wait_set_data_;
   }
-  query_queue_.emplace_back(std::move(query));
-
-  // Since we added new data, trigger user callback and guard condition if they are available
-  data_callback_mgr_.trigger_callback();
-  if (wait_set_data_ != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
-    wait_set_data_->triggered = true;
-    wait_set_data_->condition_variable.notify_one();
+
+  if (wait_set_data_to_trigger != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
+    wait_set_data_to_trigger->triggered = true;
+    wait_set_data_to_trigger->condition_variable.notify_one();
   }
 }
 

rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp

@@ -497,61 +497,74 @@ rmw_ret_t SubscriptionData::take_serialized_message(
 void SubscriptionData::add_new_message(
   std::unique_ptr<SubscriptionData::Message> msg, const std::string & topic_name)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
-  if (is_shutdown_) {
-    return;
-  }
-  const rmw_qos_profile_t adapted_qos_profile = entity_->topic_info().value().qos_;
-  if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
-    message_queue_.size() >= adapted_qos_profile.depth)
-  {
-    // Log warning if message is discarded due to hitting the queue depth
-    RMW_ZENOH_LOG_DEBUG_NAMED(
-      "rmw_zenoh_cpp",
-      "Message queue depth of %ld reached, discarding oldest message "
-      "for subscription for %s",
-      adapted_qos_profile.depth,
-      topic_name.c_str());
+  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
+  bool message_lost = false;
+  int32_t num_msg_lost = 0;
 
-    // If the adapted_qos_profile.depth is 0, the std::move command below will result
-    // in UB and the z_drop will segfault. We explicitly set the depth to a minimum of 1
-    // in rmw_create_subscription() but to be safe, we only attempt to discard from the
-    // queue if it is non-empty.
-    if (!message_queue_.empty()) {
-      std::unique_ptr<Message> old = std::move(message_queue_.front());
-      message_queue_.pop_front();
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (is_shutdown_) {
+      return;
+    }
+    const rmw_qos_profile_t adapted_qos_profile = entity_->topic_info().value().qos_;
+    if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
+      message_queue_.size() >= adapted_qos_profile.depth)
+    {
+      // Log warning if message is discarded due to hitting the queue depth
+      RMW_ZENOH_LOG_DEBUG_NAMED(
+        "rmw_zenoh_cpp",
+        "Message queue depth of %ld reached, discarding oldest message "
+        "for subscription for %s",
+        adapted_qos_profile.depth,
+        topic_name.c_str());
+
+      // If the adapted_qos_profile.depth is 0, the std::move command below will result
+      // in UB and the z_drop will segfault. We explicitly set the depth to a minimum of 1
+      // in rmw_create_subscription() but to be safe, we only attempt to discard from the
+      // queue if it is non-empty.
+      if (!message_queue_.empty()) {
+        std::unique_ptr<Message> old = std::move(message_queue_.front());
+        message_queue_.pop_front();
+      }
     }
-  }
 
-  // Check for messages lost if the new sequence number is not monotonically increasing.
-  const size_t gid_hash = hash_gid(msg->attachment.copy_gid());
-  auto last_known_pub_it = last_known_published_msg_.find(gid_hash);
-  if (last_known_pub_it != last_known_published_msg_.end()) {
-    const int64_t seq_increment = std::abs(
-      msg->attachment.sequence_number() -
-      last_known_pub_it->second);
-    if (seq_increment > 1) {
-      int32_t num_msg_lost =
-        static_cast<int32_t>(std::clamp(
-          seq_increment - 1,
-          static_cast<int64_t>(std::numeric_limits<int32_t>::min()),
-          static_cast<int64_t>(std::numeric_limits<int32_t>::max())));
-      events_mgr_->update_event_status(
-        ZENOH_EVENT_MESSAGE_LOST,
-        std::move(num_msg_lost));
+    // Check for messages lost if the new sequence number is not monotonically increasing.
+    const size_t gid_hash = hash_gid(msg->attachment.copy_gid());
+    auto last_known_pub_it = last_known_published_msg_.find(gid_hash);
+    if (last_known_pub_it != last_known_published_msg_.end()) {
+      const int64_t seq_increment = std::abs(
+        msg->attachment.sequence_number() -
+        last_known_pub_it->second);
+      if (seq_increment > 1) {
+        num_msg_lost =
+          static_cast<int32_t>(std::clamp(
+            seq_increment - 1,
+            static_cast<int64_t>(std::numeric_limits<int32_t>::min()),
+            static_cast<int64_t>(std::numeric_limits<int32_t>::max())));
+        message_lost = true;
+      }
     }
+    // Always update the last known sequence number for the publisher.
+    last_known_published_msg_[gid_hash] = msg->attachment.sequence_number();
+
+    message_queue_.emplace_back(std::move(msg));
+
+    // Since we added new data, trigger user callback and guard condition if they are available
+    data_callback_mgr_.trigger_callback();
+    wait_set_data_to_trigger = wait_set_data_;
   }
-  // Always update the last known sequence number for the publisher.
-  last_known_published_msg_[gid_hash] = msg->attachment.sequence_number();
 
-  message_queue_.emplace_back(std::move(msg));
+  // Trigger lost message event outside the subscription mutex to avoid deadlocks.
+  if (message_lost) {
+    events_mgr_->update_event_status(
+      ZENOH_EVENT_MESSAGE_LOST,
+      std::move(num_msg_lost));
+  }
 
-  // Since we added new data, trigger user callback and guard condition if they are available
-  data_callback_mgr_.trigger_callback();
-  if (wait_set_data_ != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
-    wait_set_data_->triggered = true;
-    wait_set_data_->condition_variable.notify_one();
+  if (wait_set_data_to_trigger != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
+    wait_set_data_to_trigger->triggered = true;
+    wait_set_data_to_trigger->condition_variable.notify_one();
   }
 }
 

test_rmw_zenoh_cpp/CMakeLists.txt

@@ -19,6 +19,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_lint_common REQUIRED)
   find_package(rclcpp REQUIRED)
+  find_package(std_msgs REQUIRED)
   find_package(rmw_zenoh_cpp REQUIRED)
   find_package(zenoh_cpp_vendor REQUIRED)
 
@@ -32,6 +33,15 @@ if(BUILD_TESTING)
     rmw_zenoh_cpp::rmw_zenoh_cpp
     zenohcxx::zenohc
   )
+
+  ament_add_ros_isolated_gtest(test_issue_921
+    test/test_issue_921.cpp
+    ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp)
+  target_link_libraries(test_issue_921
+    rclcpp::rclcpp
+    rmw_zenoh_cpp::rmw_zenoh_cpp
+    ${std_msgs_TARGETS}
+  )
 endif()
 
 ament_package()

test_rmw_zenoh_cpp/package.xml

@@ -14,6 +14,7 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rclcpp</test_depend>
+  <test_depend>std_msgs</test_depend>
   <test_depend>rmw_zenoh_cpp</test_depend>
   <test_depend>zenoh_cpp_vendor</test_depend>