From d13d477025c8dea1a6e0517bd720caf129e8e0f8 Mon Sep 17 00:00:00 2001 From: yadunund Date: Thu, 18 Jun 2026 09:43:41 -0700 Subject: [PATCH 1/2] Fix deadlock between wait_set condition_mutex and entity-specific mutexes (#992) Signed-off-by: Yadunund --- rmw_zenoh_cpp/src/detail/guard_condition.cpp | 22 ++-- rmw_zenoh_cpp/src/detail/rmw_client_data.cpp | 47 ++++---- rmw_zenoh_cpp/src/detail/rmw_service_data.cpp | 61 +++++----- .../src/detail/rmw_subscription_data.cpp | 109 ++++++++++-------- test_rmw_zenoh_cpp/CMakeLists.txt | 10 ++ test_rmw_zenoh_cpp/package.xml | 1 + test_rmw_zenoh_cpp/test/test_issue_921.cpp | 97 ++++++++++++++++ 7 files changed, 241 insertions(+), 106 deletions(-) create mode 100644 test_rmw_zenoh_cpp/test/test_issue_921.cpp diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.cpp b/rmw_zenoh_cpp/src/detail/guard_condition.cpp index 1b6b6ba08..9fe2cfa06 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.cpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.cpp @@ -30,17 +30,21 @@ GuardCondition::GuardCondition() ///============================================================================= void GuardCondition::trigger() { - std::lock_guard lock(internal_mutex_); + rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr; + { + std::lock_guard 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 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 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(); } } diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp index d4dc735ae..447f31a1a 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp @@ -229,29 +229,34 @@ std::array ClientData::copy_gid() const ///============================================================================= void ClientData::add_new_reply(std::unique_ptr reply) { - std::lock_guard 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 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 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 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(); } } diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp index bbe02e837..b16dad260 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp +++ b/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 query) { - std::lock_guard 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 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 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 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(); } } diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp index db7a1bfa4..fe5d04dad 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp +++ b/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 msg, const std::string & topic_name) { - std::lock_guard 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 old = std::move(message_queue_.front()); - message_queue_.pop_front(); + { + std::lock_guard 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 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(std::clamp( - seq_increment - 1, - static_cast(std::numeric_limits::min()), - static_cast(std::numeric_limits::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(std::clamp( + seq_increment - 1, + static_cast(std::numeric_limits::min()), + static_cast(std::numeric_limits::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 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 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(); } } diff --git a/test_rmw_zenoh_cpp/CMakeLists.txt b/test_rmw_zenoh_cpp/CMakeLists.txt index fe8e56991..937397f51 100644 --- a/test_rmw_zenoh_cpp/CMakeLists.txt +++ b/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 + std_msgs::std_msgs + rmw_zenoh_cpp::rmw_zenoh_cpp + ) endif() ament_package() diff --git a/test_rmw_zenoh_cpp/package.xml b/test_rmw_zenoh_cpp/package.xml index d426022ff..627ba7b0a 100644 --- a/test_rmw_zenoh_cpp/package.xml +++ b/test_rmw_zenoh_cpp/package.xml @@ -14,6 +14,7 @@ ament_lint_auto ament_lint_common rclcpp + std_msgs rmw_zenoh_cpp zenoh_cpp_vendor diff --git a/test_rmw_zenoh_cpp/test/test_issue_921.cpp b/test_rmw_zenoh_cpp/test/test_issue_921.cpp new file mode 100644 index 000000000..8f14c432c --- /dev/null +++ b/test_rmw_zenoh_cpp/test/test_issue_921.cpp @@ -0,0 +1,97 @@ +// Copyright 2026 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +// This test references issue ticket: https://github.com/ros2/rmw_zenoh/issues/921 +// It simulates heavy traffic by publishing messages concurrently with wait execution. +class TestIssue921 : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestIssue921, TestDeadlockUnderHeavyTraffic) +{ + auto node = std::make_shared("test_issue_921_node"); + auto publisher = node->create_publisher("test_issue_921_topic", 10); + + std::atomic received_count{0}; + auto subscription = node->create_subscription( + "test_issue_921_topic", 10, + [&received_count](std_msgs::msg::String::ConstSharedPtr) { + received_count.fetch_add(1); + }); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + std::atomic running{true}; + std::promise test_finished_promise; + auto test_finished_future = test_finished_promise.get_future(); + + // Run the test in a thread so we can monitor for deadlocks with a watchdog + std::thread test_thread([&]() { + // Publisher thread that publishes messages as fast as possible + std::thread pub_thread([&]() { + std_msgs::msg::String msg; + msg.data = "hello"; + for (size_t i = 0; i < 500 && running; ++i) { + publisher->publish(msg); + std::this_thread::sleep_for(std::chrono::microseconds(500)); + } + running = false; + }); + + // Executor spin loop + while (running) { + executor.spin_some(std::chrono::milliseconds(5)); + } + + pub_thread.join(); + test_finished_promise.set_value(); + }); + + // Watchdog timeout (5 seconds) + if (test_finished_future.wait_for(std::chrono::seconds(5)) == std::future_status::timeout) { + running = false; + // We timed out! Fails the test due to deadlock + FAIL() << "Test timed out! Possible deadlock in rmw_zenoh_cpp (Issue #921)."; + // We terminate to avoid hanging the process if threads are locked + std::terminate(); + } else { + test_thread.join(); + } + + // Ensure we actually received some messages + EXPECT_GT(received_count.load(), 0u); +} From 5138a4d145fed3dc0eb58f25d456132020d7a8ab Mon Sep 17 00:00:00 2001 From: Yadunund Date: Mon, 22 Jun 2026 22:44:50 +0000 Subject: [PATCH 2/2] Update link target for std_msgs Signed-off-by: Yadunund --- test_rmw_zenoh_cpp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test_rmw_zenoh_cpp/CMakeLists.txt b/test_rmw_zenoh_cpp/CMakeLists.txt index 937397f51..b28454b8f 100644 --- a/test_rmw_zenoh_cpp/CMakeLists.txt +++ b/test_rmw_zenoh_cpp/CMakeLists.txt @@ -39,8 +39,8 @@ if(BUILD_TESTING) ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp) target_link_libraries(test_issue_921 rclcpp::rclcpp - std_msgs::std_msgs rmw_zenoh_cpp::rmw_zenoh_cpp + ${std_msgs_TARGETS} ) endif()