Skip to content

Commit 4feecc5

Browse files
guillaumeautranivanpauno
guillaumeautran
authored and
ivanpauno
committed
Crash in callback group pointer vector iterator (#814)
Signed-off-by: Guillaume Autran <[email protected]>
1 parent 17841d6 commit 4feecc5

File tree

7 files changed

+164
-135
lines changed

7 files changed

+164
-135
lines changed

‎rclcpp/include/rclcpp/callback_group.hpp‎

Lines changed: 49 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -62,25 +62,40 @@ class CallbackGroup
6262
RCLCPP_PUBLIC
6363
explicit CallbackGroup(CallbackGroupType group_type);
6464

65-
RCLCPP_PUBLIC
66-
const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
67-
get_subscription_ptrs() const;
68-
69-
RCLCPP_PUBLIC
70-
const std::vector<rclcpp::TimerBase::WeakPtr> &
71-
get_timer_ptrs() const;
72-
73-
RCLCPP_PUBLIC
74-
const std::vector<rclcpp::ServiceBase::WeakPtr> &
75-
get_service_ptrs() const;
76-
77-
RCLCPP_PUBLIC
78-
const std::vector<rclcpp::ClientBase::WeakPtr> &
79-
get_client_ptrs() const;
80-
81-
RCLCPP_PUBLIC
82-
const std::vector<rclcpp::Waitable::WeakPtr> &
83-
get_waitable_ptrs() const;
65+
template<typename Function>
66+
rclcpp::SubscriptionBase::SharedPtr
67+
find_subscription_ptrs_if(Function func) const
68+
{
69+
return _find_ptrs_if_impl<rclcpp::SubscriptionBase, Function>(func, subscription_ptrs_);
70+
}
71+
72+
template<typename Function>
73+
rclcpp::TimerBase::SharedPtr
74+
find_timer_ptrs_if(Function func) const
75+
{
76+
return _find_ptrs_if_impl<rclcpp::TimerBase, Function>(func, timer_ptrs_);
77+
}
78+
79+
template<typename Function>
80+
rclcpp::ServiceBase::SharedPtr
81+
find_service_ptrs_if(Function func) const
82+
{
83+
return _find_ptrs_if_impl<rclcpp::ServiceBase, Function>(func, service_ptrs_);
84+
}
85+
86+
template<typename Function>
87+
rclcpp::ClientBase::SharedPtr
88+
find_client_ptrs_if(Function func) const
89+
{
90+
return _find_ptrs_if_impl<rclcpp::ClientBase, Function>(func, client_ptrs_);
91+
}
92+
93+
template<typename Function>
94+
rclcpp::Waitable::SharedPtr
95+
find_waitable_ptrs_if(Function func) const
96+
{
97+
return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
98+
}
8499

85100
RCLCPP_PUBLIC
86101
std::atomic_bool &
@@ -130,6 +145,21 @@ class CallbackGroup
130145
std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
131146
std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
132147
std::atomic_bool can_be_taken_from_;
148+
149+
private:
150+
template<typename TypeT, typename Function>
151+
typename TypeT::SharedPtr _find_ptrs_if_impl(
152+
Function func, const std::vector<typename TypeT::WeakPtr> & vect_ptrs) const
153+
{
154+
std::lock_guard<std::mutex> lock(mutex_);
155+
for (auto & weak_ptr : vect_ptrs) {
156+
auto ref_ptr = weak_ptr.lock();
157+
if (ref_ptr && func(ref_ptr)) {
158+
return ref_ptr;
159+
}
160+
}
161+
return typename TypeT::SharedPtr();
162+
}
133163
};
134164

135165
} // namespace callback_group

‎rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
1616
#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
1717

18+
#include <chrono>
1819
#include <memory>
1920
#include <mutex>
2021
#include <set>
@@ -53,7 +54,8 @@ class MultiThreadedExecutor : public executor::Executor
5354
MultiThreadedExecutor(
5455
const executor::ExecutorArgs & args = executor::ExecutorArgs(),
5556
size_t number_of_threads = 0,
56-
bool yield_before_execute = false);
57+
bool yield_before_execute = false,
58+
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
5759

5860
RCLCPP_PUBLIC
5961
virtual ~MultiThreadedExecutor();
@@ -77,6 +79,7 @@ class MultiThreadedExecutor : public executor::Executor
7779
std::mutex wait_mutex_;
7880
size_t number_of_threads_;
7981
bool yield_before_execute_;
82+
std::chrono::nanoseconds next_exec_timeout_;
8083

