Skip to content

Commit 8553fbe

Browse files
dirk-thomasdirk-thomas
authored
ensure removal of guard conditions of expired nodes from memory strategy (#741)
* change memory strategy API from vector of nodes to list of nodes Signed-off-by: Dirk Thomas <[email protected]> * store guard_condition of node in executor and ensure that it is removed from the memory strategy Signed-off-by: Dirk Thomas <[email protected]> * add unit test Signed-off-by: Dirk Thomas <[email protected]>
1 parent 131a11b commit 8553fbe

File tree

7 files changed

+74
-54
lines changed

7 files changed

+74
-54
lines changed

‎rclcpp/include/rclcpp/executor.hpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -364,7 +364,8 @@ class Executor
364364
private:
365365
RCLCPP_DISABLE_COPY(Executor)
366366

367-
std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
367+
std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> weak_nodes_;
368+
std::list<const rcl_guard_condition_t *> guard_conditions_;
368369
};
369370

370371
} // namespace executor

‎rclcpp/include/rclcpp/memory_strategy.hpp‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -15,8 +15,8 @@
1515
#ifndef RCLCPP__MEMORY_STRATEGY_HPP_
1616
#define RCLCPP__MEMORY_STRATEGY_HPP_
1717

18+
#include <list>
1819
#include <memory>
19-
#include <vector>
2020

2121
#include "rcl/allocator.h"
2222
#include "rcl/wait.h"
@@ -42,11 +42,11 @@ class RCLCPP_PUBLIC MemoryStrategy
4242
{
4343
public:
4444
RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy)
45-
using WeakNodeVector = std::vector<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
45+
using WeakNodeList = std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>;
4646

4747
virtual ~MemoryStrategy() = default;
4848

49-
virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
49+
virtual bool collect_entities(const WeakNodeList & weak_nodes) = 0;
5050

5151
virtual size_t number_of_ready_subscriptions() const = 0;
5252
virtual size_t number_of_ready_services() const = 0;
@@ -67,65 +67,65 @@ class RCLCPP_PUBLIC MemoryStrategy
6767
virtual void
6868
get_next_subscription(
6969
rclcpp::executor::AnyExecutable & any_exec,
70-
const WeakNodeVector & weak_nodes) = 0;
70+
const WeakNodeList & weak_nodes) = 0;
7171

7272
virtual void
7373
get_next_service(
7474
rclcpp::executor::AnyExecutable & any_exec,
75-
const WeakNodeVector & weak_nodes) = 0;
75+
const WeakNodeList & weak_nodes) = 0;
7676

7777
virtual void
7878
get_next_client(
7979
rclcpp::executor::AnyExecutable & any_exec,
80-
const WeakNodeVector & weak_nodes) = 0;
80+
const WeakNodeList & weak_nodes) = 0;
8181

8282
virtual void
8383
get_next_waitable(
8484
rclcpp::executor::AnyExecutable & any_exec,
85-
const WeakNodeVector & weak_nodes) = 0;
85+
const WeakNodeList & weak_nodes) = 0;
8686

8787
virtual rcl_allocator_t
8888
get_allocator() = 0;
8989

9090
static rclcpp::SubscriptionBase::SharedPtr
9191
get_subscription_by_handle(
9292
std::shared_ptr<const rcl_subscription_t> subscriber_handle,
93-
const WeakNodeVector & weak_nodes);
93+
const WeakNodeList & weak_nodes);
9494

9595
static rclcpp::ServiceBase::SharedPtr
9696
get_service_by_handle(
9797
std::shared_ptr<const rcl_service_t> service_handle,
98-
const WeakNodeVector & weak_nodes);
98+
const WeakNodeList & weak_nodes);
9999

100100
static rclcpp::ClientBase::SharedPtr
101101
get_client_by_handle(
102102
std::shared_ptr<const rcl_client_t> client_handle,
103-
const WeakNodeVector & weak_nodes);
103+
const WeakNodeList & weak_nodes);
104104

105105
static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
106106
get_node_by_group(
107107
rclcpp::callback_group::CallbackGroup::SharedPtr group,
108-
const WeakNodeVector & weak_nodes);
108+
const WeakNodeList & weak_nodes);
109109

