niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Crash in callback group pointer vector iterator (#814)

Browse source 
Signed-off-by: Guillaume Autran <gautran@clearpath.ai>
This commit is contained in:
Guillaume Autran 2019-08-28 17:11:11 -04:00 • committed by ivanpauno
parent 17841d6b7c
commit 4feecc5945
7 changed files with 160 additions and 131 deletions
Download patch file Download diff file Expand all files Collapse all files

60
rclcpp/include/rclcpp/callback_group.hpp
View file

@ -62,25 +62,40 @@ public:
RCLCPP_PUBLIC
explicit CallbackGroup(CallbackGroupType group_type);
RCLCPP_PUBLIC
const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
get_subscription_ptrs() const;
template<typename Function>
rclcpp::SubscriptionBase::SharedPtr
find_subscription_ptrs_if(Function func) const
{
return _find_ptrs_if_impl<rclcpp::SubscriptionBase, Function>(func, subscription_ptrs_);
}
RCLCPP_PUBLIC
const std::vector<rclcpp::TimerBase::WeakPtr> &
get_timer_ptrs() const;
template<typename Function>
rclcpp::TimerBase::SharedPtr
find_timer_ptrs_if(Function func) const
{
return _find_ptrs_if_impl<rclcpp::TimerBase, Function>(func, timer_ptrs_);
}
RCLCPP_PUBLIC
const std::vector<rclcpp::ServiceBase::WeakPtr> &
get_service_ptrs() const;
template<typename Function>
rclcpp::ServiceBase::SharedPtr
find_service_ptrs_if(Function func) const
{
return _find_ptrs_if_impl<rclcpp::ServiceBase, Function>(func, service_ptrs_);
}
RCLCPP_PUBLIC
const std::vector<rclcpp::ClientBase::WeakPtr> &
get_client_ptrs() const;
template<typename Function>
rclcpp::ClientBase::SharedPtr
find_client_ptrs_if(Function func) const
{
return _find_ptrs_if_impl<rclcpp::ClientBase, Function>(func, client_ptrs_);
}
RCLCPP_PUBLIC
const std::vector<rclcpp::Waitable::WeakPtr> &
get_waitable_ptrs() const;
template<typename Function>
rclcpp::Waitable::SharedPtr
find_waitable_ptrs_if(Function func) const
{
return _find_ptrs_if_impl<rclcpp::Waitable, Function>(func, waitable_ptrs_);
}
RCLCPP_PUBLIC
std::atomic_bool &
@ -130,6 +145,21 @@ protected:
std::vector<rclcpp::ClientBase::WeakPtr> client_ptrs_;
std::vector<rclcpp::Waitable::WeakPtr> waitable_ptrs_;
std::atomic_bool can_be_taken_from_;
private:
template<typename TypeT, typename Function>
typename TypeT::SharedPtr _find_ptrs_if_impl(
Function func, const std::vector<typename TypeT::WeakPtr> & vect_ptrs) const
{
std::lock_guard<std::mutex> lock(mutex_);
for (auto & weak_ptr : vect_ptrs) {
auto ref_ptr = weak_ptr.lock();
if (ref_ptr && func(ref_ptr)) {
return ref_ptr;
}
}
return typename TypeT::SharedPtr();
}
};
} // namespace callback_group

5
rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
View file

@ -15,6 +15,7 @@
#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
#include <chrono>
#include <memory>
#include <mutex>
#include <set>
@ -53,7 +54,8 @@ public:
MultiThreadedExecutor(
const executor::ExecutorArgs & args = executor::ExecutorArgs(),
size_t number_of_threads = 0,
bool yield_before_execute = false);
bool yield_before_execute = false,
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
RCLCPP_PUBLIC
virtual ~MultiThreadedExecutor();
@ -77,6 +79,7 @@ private:
std::mutex wait_mutex_;
size_t number_of_threads_;
bool yield_before_execute_;
std::chrono::nanoseconds next_exec_timeout_;
std::set<TimerBase::SharedPtr> scheduled_timers_;
};

41
rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
View file