8184
std::set<TimerBase::SharedPtr> scheduled_timers_;
8285
};

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

Lines changed: 16 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -164,40 +164,31 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
164164
if (!group || !group->can_be_taken_from().load()) {
165165
continue;
166166
}
167-
for (auto & weak_subscription : group->get_subscription_ptrs()) {
168-
auto subscription = weak_subscription.lock();
169-
if (subscription) {
167+
group->find_subscription_ptrs_if(
168+
[this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
170169
subscription_handles_.push_back(subscription->get_subscription_handle());
171170
if (subscription->get_intra_process_subscription_handle()) {
172171
subscription_handles_.push_back(
173172
subscription->get_intra_process_subscription_handle());
174173
}
175-
}
176-
}
177-
for (auto & weak_service : group->get_service_ptrs()) {
178-
auto service = weak_service.lock();
179-
if (service) {
174+
return false;
175+
});
176+
group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) {
180177
service_handles_.push_back(service->get_service_handle());
181-
}
182-
}
183-
for (auto & weak_client : group->get_client_ptrs()) {
184-
auto client = weak_client.lock();
185-
if (client) {
178+
return false;
179+
});
180+
group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) {
186181
client_handles_.push_back(client->get_client_handle());
187-
}
188-
}
189-
for (auto & weak_timer : group->get_timer_ptrs()) {
190-
auto timer = weak_timer.lock();
191-
if (timer) {
182+
return false;
183+
});
184+
group->find_timer_ptrs_if([this](const rclcpp::TimerBase::SharedPtr & timer) {
192185
timer_handles_.push_back(timer->get_timer_handle());
193-
}
194-
}
195-
for (auto & weak_waitable : group->get_waitable_ptrs()) {
196-
auto waitable = weak_waitable.lock();
197-
if (waitable) {
186+
return false;
187+
});
188+
group->find_waitable_ptrs_if([this](const rclcpp::Waitable::SharedPtr & waitable) {
198189
waitable_handles_.push_back(waitable);
199-
}
200-
}
190+
return false;
191+
});
201192
}
202193
}
203194
return has_invalid_weak_nodes;

‎rclcpp/src/rclcpp/callback_group.cpp‎

Lines changed: 30 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -23,40 +23,6 @@ CallbackGroup::CallbackGroup(CallbackGroupType group_type)
2323
: type_(group_type), can_be_taken_from_(true)
2424
{}
2525

26-
const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
27-
CallbackGroup::get_subscription_ptrs() const
28-
{
29-
std::lock_guard<std::mutex> lock(mutex_);
30-
return subscription_ptrs_;
31-
}
32-
33-
const std::vector<rclcpp::TimerBase::WeakPtr> &
34-
CallbackGroup::get_timer_ptrs() const
35-
{
36-
std::lock_guard<std::mutex> lock(mutex_);
37-
return timer_ptrs_;
38-
}
39-
40-
const std::vector<rclcpp::ServiceBase::WeakPtr> &
41-
CallbackGroup::get_service_ptrs() const
42-
{
43-
std::lock_guard<std::mutex> lock(mutex_);
44-
return service_ptrs_;
45-
}
46-
47-
const std::vector<rclcpp::ClientBase::WeakPtr> &
48-
CallbackGroup::get_client_ptrs() const
49-
{
50-
std::lock_guard<std::mutex> lock(mutex_);
51-
return client_ptrs_;
52-
}
53-
54-
const std::vector<rclcpp::Waitable::WeakPtr> &
55-
CallbackGroup::get_waitable_ptrs() const
56-
{
57-
std::lock_guard<std::mutex> lock(mutex_);
58-
return waitable_ptrs_;
59-
}
6026

6127
std::atomic_bool &
6228
CallbackGroup::can_be_taken_from()
@@ -76,34 +42,64 @@ CallbackGroup::add_subscription(
7642
{
7743
std::lock_guard<std::mutex> lock(mutex_);
7844
subscription_ptrs_.push_back(subscription_ptr);
45+
subscription_ptrs_.erase(
46+
std::remove_if(
47+
subscription_ptrs_.begin(),
48+
subscription_ptrs_.end(),
49+
[](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}),
50+
subscription_ptrs_.end());
7951
}
8052

8153
void
8254
CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr)
8355
{
8456
std::lock_guard<std::mutex> lock(mutex_);
8557
timer_ptrs_.push_back(timer_ptr);
58+
timer_ptrs_.erase(
59+
std::remove_if(
60+
timer_ptrs_.begin(),
61+
timer_ptrs_.end(),
62+
[](rclcpp::TimerBase::WeakPtr x) {return x.expired();}),
63+
timer_ptrs_.end());
8664
}
8765