110110
static rclcpp::callback_group::CallbackGroup::SharedPtr
111111
get_group_by_subscription(
112112
rclcpp::SubscriptionBase::SharedPtr subscription,
113-
const WeakNodeVector & weak_nodes);
113+
const WeakNodeList & weak_nodes);
114114

115115
static rclcpp::callback_group::CallbackGroup::SharedPtr
116116
get_group_by_service(
117117
rclcpp::ServiceBase::SharedPtr service,
118-
const WeakNodeVector & weak_nodes);
118+
const WeakNodeList & weak_nodes);
119119

120120
static rclcpp::callback_group::CallbackGroup::SharedPtr
121121
get_group_by_client(
122122
rclcpp::ClientBase::SharedPtr client,
123-
const WeakNodeVector & weak_nodes);
123+
const WeakNodeList & weak_nodes);
124124

125125
static rclcpp::callback_group::CallbackGroup::SharedPtr
126126
get_group_by_waitable(
127127
rclcpp::Waitable::SharedPtr waitable,
128-
const WeakNodeVector & weak_nodes);
128+
const WeakNodeList & weak_nodes);
129129
};
130130

131131
} // namespace memory_strategy

‎rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -150,7 +150,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
150150
);
151151
}
152152

153-
bool collect_entities(const WeakNodeVector & weak_nodes)
153+
bool collect_entities(const WeakNodeList & weak_nodes)
154154
{
155155
bool has_invalid_weak_nodes = false;
156156
for (auto & weak_node : weak_nodes) {
@@ -265,7 +265,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
265265
virtual void
266266
get_next_subscription(
267267
executor::AnyExecutable & any_exec,
268-
const WeakNodeVector & weak_nodes)
268+
const WeakNodeList & weak_nodes)
269269
{
270270
auto it = subscription_handles_.begin();
271271
while (it != subscription_handles_.end()) {
@@ -309,7 +309,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
309309
virtual void
310310
get_next_service(
311311
executor::AnyExecutable & any_exec,
312-
const WeakNodeVector & weak_nodes)
312+
const WeakNodeList & weak_nodes)
313313
{
314314
auto it = service_handles_.begin();
315315
while (it != service_handles_.end()) {
@@ -342,7 +342,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
342342
}
343343

344344
virtual void
345-
get_next_client(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
345+
get_next_client(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes)
346346
{
347347
auto it = client_handles_.begin();
348348
while (it != client_handles_.end()) {
@@ -375,7 +375,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
375375
}
376376

377377
virtual void
378-
get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeVector & weak_nodes)
378+
get_next_waitable(executor::AnyExecutable & any_exec, const WeakNodeList & weak_nodes)
379379
{
380380
auto it = waitable_handles_.begin();
381381
while (it != waitable_handles_.end()) {

‎rclcpp/src/rclcpp/executor.cpp‎

Lines changed: 31 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,10 @@ Executor::~Executor()
9191
}
9292
}
9393
weak_nodes_.clear();
94+
for (auto & guard_condition : guard_conditions_) {
95+
memory_strategy_->remove_guard_condition(guard_condition);
96+
}
97+
guard_conditions_.clear();
9498

9599
// Finalize the wait set.
96100
if (rcl_wait_set_fini(&wait_set_) != RCL_RET_OK) {
@@ -128,6 +132,7 @@ Executor::add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_pt
128132
}
129133
}
130134
weak_nodes_.push_back(node_ptr);
135+
guard_conditions_.push_back(node_ptr->get_notify_guard_condition());
131136
if (notify) {
132137
// Interrupt waiting to handle new node
133138
if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) {
@@ -148,17 +153,21 @@ void
148153
Executor::remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify)
149154
{
150155
bool node_removed = false;
151-
weak_nodes_.erase(
152-
std::remove_if(
153-
weak_nodes_.begin(), weak_nodes_.end(),
154-
[&](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr & i)
155-
{
156-
bool matched = (i.lock() == node_ptr);
157-
node_removed |= matched;
158-
return matched;
156+
{
157+
auto node_it = weak_nodes_.begin();
158+
auto gc_it = guard_conditions_.begin();
159+
while (node_it != weak_nodes_.end()) {
160+
bool matched = (node_it->lock() == node_ptr);
161+
if (matched) {
162+
node_it = weak_nodes_.erase(node_it);
163+
gc_it = guard_conditions_.erase(gc_it);
164+
node_removed = true;
165+
} else {
166+
++node_it;
167+
++gc_it;
159168
}
160-
)
161-
);
169+
}
170+
}
162171
std::atomic_bool & has_executor = node_ptr->get_associated_with_executor_atomic();
163172
has_executor.store(false);
164173
if (notify) {
@@ -420,15 +429,18 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
420429

421430
// Clean up any invalid nodes, if they were detected
422431
if (has_invalid_weak_nodes) {
423-
weak_nodes_.erase(
424-
remove_if(
425-
weak_nodes_.begin(), weak_nodes_.end(),
426-
[](rclcpp::node_interfaces::NodeBaseInterface::WeakPtr i)
427-
{
428-
return i.expired();
429-
}
430-
)
431-
);
432+
auto node_it = weak_nodes_.begin();
433+
auto gc_it = guard_conditions_.begin();
434+
while (node_it != weak_nodes_.end()) {
435+
if (node_it->expired()) {
436+
node_it = weak_nodes_.erase(node_it);
437+
memory_strategy_->remove_guard_condition(*gc_it);
438+
gc_it = guard_conditions_.erase(gc_it);
439+
} else {
440+
++node_it;
441+
++gc_it;
442+
}
443+
}
432444
}
433445
// clear wait set
434446
if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) {

‎rclcpp/src/rclcpp/memory_strategy.cpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ using rclcpp::memory_strategy::MemoryStrategy;
2020
rclcpp::SubscriptionBase::SharedPtr
2121
MemoryStrategy::get_subscription_by_handle(
2222
std::shared_ptr<const rcl_subscription_t> subscriber_handle,
23-
const WeakNodeVector & weak_nodes)
23+
const WeakNodeList & weak_nodes)
2424
{
2525
for (auto & weak_node : weak_nodes) {
2626
auto node = weak_node.lock();
@@ -51,7 +51,7 @@ MemoryStrategy::get_subscription_by_handle(
5151
rclcpp::ServiceBase::SharedPtr
5252
MemoryStrategy::get_service_by_handle(
5353
std::shared_ptr<const rcl_service_t> service_handle,
54-
const WeakNodeVector & weak_nodes)
54+
const WeakNodeList & weak_nodes)
5555
{
5656
for (auto & weak_node : weak_nodes) {
5757
auto node = weak_node.lock();
@@ -77,7 +77,7 @@ MemoryStrategy::get_service_by_handle(
7777
rclcpp::ClientBase::SharedPtr
7878
MemoryStrategy::get_client_by_handle(
7979
std::shared_ptr<const rcl_client_t> client_handle,
80-
const WeakNodeVector & weak_nodes)
80+
const WeakNodeList & weak_nodes)
8181
{
8282
for (auto & weak_node : weak_nodes) {
8383
auto node = weak_node.lock();
@@ -103,7 +103,7 @@ MemoryStrategy::get_client_by_handle(
103103
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
104104
MemoryStrategy::get_node_by_group(
105105
rclcpp::callback_group::CallbackGroup::SharedPtr group,
106-
const WeakNodeVector & weak_nodes)
106+
const WeakNodeList & weak_nodes)
107107
{
108108
if (!group) {
109109
return nullptr;
@@ -126,7 +126,7 @@ MemoryStrategy::get_node_by_group(
126126
rclcpp::callback_group::CallbackGroup::SharedPtr
127127
MemoryStrategy::get_group_by_subscription(
128128
rclcpp::SubscriptionBase::SharedPtr subscription,
129-
const WeakNodeVector & weak_nodes)
129+
const WeakNodeList & weak_nodes)
130130
{
131131
for (auto & weak_node : weak_nodes) {
132132
auto node = weak_node.lock();
@@ -152,7 +152,7 @@ MemoryStrategy::get_group_by_subscription(
152152
rclcpp::callback_group::CallbackGroup::SharedPtr
153153
MemoryStrategy::get_group_by_service(
154154
rclcpp::ServiceBase::SharedPtr service,
155-
const WeakNodeVector & weak_nodes)
155+
const WeakNodeList & weak_nodes)
156156
{
157157
for (auto & weak_node : weak_nodes) {
158158
auto node = weak_node.lock();
@@ -178,7 +178,7 @@ MemoryStrategy::get_group_by_service(
178178
rclcpp::callback_group::CallbackGroup::SharedPtr
179179
MemoryStrategy::get_group_by_client(
180180
rclcpp::ClientBase::SharedPtr client,
181-
const WeakNodeVector & weak_nodes)
181+
const WeakNodeList & weak_nodes)
182182
{
183183
for (auto & weak_node : weak_nodes) {
184184
auto node = weak_node.lock();
@@ -204,7 +204,7 @@ MemoryStrategy::get_group_by_client(
204204
rclcpp::callback_group::CallbackGroup::SharedPtr
205205
MemoryStrategy::get_group_by_waitable(
206206
rclcpp::Waitable::SharedPtr waitable,
207-
const WeakNodeVector & weak_nodes)
207+
const WeakNodeList & weak_nodes)
208208
{
209209
for (auto & weak_node : weak_nodes) {
210210
auto node = weak_node.lock();

‎rclcpp/test/test_executor.cpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,3 +60,10 @@ TEST_F(TestExecutors, detachOnDestruction) {
6060
EXPECT_NO_THROW(executor.add_node(node));
6161
}
6262
}
63+
64+
// Make sure that the executor can automatically remove expired nodes correctly
65+
TEST_F(TestExecutors, addTemporaryNode) {
66+
rclcpp::executors::SingleThreadedExecutor executor;
67+
executor.add_node(std::make_shared<rclcpp::Node>("temporary_node"));
68+
EXPECT_NO_THROW(executor.spin_some());
69+
}

‎rclcpp/test/test_find_weak_nodes.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -36,15 +36,15 @@ TEST_F(TestFindWeakNodes, allocator_strategy_with_weak_nodes) {
3636
rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
3737
auto existing_node = rclcpp::Node::make_shared("existing_node");
3838
auto dead_node = rclcpp::Node::make_shared("dead_node");
39-
rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes;
39+
rclcpp::memory_strategy::MemoryStrategy::WeakNodeList weak_nodes;
4040
weak_nodes.push_back(existing_node->get_node_base_interface());
4141
weak_nodes.push_back(dead_node->get_node_base_interface());
4242

4343
// AND
4444
// Delete dead_node, creating a dangling pointer in weak_nodes
4545
dead_node.reset();
46-
ASSERT_FALSE(weak_nodes[0].expired());
47-
ASSERT_TRUE(weak_nodes[1].expired());
46+
ASSERT_FALSE(weak_nodes.front().expired());
47+
ASSERT_TRUE(weak_nodes.back().expired());
4848

4949
// WHEN
5050
bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes);
@@ -64,11 +64,11 @@ TEST_F(TestFindWeakNodes, allocator_strategy_no_weak_nodes) {
6464
rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
6565
auto existing_node1 = rclcpp::Node::make_shared("existing_node1");
6666
auto existing_node2 = rclcpp::Node::make_shared("existing_node2");
67-
rclcpp::memory_strategy::MemoryStrategy::WeakNodeVector weak_nodes;
67+
rclcpp::memory_strategy::MemoryStrategy::WeakNodeList weak_nodes;
6868
weak_nodes.push_back(existing_node1->get_node_base_interface());
6969
weak_nodes.push_back(existing_node2->get_node_base_interface());
70-
ASSERT_FALSE(weak_nodes[0].expired());
71-
ASSERT_FALSE(weak_nodes[1].expired());
70+
ASSERT_FALSE(weak_nodes.front().expired());
71+
ASSERT_FALSE(weak_nodes.back().expired());
7272

7373
// WHEN
7474
bool has_invalid_weak_nodes = memory_strategy->collect_entities(weak_nodes);

0 commit comments

Comments
 (0)