@ -164,40 +164,31 @@ public:
if (!group || !group->can_be_taken_from().load()) {
continue;
}
for (auto & weak_subscription : group->get_subscription_ptrs()) {
auto subscription = weak_subscription.lock();
if (subscription) {
group->find_subscription_ptrs_if(
[this](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
subscription_handles_.push_back(subscription->get_subscription_handle());
if (subscription->get_intra_process_subscription_handle()) {
subscription_handles_.push_back(
subscription->get_intra_process_subscription_handle());
}
}
}
for (auto & weak_service : group->get_service_ptrs()) {
auto service = weak_service.lock();
if (service) {
return false;
});
group->find_service_ptrs_if([this](const rclcpp::ServiceBase::SharedPtr & service) {
service_handles_.push_back(service->get_service_handle());
}
}
for (auto & weak_client : group->get_client_ptrs()) {
auto client = weak_client.lock();
if (client) {
return false;
});
group->find_client_ptrs_if([this](const rclcpp::ClientBase::SharedPtr & client) {
client_handles_.push_back(client->get_client_handle());
}
}
for (auto & weak_timer : group->get_timer_ptrs()) {
auto timer = weak_timer.lock();
if (timer) {
return false;
});
group->find_timer_ptrs_if([this](const rclcpp::TimerBase::SharedPtr & timer) {
timer_handles_.push_back(timer->get_timer_handle());
}
}
for (auto & weak_waitable : group->get_waitable_ptrs()) {
auto waitable = weak_waitable.lock();
if (waitable) {
return false;
});
group->find_waitable_ptrs_if([this](const rclcpp::Waitable::SharedPtr & waitable) {
waitable_handles_.push_back(waitable);
}
}
return false;
});
}
}
return has_invalid_weak_nodes;

64
rclcpp/src/rclcpp/callback_group.cpp
View file