8866
void
8967
CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr)
9068
{
9169
std::lock_guard<std::mutex> lock(mutex_);
9270
service_ptrs_.push_back(service_ptr);
71+
service_ptrs_.erase(
72+
std::remove_if(
73+
service_ptrs_.begin(),
74+
service_ptrs_.end(),
75+
[](rclcpp::ServiceBase::WeakPtr x) {return x.expired();}),
76+
service_ptrs_.end());
9377
}
9478

9579
void
9680
CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
9781
{
9882
std::lock_guard<std::mutex> lock(mutex_);
9983
client_ptrs_.push_back(client_ptr);
84+
client_ptrs_.erase(
85+
std::remove_if(
86+
client_ptrs_.begin(),
87+
client_ptrs_.end(),
88+
[](rclcpp::ClientBase::WeakPtr x) {return x.expired();}),
89+
client_ptrs_.end());
10090
}
10191

10292
void
10393
CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
10494
{
10595
std::lock_guard<std::mutex> lock(mutex_);
10696
waitable_ptrs_.push_back(waitable_ptr);
97+
waitable_ptrs_.erase(
98+
std::remove_if(
99+
waitable_ptrs_.begin(),
100+
waitable_ptrs_.end(),
101+
[](rclcpp::Waitable::WeakPtr x) {return x.expired();}),
102+
waitable_ptrs_.end());
107103
}
108104

109105
void

‎rclcpp/src/rclcpp/executor.cpp‎

Lines changed: 15 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -511,11 +511,12 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
511511
if (!group) {
512512
continue;
513513
}
514-
for (auto & weak_timer : group->get_timer_ptrs()) {
515-
auto t = weak_timer.lock();
516-
if (t == timer) {
517-
return group;
518-
}
514+
auto timer_ref = group->find_timer_ptrs_if(
515+
[timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
516+
return timer_ptr == timer;
517+
});
518+
if (timer_ref) {
519+
return group;
519520
}
520521
}
521522
}
@@ -535,14 +536,15 @@ Executor::get_next_timer(AnyExecutable & any_exec)
535536
if (!group || !group->can_be_taken_from().load()) {
536537
continue;
537538
}
538-
for (auto & timer_ref : group->get_timer_ptrs()) {
539-
auto timer = timer_ref.lock();
540-
if (timer && timer->is_ready()) {
541-
any_exec.timer = timer;
542-
any_exec.callback_group = group;
543-
any_exec.node_base = node;
544-
return;
545-
}
539+
auto timer_ref = group->find_timer_ptrs_if(
540+
[](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
541+
return timer->is_ready();
542+
});
543+
if (timer_ref) {
544+
any_exec.timer = timer_ref;
545+
any_exec.callback_group = group;
546+
any_exec.node_base = node;
547+
return;
546548
}
547549
}
548550
}

‎rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,11 @@ using rclcpp::executors::MultiThreadedExecutor;
2727
MultiThreadedExecutor::MultiThreadedExecutor(
2828
const rclcpp::executor::ExecutorArgs & args,
2929
size_t number_of_threads,
30-
bool yield_before_execute)
31-
: executor::Executor(args), yield_before_execute_(yield_before_execute)
30+
bool yield_before_execute,
31+
std::chrono::nanoseconds next_exec_timeout)
32+
: executor::Executor(args),
33+
yield_before_execute_(yield_before_execute),
34+
next_exec_timeout_(next_exec_timeout)
3235
{
3336
number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
3437
if (number_of_threads_ == 0) {
@@ -77,7 +80,7 @@ MultiThreadedExecutor::run(size_t)
7780
if (!rclcpp::ok(this->context_) || !spinning.load()) {
7881
return;
7982
}
80-
if (!get_next_executable(any_exec)) {
83+
if (!get_next_executable(any_exec, next_exec_timeout_)) {
8184
continue;
8285
}
8386
if (any_exec.timer) {

0 commit comments

Comments
 (0)