diff --git a/rmw_zenoh_cpp/src/detail/event.cpp b/rmw_zenoh_cpp/src/detail/event.cpp index 0256f3e4..f26ce356 100644 --- a/rmw_zenoh_cpp/src/detail/event.cpp +++ b/rmw_zenoh_cpp/src/detail/event.cpp @@ -252,13 +252,8 @@ void EventsManager::notify_event(rmw_zenoh_event_type_t event_id) return; } - /* Make sure to not lock both event_mutex_ and event_condition_mutex_ at the same time to avoid - * deadlocks with rmw_wait */ - rmw_wait_set_data_t * wait_set_data = nullptr; - { - std::lock_guard lock(event_condition_mutex_); - wait_set_data = wait_set_data_[event_id]; - } + std::lock_guard lock(event_condition_mutex_); + auto *wait_set_data = wait_set_data_[event_id]; if (wait_set_data != nullptr) { std::lock_guard wait_set_lock(wait_set_data->condition_mutex); wait_set_data->triggered = true; diff --git a/rmw_zenoh_cpp/src/detail/guard_condition.cpp b/rmw_zenoh_cpp/src/detail/guard_condition.cpp index 9fe2cfa0..1b6b6ba0 100644 --- a/rmw_zenoh_cpp/src/detail/guard_condition.cpp +++ b/rmw_zenoh_cpp/src/detail/guard_condition.cpp @@ -30,21 +30,17 @@ GuardCondition::GuardCondition() ///============================================================================= void GuardCondition::trigger() { - rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr; - { - std::lock_guard lock(internal_mutex_); + 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; - wait_set_data_to_trigger = wait_set_data_; - } + // 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; - 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(); + 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(); } } diff --git a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp index 1b4a3e1d..36f1f4a9 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_client_data.cpp @@ -231,34 +231,29 @@ std::array ClientData::copy_gid() const ///============================================================================= void ClientData::add_new_reply(std::unique_ptr reply) { - rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr; + 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) { - 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_; + // 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(); } - - 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(); + 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(); } } diff --git a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp index 051c96aa..04f9e648 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_service_data.cpp @@ -237,41 +237,36 @@ bool ServiceData::liveliness_is_valid() const ///============================================================================= void ServiceData::add_new_query(std::unique_ptr query) { - rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr; + 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) { - 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_; + // 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(); } - - 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(); + 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(); } } diff --git a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp index 0401f76f..4126ebc5 100644 --- a/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp +++ b/rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp @@ -1111,80 +1111,67 @@ void SubscriptionData::add_new_message( std::unique_ptr msg, const std::string & topic_name) { - rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr; - bool message_lost = false; - int32_t num_msg_lost = 0; - + std::lock_guard lock(mutex_); + if (is_shutdown_) { + return; + } + RMW_ZENOH_ROSIDL_BUFFER_LOG_DEBUG_NAMED( + "rmw_zenoh_cpp", + "[Subscription] add_new_message topic='%s' is_buffer_aware=%d payload_size=%zu", + topic_name.c_str(), + is_buffer_aware_, + msg->payload.size()); + 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) { - std::lock_guard lock(mutex_); - if (is_shutdown_) { - return; - } - RMW_ZENOH_ROSIDL_BUFFER_LOG_DEBUG_NAMED( + // Log warning if message is discarded due to hitting the queue depth + RMW_ZENOH_LOG_DEBUG_NAMED( "rmw_zenoh_cpp", - "[Subscription] add_new_message topic='%s' is_buffer_aware=%d payload_size=%zu", - topic_name.c_str(), - is_buffer_aware_, - msg->payload.size()); - 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(); - } - } + "Message queue depth of %ld reached, discarding oldest message " + "for subscription for %s", + adapted_qos_profile.depth, + topic_name.c_str()); - // 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; - } + // 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(); } - // 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_; } - // 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)); + // 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)); + } } + // 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)); - 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(); + // 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(); } } diff --git a/rmw_zenoh_cpp/src/rmw_zenoh.cpp b/rmw_zenoh_cpp/src/rmw_zenoh.cpp index 7b0ef7a7..849c4353 100644 --- a/rmw_zenoh_cpp/src/rmw_zenoh.cpp +++ b/rmw_zenoh_cpp/src/rmw_zenoh.cpp @@ -2235,13 +2235,27 @@ rmw_wait( // a valid pointer. { - // Take the lock before the check_and_attach_condition to ensure conditions and flags - // are not modified while being checked by concurrent calls. + // reset the trigger prior to attaching any entities std::unique_lock lock(wait_set_data->condition_mutex); + wait_set_data->triggered = false; + } + { + // We explicitly do not lock the condition_mutex here + // This is fine, as the attachment returns atomically is a signal was ready + // If anything triggers after that point, wait_set_data->triggered will be set + // to true under mutex. + // Note taking the mutex here leads to a deadlock. bool skip_wait = check_and_attach_condition( subscriptions, guard_conditions, services, clients, events, wait_set_data); + + if (!skip_wait) { + // now it is safe to take the lock + // if wait_set_data->triggered was set to true in between, + // the wait on the conditional will instantly return. + std::unique_lock lock(wait_set_data->condition_mutex); + // According to the RMW documentation, if wait_timeout is NULL that means // "wait forever", if it specified as 0 it means "never wait", and if it is anything else wait // for that amount of time. @@ -2258,12 +2272,6 @@ rmw_wait( [wait_set_data]() {return wait_set_data->triggered;}); } } - - // It is important to reset this here while still holding the lock, otherwise every subsequent - // call to rmw_wait() will be immediately ready. We could handle this another way by making - // "triggered" a stack variable in this function and "attaching" it during - // "check_and_attach_condition", but that isn't clearly better so leaving this. - wait_set_data->triggered = false; } } diff --git a/test_rmw_zenoh_cpp/CMakeLists.txt b/test_rmw_zenoh_cpp/CMakeLists.txt index cb4ede73..30dcbc46 100644 --- a/test_rmw_zenoh_cpp/CMakeLists.txt +++ b/test_rmw_zenoh_cpp/CMakeLists.txt @@ -16,7 +16,6 @@ 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) @@ -31,16 +30,6 @@ if(BUILD_TESTING) zenohcxx::zenohc ament_cmake_ros_core::ament_ros_defaults ) - - 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 - ament_cmake_ros_core::ament_ros_defaults - ) endif() ament_package() diff --git a/test_rmw_zenoh_cpp/package.xml b/test_rmw_zenoh_cpp/package.xml index 6965b148..e5722a38 100644 --- a/test_rmw_zenoh_cpp/package.xml +++ b/test_rmw_zenoh_cpp/package.xml @@ -15,7 +15,6 @@ 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 deleted file mode 100644 index 8f14c432..00000000 --- a/test_rmw_zenoh_cpp/test/test_issue_921.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// 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); -}