@ -23,40 +23,6 @@ CallbackGroup::CallbackGroup(CallbackGroupType group_type)
: type_(group_type), can_be_taken_from_(true)
{}
const std::vector<rclcpp::SubscriptionBase::WeakPtr> &
CallbackGroup::get_subscription_ptrs() const
{
std::lock_guard<std::mutex> lock(mutex_);
return subscription_ptrs_;
}
const std::vector<rclcpp::TimerBase::WeakPtr> &
CallbackGroup::get_timer_ptrs() const
{
std::lock_guard<std::mutex> lock(mutex_);
return timer_ptrs_;
}
const std::vector<rclcpp::ServiceBase::WeakPtr> &
CallbackGroup::get_service_ptrs() const
{
std::lock_guard<std::mutex> lock(mutex_);
return service_ptrs_;
}
const std::vector<rclcpp::ClientBase::WeakPtr> &
CallbackGroup::get_client_ptrs() const
{
std::lock_guard<std::mutex> lock(mutex_);
return client_ptrs_;
}
const std::vector<rclcpp::Waitable::WeakPtr> &
CallbackGroup::get_waitable_ptrs() const
{
std::lock_guard<std::mutex> lock(mutex_);
return waitable_ptrs_;
}
std::atomic_bool &
CallbackGroup::can_be_taken_from()
@ -76,6 +42,12 @@ CallbackGroup::add_subscription(
{
std::lock_guard<std::mutex> lock(mutex_);
subscription_ptrs_.push_back(subscription_ptr);
subscription_ptrs_.erase(
std::remove_if(
subscription_ptrs_.begin(),
subscription_ptrs_.end(),
[](rclcpp::SubscriptionBase::WeakPtr x) {return x.expired();}),
subscription_ptrs_.end());
}
void
@ -83,6 +55,12 @@ CallbackGroup::add_timer(const rclcpp::TimerBase::SharedPtr timer_ptr)
{
std::lock_guard<std::mutex> lock(mutex_);
timer_ptrs_.push_back(timer_ptr);
timer_ptrs_.erase(
std::remove_if(
timer_ptrs_.begin(),
timer_ptrs_.end(),
[](rclcpp::TimerBase::WeakPtr x) {return x.expired();}),
timer_ptrs_.end());
}
void
@ -90,6 +68,12 @@ CallbackGroup::add_service(const rclcpp::ServiceBase::SharedPtr service_ptr)
{
std::lock_guard<std::mutex> lock(mutex_);
service_ptrs_.push_back(service_ptr);
service_ptrs_.erase(
std::remove_if(
service_ptrs_.begin(),
service_ptrs_.end(),
[](rclcpp::ServiceBase::WeakPtr x) {return x.expired();}),
service_ptrs_.end());
}
void
@ -97,6 +81,12 @@ CallbackGroup::add_client(const rclcpp::ClientBase::SharedPtr client_ptr)
{
std::lock_guard<std::mutex> lock(mutex_);
client_ptrs_.push_back(client_ptr);
client_ptrs_.erase(
std::remove_if(
client_ptrs_.begin(),
client_ptrs_.end(),
[](rclcpp::ClientBase::WeakPtr x) {return x.expired();}),
client_ptrs_.end());
}
void
@ -104,6 +94,12 @@ CallbackGroup::add_waitable(const rclcpp::Waitable::SharedPtr waitable_ptr)
{
std::lock_guard<std::mutex> lock(mutex_);
waitable_ptrs_.push_back(waitable_ptr);
waitable_ptrs_.erase(
std::remove_if(
waitable_ptrs_.begin(),
waitable_ptrs_.end(),
[](rclcpp::Waitable::WeakPtr x) {return x.expired();}),
waitable_ptrs_.end());
}
void

20
rclcpp/src/rclcpp/executor.cpp
View file

@ -511,14 +511,15 @@ Executor::get_group_by_timer(rclcpp::TimerBase::SharedPtr timer)
if (!group) {
continue;
}
for (auto & weak_timer : group->get_timer_ptrs()) {
auto t = weak_timer.lock();
if (t == timer) {
auto timer_ref = group->find_timer_ptrs_if(
[timer](const rclcpp::TimerBase::SharedPtr & timer_ptr) -> bool {
return timer_ptr == timer;
});
if (timer_ref) {
return group;
}
}
}
}
return rclcpp::callback_group::CallbackGroup::SharedPtr();
}
@ -535,10 +536,12 @@ Executor::get_next_timer(AnyExecutable & any_exec)
if (!group || !group->can_be_taken_from().load()) {
continue;
}
for (auto & timer_ref : group->get_timer_ptrs()) {
auto timer = timer_ref.lock();
if (timer && timer->is_ready()) {
any_exec.timer = timer;
auto timer_ref = group->find_timer_ptrs_if(
[](const rclcpp::TimerBase::SharedPtr & timer) -> bool {
return timer->is_ready();
});
if (timer_ref) {
any_exec.timer = timer_ref;
any_exec.callback_group = group;
any_exec.node_base = node;
return;
@ -546,7 +549,6 @@ Executor::get_next_timer(AnyExecutable & any_exec)
}
}
}
}
bool
Executor::get_next_ready_executable(AnyExecutable & any_executable)

9
rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp
View file

