Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -490,7 +490,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
return waitable_handles_.size();
}

private:
protected:
template<typename T>
using VectorRebind =
std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
Expand Down
219 changes: 218 additions & 1 deletion rclcpp/src/rclcpp/memory_strategies.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,14 +14,231 @@

#include "rclcpp/memory_strategies.hpp"

#include <algorithm>
#include <list>
#include <memory>

#include "rclcpp/strategies/allocator_memory_strategy.hpp"

using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;

namespace
{

/// Keeps the handles that were ready while their callback group was in use.
/**
* These handles are not waited on again, they are executed first, in the order in which
* they were blocked, as soon as their callback group can be taken from.
*/
class DefaultMemoryStrategy : public AllocatorMemoryStrategy<>
{
public:
void clear_handles() override
{
if (waited_) {
// The handles that are left were ready, but their callback group was in use.
keep_blocked(EntityType::Timer, timer_handles_);
keep_blocked(EntityType::Subscription, subscription_handles_);
keep_blocked(EntityType::Service, service_handles_);
keep_blocked(EntityType::Client, client_handles_);
keep_blocked(EntityType::Waitable, waitable_handles_);
waited_ = false;
}
AllocatorMemoryStrategy<>::clear_handles();
}

void remove_null_handles(rcl_wait_set_t * wait_set) override
{
AllocatorMemoryStrategy<>::remove_null_handles(wait_set);
// A handle that is already blocked must not be executed a second time.
for (const auto & blocked : blocked_handles_) {
auto handle = blocked.handle.lock();
remove_handle(timer_handles_, handle);
remove_handle(subscription_handles_, handle);
remove_handle(service_handles_, handle);
remove_handle(client_handles_, handle);
remove_handle(waitable_handles_, handle);
}
waited_ = true;
}

void
get_next_timer(
rclcpp::AnyExecutable & any_exec,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
{
if (restore_blocked(EntityType::Timer, weak_groups_to_nodes)) {
AllocatorMemoryStrategy<>::get_next_timer(any_exec, weak_groups_to_nodes);
}
}

void
get_next_subscription(
rclcpp::AnyExecutable & any_exec,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
{
if (restore_blocked(EntityType::Subscription, weak_groups_to_nodes)) {
AllocatorMemoryStrategy<>::get_next_subscription(any_exec, weak_groups_to_nodes);
}
}

void
get_next_service(
rclcpp::AnyExecutable & any_exec,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
{
if (restore_blocked(EntityType::Service, weak_groups_to_nodes)) {
AllocatorMemoryStrategy<>::get_next_service(any_exec, weak_groups_to_nodes);
}
}

void
get_next_client(
rclcpp::AnyExecutable & any_exec,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
{
if (restore_blocked(EntityType::Client, weak_groups_to_nodes)) {
AllocatorMemoryStrategy<>::get_next_client(any_exec, weak_groups_to_nodes);
}
}

void
get_next_waitable(
rclcpp::AnyExecutable & any_exec,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override
{
if (restore_blocked(EntityType::Waitable, weak_groups_to_nodes)) {
AllocatorMemoryStrategy<>::get_next_waitable(any_exec, weak_groups_to_nodes);
}
}

private:
enum class EntityType {Timer, Subscription, Service, Client, Waitable};

struct BlockedHandle
{
EntityType type;
std::weak_ptr<const void> handle;
};

template<typename HandlesT>
void keep_blocked(EntityType type, const HandlesT & handles)
{
for (const auto & handle : handles) {
blocked_handles_.push_back({type, handle});
}
}

template<typename HandlesT>
static void remove_handle(HandlesT & handles, const std::shared_ptr<const void> & handle)
{
handles.erase(std::remove(handles.begin(), handles.end(), handle), handles.end());
}

rclcpp::CallbackGroup::SharedPtr
get_group(
const BlockedHandle & blocked,
const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
{
auto handle = blocked.handle.lock();
if (!handle) {
return nullptr;
}
switch (blocked.type) {
case EntityType::Timer:
{
auto timer = get_timer_by_handle(
std::static_pointer_cast<const rcl_timer_t>(handle), weak_groups_to_nodes);
// The timer may have been canceled or reset in the meantime.
if (!timer || !timer->is_ready()) {
return nullptr;
}
return get_group_by_timer(timer, weak_groups_to_nodes);
}
case EntityType::Subscription:
return get_group_by_subscription(
get_subscription_by_handle(
std::static_pointer_cast<const rcl_subscription_t>(handle), weak_groups_to_nodes),
weak_groups_to_nodes);
case EntityType::Service:
return get_group_by_service(
get_service_by_handle(
std::static_pointer_cast<const rcl_service_t>(handle), weak_groups_to_nodes),
weak_groups_to_nodes);
case EntityType::Client:
return get_group_by_client(
get_client_by_handle(
std::static_pointer_cast<const rcl_client_t>(handle), weak_groups_to_nodes),
weak_groups_to_nodes);
case EntityType::Waitable:
return get_group_by_waitable(to_waitable(handle), weak_groups_to_nodes);
}
return nullptr;
}

static rclcpp::Waitable::SharedPtr to_waitable(const std::shared_ptr<const void> & handle)
{
return std::const_pointer_cast<rclcpp::Waitable>(
std::static_pointer_cast<const rclcpp::Waitable>(handle));
}

/// Put the oldest blocked handle whose callback group can be taken from back in front.
/**
* \return false if that handle is of another type, nothing of this type may be taken then.
*/
bool
restore_blocked(EntityType type, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes)
{
for (auto it = blocked_handles_.begin(); it != blocked_handles_.end(); ) {
auto group = get_group(*it, weak_groups_to_nodes);
if (!group) {
it = blocked_handles_.erase(it);
continue;
}
if (!group->can_be_taken_from().load()) {
++it;
continue;
}
if (it->type != type) {
return false;
}
auto handle = it->handle.lock();
switch (type) {
case EntityType::Timer:
timer_handles_.insert(
timer_handles_.begin(), std::static_pointer_cast<const rcl_timer_t>(handle));
break;
case EntityType::Subscription:
subscription_handles_.insert(
subscription_handles_.begin(),
std::static_pointer_cast<const rcl_subscription_t>(handle));
break;
case EntityType::Service:
service_handles_.insert(
service_handles_.begin(), std::static_pointer_cast<const rcl_service_t>(handle));
break;
case EntityType::Client:
client_handles_.insert(
client_handles_.begin(), std::static_pointer_cast<const rcl_client_t>(handle));
break;
case EntityType::Waitable:
waitable_handles_.insert(waitable_handles_.begin(), to_waitable(handle));
break;
}
blocked_handles_.erase(it);
return true;
}
return true;
}

std::list<BlockedHandle> blocked_handles_;
bool waited_ = false;
};

} // namespace

rclcpp::memory_strategy::MemoryStrategy::SharedPtr
rclcpp::memory_strategies::create_default_strategy()
{
return std::make_shared<AllocatorMemoryStrategy<>>();
return std::make_shared<DefaultMemoryStrategy>();
}
34 changes: 34 additions & 0 deletions rclcpp/test/rclcpp/executors/test_multi_threaded_executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,11 @@

#include <gtest/gtest.h>

#include <atomic>
#include <chrono>
#include <string>
#include <memory>
#include <thread>

#include "rclcpp/exceptions.hpp"
#include "rclcpp/node.hpp"
Expand Down Expand Up @@ -97,3 +99,35 @@ TEST_F(TestMultiThreadedExecutor, timer_over_take) {
executor.add_node(node);
executor.spin();
}

/*
Test that two timers in the same mutually exclusive callback group are both executed
if their callbacks take longer than the timer period.
*/
TEST_F(TestMultiThreadedExecutor, mutually_exclusive_timers_starvation) {
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2u);

std::shared_ptr<rclcpp::Node> node =
std::make_shared<rclcpp::Node>("test_multi_threaded_executor_timer_starvation");

auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

std::atomic_int count_one {0};
std::atomic_int count_two {0};

auto timer_callback = [&executor, &count_one, &count_two](std::atomic_int & count) {
std::this_thread::sleep_for(20ms);
count++;
if (count_one + count_two >= 20) {
executor.cancel();
}
};

auto timer_one = node->create_wall_timer(5ms, [&]() {timer_callback(count_one);}, cbg);
auto timer_two = node->create_wall_timer(5ms, [&]() {timer_callback(count_two);}, cbg);
executor.add_node(node);
executor.spin();

EXPECT_GE(count_one, 5);
EXPECT_GE(count_two, 5);
}