diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 473e3b4d02..5d925a894b 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -225,16 +225,17 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // It will throw if the subscription is not in the wait set. if (mask.include_subscription) { auto local_subscription = inner_subscription; - local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), false); this->storage_remove_subscription(std::move(local_subscription)); + inner_subscription->exchange_in_use_by_wait_set_state(inner_subscription.get(), false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } if (mask.include_events) { for (auto key_event_pair : inner_subscription->get_event_handlers()) { auto event = key_event_pair.second; auto local_subscription = inner_subscription; - local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); this->storage_remove_waitable(std::move(event)); + local_subscription->exchange_in_use_by_wait_set_state( + key_event_pair.second.get(), false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } @@ -242,8 +243,9 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli auto local_waitable = inner_subscription->get_intra_process_waitable(); if (nullptr != local_waitable) { // This is the case when intra process is enabled for the subscription. - inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); + auto waitable = local_waitable; this->storage_remove_waitable(std::move(local_waitable)); + inner_subscription->exchange_in_use_by_wait_set_state(waitable.get(), false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} } } @@ -328,11 +330,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_guard_condition( std::move(guard_condition), [this](std::shared_ptr && inner_guard_condition) { - inner_guard_condition->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the guard condition is not in the wait set. - this->storage_remove_guard_condition(std::move(inner_guard_condition)); + auto local_guard_condition = inner_guard_condition; + this->storage_remove_guard_condition(std::move(local_guard_condition)); + inner_guard_condition->exchange_in_use_by_wait_set_state(false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -388,11 +391,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_timer( std::move(timer), [this](std::shared_ptr && inner_timer) { - inner_timer->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the timer is not in the wait set. - this->storage_remove_timer(std::move(inner_timer)); + auto local_timer = inner_timer; + this->storage_remove_timer(std::move(local_timer)); + inner_timer->exchange_in_use_by_wait_set_state(false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -448,11 +452,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_client( std::move(client), [this](std::shared_ptr && inner_client) { - inner_client->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the client is not in the wait set. - this->storage_remove_client(std::move(inner_client)); + auto local_client = inner_client; + this->storage_remove_client(std::move(local_client)); + inner_client->exchange_in_use_by_wait_set_state(false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -508,11 +513,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_service( std::move(service), [this](std::shared_ptr && inner_service) { - inner_service->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the service is not in the wait set. - this->storage_remove_service(std::move(inner_service)); + auto local_service = inner_service; + this->storage_remove_service(std::move(local_service)); + inner_service->exchange_in_use_by_wait_set_state(false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } @@ -588,11 +594,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->sync_remove_waitable( std::move(waitable), [this](std::shared_ptr && inner_waitable) { - inner_waitable->exchange_in_use_by_wait_set_state(false); // This method comes from the StoragePolicy, and it may not exist for // fixed sized storage policies. // It will throw if the waitable is not in the wait set. - this->storage_remove_waitable(std::move(inner_waitable)); + auto local_waitable = inner_waitable; + this->storage_remove_waitable(std::move(local_waitable)); + inner_waitable->exchange_in_use_by_wait_set_state(false); if (this->wait_result_holding_) {this->wait_result_dirty_ = true;} }); } diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index af78a28010..645a63e3bd 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -27,9 +27,11 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/publisher_options.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription_wait_set_mask.hpp" #include "rclcpp/timer.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/wait_result.hpp" @@ -287,6 +289,112 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { } +TEST_F(TestWaitSet, failed_removal_preserves_entity_ownership) { + rclcpp::WaitSet owner; + rclcpp::WaitSet other; + auto node = std::make_shared("failed_removal_preserves_entity_ownership"); + + auto guard_condition = std::make_shared(); + owner.add_guard_condition(guard_condition); + EXPECT_THROW(other.remove_guard_condition(guard_condition), std::runtime_error); + EXPECT_THROW(other.add_guard_condition(guard_condition), std::runtime_error); + guard_condition->trigger(); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, owner.wait(std::chrono::seconds(0)).kind()); + EXPECT_NO_THROW(owner.remove_guard_condition(guard_condition)); + EXPECT_THROW(owner.remove_guard_condition(guard_condition), std::runtime_error); + EXPECT_NO_THROW(other.add_guard_condition(guard_condition)); + EXPECT_NO_THROW(other.remove_guard_condition(guard_condition)); + EXPECT_THROW(other.remove_guard_condition(nullptr), std::invalid_argument); + + auto do_nothing = [](std::shared_ptr) {}; + auto sub = node->create_subscription("~/test", 1, do_nothing); + owner.add_subscription(sub, {true, false, false}); + EXPECT_THROW(other.remove_subscription(sub, {true, false, false}), std::runtime_error); + EXPECT_THROW(other.add_subscription(sub, {true, false, false}), std::runtime_error); + EXPECT_NO_THROW(owner.remove_subscription(sub, {true, false, false})); + EXPECT_THROW(owner.remove_subscription(sub, {true, false, false}), std::runtime_error); + EXPECT_NO_THROW(other.add_subscription(sub, {true, false, false})); + EXPECT_NO_THROW(other.remove_subscription(sub, {true, false, false})); + EXPECT_THROW(other.remove_subscription(nullptr, {true, false, false}), std::invalid_argument); + + auto timer = node->create_wall_timer(std::chrono::seconds(1), []() {}); + owner.add_timer(timer); + EXPECT_THROW(other.remove_timer(timer), std::runtime_error); + EXPECT_THROW(other.add_timer(timer), std::runtime_error); + EXPECT_NO_THROW(owner.remove_timer(timer)); + EXPECT_THROW(owner.remove_timer(timer), std::runtime_error); + EXPECT_NO_THROW(other.add_timer(timer)); + EXPECT_NO_THROW(other.remove_timer(timer)); + EXPECT_THROW(other.remove_timer(nullptr), std::invalid_argument); + + auto client = node->create_client("~/test"); + owner.add_client(client); + EXPECT_THROW(other.remove_client(client), std::runtime_error); + EXPECT_THROW(other.add_client(client), std::runtime_error); + EXPECT_NO_THROW(owner.remove_client(client)); + EXPECT_THROW(owner.remove_client(client), std::runtime_error); + EXPECT_NO_THROW(other.add_client(client)); + EXPECT_NO_THROW(other.remove_client(client)); + EXPECT_THROW(other.remove_client(nullptr), std::invalid_argument); + + auto srv_do_nothing = []( + const std::shared_ptr, + std::shared_ptr) {}; + auto service = + node->create_service("~/test", srv_do_nothing); + owner.add_service(service); + EXPECT_THROW(other.remove_service(service), std::runtime_error); + EXPECT_THROW(other.add_service(service), std::runtime_error); + EXPECT_NO_THROW(owner.remove_service(service)); + EXPECT_THROW(owner.remove_service(service), std::runtime_error); + EXPECT_NO_THROW(other.add_service(service)); + EXPECT_NO_THROW(other.remove_service(service)); + EXPECT_THROW(other.remove_service(nullptr), std::invalid_argument); + + rclcpp::PublisherOptions publisher_options; + publisher_options.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {}; + auto pub = node->create_publisher("~/test", 1, publisher_options); + auto qos_event = pub->get_event_handlers().begin()->second; + owner.add_waitable(qos_event, pub); + EXPECT_THROW(other.remove_waitable(qos_event), std::runtime_error); + EXPECT_THROW(other.add_waitable(qos_event, pub), std::runtime_error); + EXPECT_NO_THROW(owner.remove_waitable(qos_event)); + EXPECT_THROW(owner.remove_waitable(qos_event), std::runtime_error); + EXPECT_NO_THROW(other.add_waitable(qos_event, pub)); + EXPECT_NO_THROW(other.remove_waitable(qos_event)); + EXPECT_THROW(other.remove_waitable(nullptr), std::invalid_argument); +} + +TEST_F(TestWaitSet, failed_removal_preserves_subscription_waitable_ownership) { + auto node = std::make_shared( + "failed_removal_preserves_subscription_waitable_ownership", + rclcpp::NodeOptions().use_intra_process_comms(true)); + rclcpp::SubscriptionOptions options; + options.use_default_callbacks = false; + options.event_callbacks.deadline_callback = [](auto) {}; + auto do_nothing = [](std::shared_ptr) {}; + auto sub = + node->create_subscription("~/test", 1, do_nothing, options); + ASSERT_EQ(1u, sub->get_event_handlers().size()); + ASSERT_NE(nullptr, sub->get_intra_process_waitable()); + + for (const auto & mask : { + rclcpp::SubscriptionWaitSetMask{false, true, false}, + rclcpp::SubscriptionWaitSetMask{false, false, true}}) + { + SCOPED_TRACE(mask.include_events ? "events" : "intra-process waitable"); + rclcpp::WaitSet owner; + rclcpp::WaitSet other; + owner.add_subscription(sub, mask); + EXPECT_THROW(other.remove_subscription(sub, mask), std::runtime_error); + EXPECT_THROW(other.add_subscription(sub, mask), std::runtime_error); + EXPECT_NO_THROW(owner.remove_subscription(sub, mask)); + EXPECT_THROW(owner.remove_subscription(sub, mask), std::runtime_error); + EXPECT_NO_THROW(other.add_subscription(sub, mask)); + EXPECT_NO_THROW(other.remove_subscription(sub, mask)); + } +} + /* * Testing adding each entity and waiting, and removing each entity and waiting */