@ -27,8 +27,11 @@ using rclcpp::executors::MultiThreadedExecutor;
MultiThreadedExecutor::MultiThreadedExecutor(
const rclcpp::executor::ExecutorArgs & args,
size_t number_of_threads,
bool yield_before_execute)
: executor::Executor(args), yield_before_execute_(yield_before_execute)
bool yield_before_execute,
std::chrono::nanoseconds next_exec_timeout)
: executor::Executor(args),
yield_before_execute_(yield_before_execute),
next_exec_timeout_(next_exec_timeout)
{
number_of_threads_ = number_of_threads ? number_of_threads : std::thread::hardware_concurrency();
if (number_of_threads_ == 0) {
@ -77,7 +80,7 @@ MultiThreadedExecutor::run(size_t)
if (!rclcpp::ok(this->context_) || !spinning.load()) {
return;
}
if (!get_next_executable(any_exec)) {
if (!get_next_executable(any_exec, next_exec_timeout_)) {
continue;
}
if (any_exec.timer) {

76
rclcpp/src/rclcpp/memory_strategy.cpp
View file

@ -32,16 +32,14 @@ MemoryStrategy::get_subscription_by_handle(
if (!group) {
continue;
}
for (auto & weak_subscription : group->get_subscription_ptrs()) {
auto subscription = weak_subscription.lock();
if (subscription) {
if (subscription->get_subscription_handle() == subscriber_handle) {
return subscription;
}
if (subscription->get_intra_process_subscription_handle() == subscriber_handle) {
return subscription;
}
}
auto match_subscription = group->find_subscription_ptrs_if(
[&subscriber_handle](const rclcpp::SubscriptionBase::SharedPtr & subscription) -> bool {
return
(subscription->get_subscription_handle() == subscriber_handle) ||
(subscription->get_intra_process_subscription_handle() == subscriber_handle);
});
if (match_subscription) {
return match_subscription;
}
}
}
@ -63,11 +61,12 @@ MemoryStrategy::get_service_by_handle(
if (!group) {
continue;
}
for (auto & weak_service : group->get_service_ptrs()) {
auto service = weak_service.lock();
if (service && service->get_service_handle() == service_handle) {
return service;
}
auto service_ref = group->find_service_ptrs_if(
[&service_handle](const rclcpp::ServiceBase::SharedPtr & service) -> bool {
return service->get_service_handle() == service_handle;
});
if (service_ref) {
return service_ref;
}
}
}
@ -89,11 +88,12 @@ MemoryStrategy::get_client_by_handle(
if (!group) {
continue;
}
for (auto & weak_client : group->get_client_ptrs()) {
auto client = weak_client.lock();
if (client && client->get_client_handle() == client_handle) {
return client;
}
auto client_ref = group->find_client_ptrs_if(
[&client_handle](const rclcpp::ClientBase::SharedPtr & client) -> bool {
return client->get_client_handle() == client_handle;
});
if (client_ref) {
return client_ref;
}
}
}
@ -138,14 +138,15 @@ MemoryStrategy::get_group_by_subscription(
if (!group) {
continue;
}
for (auto & weak_sub : group->get_subscription_ptrs()) {
auto sub = weak_sub.lock();
if (sub == subscription) {
auto match_sub = group->find_subscription_ptrs_if(
[&subscription](const rclcpp::SubscriptionBase::SharedPtr & sub) -> bool {
return sub == subscription;
});
if (match_sub) {
return group;
}
}
}
}
return nullptr;
}
@ -164,14 +165,15 @@ MemoryStrategy::get_group_by_service(
if (!group) {
continue;
}
for (auto & weak_serv : group->get_service_ptrs()) {
auto serv = weak_serv.lock();
if (serv && serv == service) {
auto service_ref = group->find_service_ptrs_if(
[&service](const rclcpp::ServiceBase::SharedPtr & serv) -> bool {
return serv == service;
});
if (service_ref) {
return group;
}
}
}
}
return nullptr;
}
@ -190,14 +192,15 @@ MemoryStrategy::get_group_by_client(
if (!group) {
continue;
}
for (auto & weak_client : group->get_client_ptrs()) {
auto cli = weak_client.lock();
if (cli && cli == client) {
auto client_ref = group->find_client_ptrs_if(
[&client](const rclcpp::ClientBase::SharedPtr & cli) -> bool {
return cli == client;
});
if (client_ref) {
return group;
}
}
}
}
return nullptr;
}
@ -216,13 +219,14 @@ MemoryStrategy::get_group_by_waitable(
if (!group) {
continue;
}
for (auto & weak_waitable : group->get_waitable_ptrs()) {
auto group_waitable = weak_waitable.lock();
if (group_waitable && group_waitable == waitable) {
auto waitable_ref = group->find_waitable_ptrs_if(
[&waitable](const rclcpp::Waitable::SharedPtr & group_waitable) -> bool {
return group_waitable == waitable;
});
if (waitable_ref) {
return group;
}
}
}
}
return nullptr;
}