Skip to content

Preserve wait-set ownership when removal fails #3294

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Miko997 wants to merge 1 commit into
base: rolling
Choose a base branch
from
+128 −13
Open

Preserve wait-set ownership when removal fails #3294

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 20 additions & 13 deletions rclcpp/include/rclcpp/wait_set_template.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,25 +225,27 @@ 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;}
}
}
if (mask.include_intra_process_waitable) {
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;}
}
}
Expand Down Expand Up @@ -328,11 +330,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli
this->sync_remove_guard_condition(
std::move(guard_condition),
[this](std::shared_ptr<rclcpp::GuardCondition> && 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;}
});
}
Expand Down Expand Up @@ -388,11 +391,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli
this->sync_remove_timer(
std::move(timer),
[this](std::shared_ptr<rclcpp::TimerBase> && 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;}
});
}
Expand Down Expand Up @@ -448,11 +452,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli
this->sync_remove_client(
std::move(client),
[this](std::shared_ptr<rclcpp::ClientBase> && 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;}
});
}
Expand Down Expand Up @@ -508,11 +513,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli
this->sync_remove_service(
std::move(service),
[this](std::shared_ptr<rclcpp::ServiceBase> && 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;}
});
}
Expand Down Expand Up @@ -588,11 +594,12 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli
this->sync_remove_waitable(
std::move(waitable),
[this](std::shared_ptr<rclcpp::Waitable> && 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;}
});
}
Expand Down
108 changes: 108 additions & 0 deletions rclcpp/test/rclcpp/test_wait_set.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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<rclcpp::Node>("failed_removal_preserves_entity_ownership");

auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
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<const test_msgs::msg::BasicTypes>) {};
auto sub = node->create_subscription<test_msgs::msg::BasicTypes>("~/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<rcl_interfaces::srv::ListParameters>("~/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<rcl_interfaces::srv::ListParameters::Request>,
std::shared_ptr<rcl_interfaces::srv::ListParameters::Response>) {};
auto service =
node->create_service<rcl_interfaces::srv::ListParameters>("~/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_msgs::msg::BasicTypes>("~/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<rclcpp::Node>(
"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<const test_msgs::msg::BasicTypes>) {};
auto sub =
node->create_subscription<test_msgs::msg::BasicTypes>("~/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
*/
Expand Down
Loading